gazebo reset

[jacknlliu]

In order to robot learning, we always need the reset function.

• reset joint position(model configuration)
• reset model velocity
• reset world
• reset without simulation time or with simulation time
• reset gazebo plugins with time or without time
• reset ros controller for gazebo correctly

But we should know that gazebo is still not friendly with this! Another thing is about gazebo robot control which is not ready for robot arm control in the context of robot learning which needs large quantities of repetitive operations with contact. I think these are the larger issues we have to fix in the future ROS and Gazebo. The ROS and gazebo for robot arm learning simulation are still not ready!

Reset joint position

Reset joint position can' work with ros controllers. See spawn_model -J initial joint positions not working #93.
But something maybe different with gazebo9, see #29 (comment)

Reset World

Currently, reset_world() will not reset time(we test this in gazebo 9 by rosservice call /gazebo/reset_world ), and reset_simulation will reset time. see issues with reset buttons.

Reset ros controller for gazebo

At present, we find that resetting the ros controllers is not correct.
And the ros controller can't get the correct joint state by robot HardwareInterfaceSim, especially on cases that after reset gazebo and contact occurs in gazebo.

Other issues

• /joint_states will return wrong and very large data after self collision or contact with other objects. And it can't be reset at present!

Reference

• reset /joint_states and gazebo plugins state in gazebo #27
• issues with reset buttons
• spawn_model -J initial joint positions not working #93

[jacknlliu]

Reset gazebo model joint position which spawned from ros URDF with gazebo_ros_control plugin

NOTE: this didn't work with ros kinetic and gazebo 7. But it seems work with ros kinetic and gazebo 9. But it doesn't work with gazebo 9 (9.0.0-1~xenial ) and bullet physics engine.

# stop ros controllers should before pause physics, or will be blocked
rosservice call /controller_manager/switch_controller [] ["arm_gazebo_controller"] 2

# pause physics
rosservice call /gazebo/pause_physics

# rosrun gazebo_ros spawn_model ....

# set model joint configuration with URDF parameter
rosservice call /gazebo/set_model_configuration '{model_name: "robot", urdf_param_name: "robot_description", joint_names:['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint','wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], joint_positions:[1.2, 0.3, -1.5, -0.5, -1.5, 0.0]}'
sleep 5

# unpause physics
rosservice call /gazebo/unpause_physics
# after unpause the robot will be set to the previous pose(before paused)

# restart ros controllers should after unpaused physics, or will be blocked
rosservice call /controller_manager/switch_controller ["arm_gazebo_controller"]  [] 2

If we want start gazebo without ros controller

# We can start gazebo with ROS service support
roslaunch gazebo_ros empty_world.launch

# insert PR2 or UR10 robot in gazebo 

# set model joint configuration directly without URDF
rosservice call /gazebo/set_model_configuration '{model_name: "ur10", joint_names:['shoulder_pan_joint', 'shoulder_lift', 'elbow','wrist_1', 'wrist_2', 'wrist_3'], joint_positions:[2, -1, -1.5, -1.5, -1.5, 0.0]}'

[jacknlliu]

We find that /gazebo/set_model_configuration can work correctly with gazebo9 even we use spawn_model with URDF to spawn robot model into gazebo.

Important findings:
If we spawn the robot from gazebo .world file, not using the following spawn_model node

<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot" respawn="false" output="screen" />

then we can set the model configuration correctly with the following procedure

• stop ros controllers
• set model configuration using ros service

rosservice call /gazebo/set_model_configuration '{model_name: "ur10", joint_names:['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint','wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], joint_positions:[1.0, 0.0, 1.9, 0.2, 0.0, 0.0]}'

• start ros controllers
  The controller wouln't set the joint value as the previous values.

---

Appendix

We reproduce this following these steps:

• create a world for our ur10 robot model

The world file like this:

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">
    <physics type='ode'/>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ur10_clone</uri>
      <name>ur10</name>
      <pose>0 -2 0 0 0 0</pose>
      <plugin name="ros_control" filename="libgazebo_ros_control.so">
      </plugin>
    </include>
  </world>
</sdf>

where we clone the ur10 model from gazebo models to ur10_clone which modified the joint names to align with the urdf file in ros description package.

• start gazebo

roslaunch gazebo_ros empty_world.launch world_name:="/data/assembler/ros_test/ros_control.world"

• load joint controller parameters
  The controller parameter yaml file is:

/arm_gazebo_controller:
  type: velocity_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

load the parameters into the namespace /ur10

rosparam load  $(rospack find assembler_gazebo)/config/arm_velocity_controller_ur5.yaml  /ur10

• start controller

rosrun controller_manager spawner --namespace="ur10"  "arm_gazebo_controller"

• using rqt controller_manager and rosservice to stop/start controller and /gazebo/set_model_configuration

rosservice call /gazebo/set_model_configuration '{model_name: "ur10", joint_names:['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint','wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], joint_positions:[1.0, 0.0, 1.9, 0.2, 0.0, 0.0]}'

NOTE: you can also pause physics after stopping controller and unpause physics before starting controller, and it will also work correctly.

[jacknlliu]

Possible causes of reset failure

• sim hardware interface

see default_robot_hw_sim

Only if we set the ros control plugin in the URDF before loading by gazebo, this node will start, and regardless of whether or not a corresponding controller exists for the robot.

This will reset the position, but for some physics engine, the interface may not supported by ros, so it failed.

• gazebo failed to set model configuration

• controller will set the joint position by its states(from where?)

• controller manager how to set the states of the hardware interface