Skip to content →

Gazebo’s gravity bug

I wrote this post in case I forgot.

This post is about the bug happens in the condition when

・called Gazebo’s Joint::SetPosition function without specifying _preserveWorldVelocity as true.

Then this happens when

"position_controllers/PositionJointController" is used without specifying pid_gains in ROS and Gazebo integration.

This time, we ran into this problem exactly.

We investigated a lot about this bug, so I try to summarize in this post.

What kind of problem this is

http://answers.gazebosim.org/question/18684/mobile-arm-simulation-behaves-strangely-when-adding-ros_control-plugin/

This question and answer describe this problem well.

This problem is that model behaves strangely when ros_control plugin is loaded.

Joint::SetPosition is the main cause, and "position_controllers/PositionJointController call this function. (Around here)

The answer shows two workarounds:

  1. Use EffortJointInterface or VelocityJointInterface
  2. Set /gazebo_ros_control/pid_gains/

We tried both of them, and both of them fixed the problem. Because none of them call SetPosition(1., 2.)

However, workaround 1. is not suitable for out humanoid because we control servos using position control. Also, when we want to control robot without PID, 2. is not good.

How to Fix?

So how can we deal with this. from Gazebo 9, the parameter named _preserveWorldVelocity was added to Joint::SetPosition.

Details are written in these Issue and PR:

  1. https://bitbucket.org/osrf/gazebo/issues/2111/joint-setposition-alterate-the-simulation
  2. https://bitbucket.org/osrf/gazebo/pull-requests/2598/fix2111
  3. https://bitbucket.org/osrf/gazebo/pull-requests/2814

They seemed to fix this behavior as to be expected. (1. 2.)

However, there were some use cases in current behavior, then _preserveWorldVelocity was introduced. Passing true to this parameter, then fixed (3.)

Yeah, it’s all good, but _preserveWorldVelocity‘s default value is false.

And there seems to be no way to set this parameter in URDF or roslaunch.

That means… I have to build it from source.

Fixes in sources

I wrote needed fixes in This patch.

Taking some code from the patch, I set the default argument to true.

Also, I found that someone calls this by explicitly specifying false, so I made a change like this:

You know, this change is crazy. But it worked, so I don’t care anymore…

So please be careful if you’re willing to use this patch. This can be a cause of another bug.

 

Fixed…?

Left: before patching, Right: after patching.

The weird behvior in falling is fixed. Cool!

Wait a minute

…well, as some of you noticed, gazebo_ros_control sets _preserveWorldVelocity to be true.

Then it’s so mysterious who passed false to that, but I don’t want to think about this anymore because it has just worked, and I’m no longer using Gazebo anymore…

References

https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612

https://github.com/ros-simulation/gazebo_ros_pkgs/issues/479

http://answers.gazebosim.org/question/18684/mobile-arm-simulation-behaves-strangely-when-adding-ros_control-plugin/

http://answers.gazebosim.org/question/17281/problems-with-model-simulation-behaves-like-there-is-moon-gravity-solved/

http://answers.gazebosim.org/question/15554/enabling-gazebo_ros_control-plugin-changes-physics-response-of-entire-body/

https://bitbucket.org/osrf/gazebo/issues/2111/joint-setposition-alterate-the-simulation

https://bitbucket.org/osrf/gazebo/pull-requests/2598/fix2111

About _preserveWorldVelocity

http://answers.gazebosim.org/question/18708/how-can-i-set-the-preserveworldvelocity-flag-in-gazebo-9-hopefully-through-roslaunch-and-urdf/
https://bitbucket.org/osrf/gazebo/pull-requests/2814

Published in DeepL2 YamaX simulation Software