[bug] Using Gazebo `set_model_configuration` srv messes up joint_states
Opened this issue · 1 comments
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
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
- Clone the https://github.com/rickstaa/franka_ros/tree/show_gazebo_set_model_config_problem.
- Build the catkin workspace.
- Source the catkin workspace.
- Start the panda simulation
roslaunch franka_gazebo panda.launch use_gripper:=false physics:=dart controller:='force_example_controller'
. - Start the
franka_gazebo/scripts/log_joint_violations.py
script (i.e.rosrun franka_gazebo log_joint_violations.py
). - Start the
franka_gazebo/scripts/set_random_joint_positions.py
script (i.e.rosrun franka_gazebo set_random_joint_positions.py
) script. - 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
- Checkout the test_fix_gazebo_set_model_config_problem branch.
- Perform set 2-5 above.
- Start the
franka_gazebo/scripts/set_random_joint_positions_226.py
script (i.e.rosrun franka_gazebo set_random_joint_positions_226.py
). - See that now the joints stay within the joint limits.
TODOs
- Merge #226
I've recently observed an issue stemming from the changes introduced in commit 89d2571. 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
- Clone and build the franka_ros catking workspace.
- Start the franka gazebo simulation (i.e.
roslaunch franka_gazebo panda.launch
). - 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"
- See the panda robot being set to the desired position and directly returning to the position before the service is called.