[bug] Using Gazebo `set_model_configuration` srv messes up joint_states

[rickstaa]

During my RL training, the robot sometimes becomes stuck. When this happens, I use the /gazebo/set_model_configuration service to set the Panda robot to a free robot position. However, when doing this, the joint positions are sometimes reported to be outside their joint limits. This prevents third-party tools like MoveIt from functioning.

Why this happens is very clear by looking at the code:

franka_ros/franka_gazebo/src/joint.cpp

Line 30 in 15f5975

this->position += angles::shortest_angular_distance(this->position, position);

Every time the /gazebo/set_model_configuration changes the joint positions, they get added to the joint->positions attribute. As a result, when the change made by the /gazebo/set_model_configuration is too big, the reported joint positions are pushed outside the joint limits.

I understand why this is implemented, but providing users with a way to reset the joint positions would be very helpful since it is a common use case.

I implemented a PR to add a set_franka_model_configuration service in #226.

See the behaviour in action

1. Clone the https://github.com/rickstaa/franka_ros/tree/show_gazebo_set_model_config_problem.
2. Build the catkin workspace.
3. Source the catkin workspace.
4. Start the panda simulation roslaunch franka_gazebo panda.launch use_gripper:=false physics:=dart controller:='force_example_controller'.
5. Start the franka_gazebo/scripts/log_joint_violations.py script (i.e. rosrun franka_gazebo log_joint_violations.py).
6. Start the franka_gazebo/scripts/set_random_joint_positions.py script (i.e. rosrun franka_gazebo set_random_joint_positions.py) script.
7. See the reported joint positions being pushed outside the joint limits.

Note

This branch also includes #211 because the ODE physics engine is not stable when performing force control (see #160 (comment)).

See the fix in action

1. Checkout the test_fix_gazebo_set_model_config_problem branch.
2. Perform set 2-5 above.
3. Start the franka_gazebo/scripts/set_random_joint_positions_226.py script (i.e. rosrun franka_gazebo set_random_joint_positions_226.py).
4. See that now the joints stay within the joint limits.

TODOs

• Merge feat(franka_gazebo): implement set_franka_model_configuration service #226