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 -0600

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

2 Answers

Sort by ยป oldest newest most voted

answered 2022-12-10 05:47:50 -0600

i did exactly the 9 steps mentioned in @sre answer, but it didn't work. with some effort, i was able to make it work at last. the problem was in step 3 for me. i had to pause physics, spawn model, set_model_configuration, load_controller and then send a request for switch_controller (to enable them). at this point, this request is not responded because the physics is stoped. but as soon as physics is unpaused, the response is returned and the robot is remained in the defined configuration. this way i didn't even need to send command (/controller/command topic) anymore. so solution for me was:

  1. Get model state via /gazebo/model_states and put it into a new Pose message.
  2. Delete the robot with /gazebo/delete_model
  3. Pause physics
  4. Spawn the robot again on the same position via /gazebo/spawn_urdf_model
  5. Set the old pose/joint states with /gazebo/set_model_configuration
  6. Load all controllers with /controller_manager/load_controller
    1. Start all controllers with /controller_manager/switch_controller (send request and wait for response in another terminal/thread)
    2. Unpause physics (switch_controller will respond immediately)

test: gazebo 11, ros noetic with 6dof arm

edit flag offensive delete link more

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

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

Question Tools



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

Seen: 1,024 times

Last updated: Jul 16 '21