Gazebo | Ignition | Community
Ask Your Question

Looking for a stable solution to reset a robot (with controller) in Gazebo

asked 2021-07-09 03:01:12 -0500

sre gravatar image


I think the problem is known to almost everyone, but here again the problem description: The position of a robot should be reset to a defined position, e.g. the start position of all joints defined in the launch file.

A simple service call with '/gazebo/reset_world' nor '/gazebo/reset_sim' wouldn't do the trick. Note: I'm aware of the famous github issue #93 and the pause/unpause solution is at least a stable one.

My best solution so far:

  1. restart all controllers
  2. pause
  3. set pose via '/gazebo/set_model_configuration'
  4. update the controllers "desired positions" via FF topic command: '/controller_ns/command'
  5. unpause
  6. for good measure a repeat of 4.

Most of the times this works well and seems to work fast as well, but sometimes one ore more joints seems to be lost and flattering around without any control. The topics show the error position and error velocity, but 0 acceleration and 0 effort (for joint effort controllers). From this point on, it can't recover to a good and clean reset, it will always lost the same joints.

What is this black magic trickery and how can i avoid it? Is it better to delete the robot and spawn it again?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2021-07-16 07:40:00 -0500

sre gravatar image

Restarting the controllers doesn't do anything. For some reason the /joint_states topic can publish partially wrong values after the position is set during '/gazebo/pause_physics' with /gazebo/set_model_configuration'. The build-in reset functions are not touched!

A solution that works for us without sleep timers or weird behavior (so far...) is the following order:

  1. Get model state via '/gazebo/model_states'and put it into a new Pose message.
  2. Delete the robot with '/gazebo/delete_model', this will also remove all controllers for the robot. Otherwise you could stop and remove all controllers manually.
  3. Spawn the robot again on the same position, e.g. via '/gazebo/spawn_urdf_model', you could also store the urdf with rospy.get_param('/robot_description') or whatever name you choose.
  4. Pause physics
  5. Set the new old pose/joint states with /gazebo/set_model_configuration'
  6. Unpause physics
  7. Load all controllers with '/controller_manager/load_controller'
  8. Start all controllers with '/controller_manager/switch_controller'
  9. Send the same joint states from 5. on the /ns/controller/command topic, otherwise the controller tries to move all joints to 0.0 rad.

Please share your thoughts or observations.

*Tested with ROS noetic, Gazebo 11 on effort_controllers/JointTrajectoryController for a 16 joints robot

edit flag offensive delete link more
Login/Signup to Answer

Question Tools



Asked: 2021-07-09 03:01:12 -0500

Seen: 373 times

Last updated: Jul 16 '21