Huge incoherences when reading joint/link positions and velocities (maybe Gazebo state handling error)

[AndrePatri]

I have found a potentially very serious issue when reading the states of my robot. To exclude potential errors in my plugin code, I have also used the plotting utility of the GUI. You can see the details at https://answers.gazebosim.org/question/27430/huge-incoherences-when-reading-jointlink-positions-and-velocities-probably-gazebo-internal-state-handling-error/

[scpeters]

are you using any plugins with that model to support the closed kinematic loop?

[AndrePatri]

Thanks for answering me @scpeters. I am not 100% sure what you mean. I have an SDF model of my robot and an attached model plugin which I use to test control strategies on the system. The plugin basically reads the state of the system and the model properties and at each sample time computes suitable control inputs. In particular the control torques are applied on the two wheel joints and on two joints of the quadrilateral linkage. I have found that this behaviour basically depends of the kp of the wheel links. In my world file I have a "custom" ground, with a very high kp. This way I can soften the contact by simply acting on the wheel links. If I put a low kp, this behaviour arises. For a sufficiently high kp this behaviour disappears, but the contact becomes to irregular (the wheels basically slips). As a consequence, as of now, I have partially solved the issue by setting a low kp (which ensures good contact) and by differentiating numerically the state variables; the problem, however, remains.

[scpeters]

I'll be more specific with my questions:

1. Are you using SDFormat or URDF for your model?
2. What joints are in your model? Are you using revolute joints for all parts of the closed kinematic loop?
3. What gazebo API's are you using for joint control?

[AndrePatri]

1. I am using SDF 1.7 for the robot
2. Yes, I have all revolute joints
3. This one http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/index.html

[scpeters]

3. What gazebo API's are you using for joint control?

• This one http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/index.html

I meant which specific functions are you calling: Joint::SetForce, Joint::SetPosition (not recommended), etc

[AndrePatri]

Well, I didn't use SetPosition since apparently does not work on closed kinematic chains (at the time I tested it and didn't work). In my plugin I make use of:

• this->hinge_name->Position(0) and this->hinge_name->GetVelocity(0) for reading joint positions and velocities
• this->left_wheel_state=physics::LinkState(this->left_wheel) and the Pose() and Velocity() methods to get the necessary link pose and velocity
• I use Joint::SetForce for applying control inputs (which are torques in my case)

[AndrePatri]

If it helps, I can share the plugin, the world file and the model.sdf file.