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

https://github.com/frankaemika/franka_ros/blob/15f597566fd9f1aa1a66bce4c1b3b183e8aaa081/franka_gazebo/src/joint.cpp#L30

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 https://github.com/frankaemika/franka_ros/pull/211 because the ODE physics engine is not stable when performing force control (see https://github.com/frankaemika/franka_ros/issues/160#issuecomment-989173918).

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 #226

[rickstaa]

I've recently observed an issue stemming from the changes introduced in commit 89d25713b741631ca9f4f417585e7835d160cf4a. Specifically, after invoking the original Gazebo set_model_configuration, the robot reverts to its initial position (see gif below). I adapted #226 to allow users to set the Franka configuration still.

How to reproduce

1. Clone and build the franka_ros catking workspace.
2. Start the franka gazebo simulation (i.e. roslaunch franka_gazebo panda.launch).
3. Execute the following /gazebo/set_model_configuration call:

rosservice call /gazebo/set_model_configuration "model_name: 'panda'
urdf_param_name: 'robot_description'
joint_names:
- 'panda_joint2'
- 'panda_joint3'
joint_positions:
- 0
- 0.5"

4. See the panda robot being set to the desired position and directly returning to the position before the service is called.