[gazebo_ros_control] Strange behaviour when adding gazebo_ros_control to a robot

[Zabot]

I was modeling up an arm, and everything seemed to be working until I added the ros control plugin:

  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

The only difference between these two arms is that the arm in front has gazebo_ros_control plugin enabled, and the one in back has those lines commented out.

I've verified the inertial properties of are all set right, tried it with collisions disabled completely and without, its the same both ways. I am still able to control the joints while the arm is falling, and they behave as expected, with normal speed, though sluggish acceleration.

Even more curious, when I manually change any one of the four revolute joints on the gazebo_ros_control arm to a fixed joint it behaves perfectly fine.

I was observing a similar issue with turtlebot_arm that I had originally attributed to inertial properties, but it seems like it may be a deeper seated issue.
turtlebot/turtlebot_arm/issues/22

[Zabot]

After further investigation this appears to be related to #479 and specifically this PR to gazebo.

It looks like the function that was being used before to change the rotation of a frame used to preserve the velocity of the frame, and now it zeros the velocity of the frame. As such every time the position of a PositionJointInterface is updated, the velocity of the child frame is zeroed, preventing it from accelerating properly.

[mintar]

Thanks @Zabot for writing this up so concisely. I just got bitten by this issue myself. I was working on a robot URDF consisting of a diffdrive mobile base with a robot arm on top. This issue caused the robot arm to act like a giant parachute, counteracting gravity, and causing some of the wheels to lose contact with the ground. It took me ages to figure out that an incorrect simulation of the arm was causing my diffdrive controller to fail...

Anyhow, for posterity here are the workarounds mentioned in #479 :

• Add pid_gains for your Gazebo controllers when using the PositionJointInterface, like this:

gazebo_ros_control:
  pid_gains:
    joint2: {p: 100.0, i: 0.01, d: 10.0}

• Alternatively, use the VelocityJointInterface or EffortJointInterface instead.

Both successfully work around this issue.

[vincentrou]

This issue can be closed since there is workaround and it now works with gazebo9.

[Zabot]

The PR over on gazebo was merged, but don't we still need to change ros_control_plugin to use the new flag that was added? If I'm not mistaken wasn't it decided that the default behavior would remain unchanged?

[mintar]

Yes, please don't close this issue. It's still there in ROS Kinetic + Gazebo7, which is what most users use. IMO, if this bug won't get fixed, it would be nice if at least a big fat warning would be printed. Usually this bug is very hard to track down, because it manifests in unintuitive problems like the mobile base isn't moving any more once you add an arm to the robot (because this bug causes the mobile base to lose contact with the ground).

There seems to be a large number of users affected - basically everyone who's using ros_control + Gazebo together. Every week, a couple of new questions pop up on ROS answers.

[mintar]

The PR over on gazebo was merged, but don't we still need to change ros_control_plugin to use the new flag that was added? If I'm not mistaken wasn't it decided that the default behavior would remain unchanged?

I agree. The PR #2814 just added the option to specify preserveWorldVelocity, but the default is false. The ros_control_plugin still has to set it to true in the call to Joint::setPosition(). Unfortunately, that PR has only been merged into Gazebo9, not Gazebo7.

[mintar]

In case somebody is interested, I've written a tutorial package that shows how to work around this bug:

https://github.com/mintar/mimic_joint_gazebo_tutorial

[j-rivero]

Thanks for all the debugging and information. If we need to backport a change from gazebo9 to previous versions, let me know. If we need to made changes in this repository, would you mind to create a PR with the needed changes?

[mintar]

@j-rivero : I've added a pull request (#688) that fixes this issue for Gazebo 9. Could you please backport gazebo PR 2814 to Gazebo 7 and Gazebo 8, so that we can relax the GAZEBO_MAJOR_VERSION guards in #688 to bring this fix to Gazebo 7?

[scpeters]

I'm going to see if it's possible to backport that bugfix to gazebo7 without breaking API/ABI

I'm testing a branch right now

[scpeters]

https://build.osrfoundation.org/job/gazebo-abichecker-any_to_any-xenial-amd64/571

[scpeters]

I think we had talked about this at the time, but we couldn't figure out a way to backport a fix without breaking API/ABI

tried tweaking the const qualifiers, but that didn't help

https://build.osrfoundation.org/job/gazebo-abichecker-any_to_any-xenial-amd64/572

https://bitbucket.org/osrf/gazebo/branches/compare/issue_2111_7%0Dgazebo7#diff

[mintar]

@scpeters : Wouldn't something like this ABI compatible?

bool Joint::SetPosition(unsigned int _index, double _position)
{
   return this->SetPosition(_index, _position, false);
}

bool Joint::SetPosition(const unsigned int _index, const double _position,
                        const bool _preserveWorldVelocity)
{
  // ...
}

That way, you wouldn't be removing any symbols, only adding new ones. It would be really nice if we could backport this to Gazebo 7. Otherwise we should add a big fat warning whenever people call SetPosition without preserveWorldVelocity, since it will break their simulation in unexpected and hard to debug ways.

[TheDash]

I'm also having this issue... except the pid_gains is not fixing it =(

Any other suggestions?

[mintar]

I've added a warning message in #691 that should tell you if you're affected by exactly this issue or not. Unfortunately, that warning is only part of release 2.5.16, and kinetic is still on 2.5.14, so you'll have to build gazebo_ros_pkgs from source to get that warning.

[j-rivero]

I've added a warning message in #691 that should tell you if you're affected by exactly this issue or not. Unfortunately, that warning is only part of release 2.5.16, and kinetic is still on 2.5.14, so you'll have to build gazebo_ros_pkgs from source to get that warning.

Waiting next sync in the shadow fixed repo: http://packages.ros.org/ros-shadow-fixed/ubuntu/pool/main/r/ros-kinetic-gazebo-ros-pkgs/ in the case you want to use .deb packages. Thanks @mintar for the contribution.