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

edit

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':

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