Gazebo | Ignition | Community
Ask Your Question
0

Robot links are breaking/not correctly updating when calling SetPosition() in fast successions.

asked 2021-11-11 14:57:31 -0600

sbob gravatar image

Hi,

I am encountering some weird behavior while using Gazebo. I am simulating a simple robot consisting of 7 joints and links. Since I am not interesting in any dynamics, I am considering the robot as a pure kinematic model. I coded a model plugin in C++, that I attached to my robot. This plugin listens to a custom topic, on which I send messages of requested joint values for each joint using a separate C++ program. In the callback upon receiving this message, my model plugin then simply updates the joint values of each joint by calling the SetPosition function of the Gazebo API:

void iiwaControlPlugin::OnJointPosMsg(ConstJointPtr &_msg)
{
    for(int i = 0; i < 7; i++)
    {
        this->joints.at(i)->SetPosition(0,_msg->angle(i),true);
    }



    gazebo::msgs::Pose msg;

    gazebo::msgs::Set(&msg,ee_link->WorldPose());

    pub_pose->WaitForConnection();
    pub_pose->Publish(msg);

}

Note, that in the second part of this function I am publishing a new message on a separate topic that outputs the frame of the end-effector. I am waiting for that message in my other C++ program before sending new joint value request to make sure that the simulator is done updating the robot. As the robot is set to kinematic and I use SetPosition, this update happens pretty much immediately (no joint controllers are used).

The bug that I am encountering arises, when I try to update the robot's joint value in very fast succession. After a while, the joint constraints seem to break, as one or more of the links are not updated correctly anymore and become 'unattached' from their parent joint/link. Here is an example screenshot, in which the second and third joints of the robot 'break':

image description

Does anyone have an idea why that might be the case? Am I missing some other function to call on the Gazebo side to ensure everything is updated correctly? Another handshake I am missing? Or maybe there are some physics settings that I didn't set up correctly?

I tried slowing down the speed with which I send joint values to my plugin (even waiting for hundreds of ms to make sure, the simulator is indeed done updating), but the problem does always appear again (but it takes longer sometimes).

Any help or pointers are highly appreciated!

-sbob

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2021-11-22 11:12:54 -0600

nlamprian gravatar image

Try storing the joint angles the plugin receives and letting gazebo itself update the positions during the world update event (i.e. doing the update synchronously).

For example, see https://github.com/ros-simulation/gaz...

edit flag offensive delete link more
0

answered 2021-11-16 14:39:47 -0600

shonigmann gravatar image

Gazebo might not be the best tool for a purely kinematic model... That said, I have noted very similar issues and the solution ultimately boiled down to correctly setting the physics parameters.

If I recall correctly, my understanding was that depending on which physics engine is being used, Gazebo tries to resolve the forces that would have been required in order to result in the position you set, in order to maintain dynamic continuity.

Long story short (you can read some of the commentary on the very similar issues here and here with gazebo_ros2_control's plugins using SetPosition()), I found 2 potential fixes.

  1. change the physics engine from ode (default) to dart in your world file. In my experience, this was the simplest fix to the unstable behavior, and if you only care about kinematics, it may be a good bet
  2. Make sure your URDF or SDF manipulator model:
  3. has accurate mass and inertia values
  4. has <friction> and <damping> tags set to reasonably values
edit flag offensive delete link more

Comments

Thanks for the answer, I shall try it out soon! For now I changed my code so that I limit the number of configuration changes that can happen in a short amount of time, which makes the phenomena less likely to appear

sbob gravatar imagesbob ( 2021-11-19 15:34:06 -0600 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2021-11-11 14:57:31 -0600

Seen: 44 times

Last updated: Nov 22