Why is a robot's link sinking under ground when I assign to it z axis coordinate of 0 at each update
I have a model plugin, in which on each update, I set a z
axis position of a floating link to zero.
math::Pose pose = this->link_ptr->GetRelativePose();
pose.pos.z=0.0;
this->link_ptr->SetRelativePose(pose);
When I run the simulation, the link slowly sinks under the ground and continues slowly falling down. What is happening? How can I fix it?
EDIT
I found out, that the link is not sinking if I turn the gravity off for the individual link. What is happening? That is definitively not how this ought to work. Is it a bug?
Moreover I can't set the gravity to false from the URDF description, as it gets reset after the conversion to SDF (viz this issue). Anybody faced this issue before?
EDIT 2
the source file of the model plugin:
#include <ros/callback_queue.h>
#include "CoverPlugin.hh"
namespace gazebo
{
// Constructor
CoverPlugin::CoverPlugin()
{}
// Default plugin init call.
void CoverPlugin::Init()
{}
void CoverPlugin::Load( physics::ModelPtr _parent,
sdf::ElementPtr _sdf)
{
// Get the world name.
this->world = _parent->GetWorld();
this->model = _parent;
// Get link
std::string link_ptr_name = this->model->GetName() + "::"
+ "cover_link";
this->link_ptr = this->model->GetLink(link_ptr_name);
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&CoverPlugin::OnUpdate, this));
ros::AsyncSpinner spinner(2); // Use 2 threads
spinner.start(); // spin() will not return until the node has been shutdown
}
void CoverPlugin::OnUpdate()
{
math::Pose pose = this->link_ptr->GetRelativePose();
if( this->link_ptr->GetRelativePose().pos.z < 0.0 )
{
pose.pos.z=0.0;
}
if( this->link_ptr->GetRelativePose().pos.x > 0.028 )
{
pose.pos.x=0.028;
}
if( this->link_ptr->GetRelativePose().pos.y > 0.014 )
{
pose.pos.y=0.014;
}
if( this->link_ptr->GetRelativePose().pos.z > 0.01 )
{
pose.pos.z=0.01;
}
if( this->link_ptr->GetRelativePose().pos.x < -0.028 )
{
pose.pos.x=-0.028;
}
if( this->link_ptr->GetRelativePose().pos.y < -0.014 )
{
pose.pos.y=-0.014;
}
if( this->link_ptr->GetRelativePose().rot.x > 0.05 )
{
pose.rot.x=0.05;
}
if( this->link_ptr->GetRelativePose().rot.y > 0.05 )
{
pose.rot.y=0.05;
}
if( this->link_ptr->GetRelativePose().rot.z > 0.05 )
{
pose.rot.z=0.05;
}
if( this->link_ptr->GetRelativePose().rot.x < -0.05)
{
pose.rot.x=-0.05;
}
if( this->link_ptr->GetRelativePose().rot.y < -0.05 )
{
pose.rot.y=-0.05;
}
if( this->link_ptr->GetRelativePose().rot.z < -0.05 )
{
pose.rot.z=-0.05;
}
this->link_ptr->SetRelativePose(pose);
math::Vector3 force;
math::Vector3 torque;
force.x = -pose.pos.x*10000;
force.y = -pose.pos.y*10000;
force.z = -pose.pos.z*50000;
torque.x = -pose.rot.x*100;
torque.y = -pose.rot.y*100;
torque.z = -pose.rot.z*100;
this->link_ptr->SetForce(force);
}
;
}
header file:
#ifndef COVER_PLUGIN_HH
#define COVER_PLUGIN_HH
#include <gazebo/gazebo.hh>
#include "ros/ros.h"
#include "std_msgs/Bool.h"
#include <gazebo/physics/physics.hh>
#include <gazebo/math/Pose.hh>
#include <gazebo/math/Vector3.hh>
#include <gazebo/common/common.hh>
namespace gazebo
{
class CoverPlugin : public ModelPlugin
{
// Constructor.
public: CoverPlugin();
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
private: physics::WorldPtr world;
private: physics::ModelPtr model;
private ...
What is the parent link of the link? The documentation says: Set the pose of the entity relative to its parent. So this could may be the cause of the problem.
The parent link is the chassis of the robot, which stays standing above the ground the whole time. The floating link moves with respect to it's parent in z axis.
Not easy. Can you post the source code of your model plugin?
source code added to the question.
Did you solve this issue?