Easiest way to move (change pose) of a URDF robot in Gazebo

asked 2022-01-25 09:26:18 -0500

MadeleineP gravatar image

I have a URDF robot that consists of some simple joints, links, and a camera sensor. I am spawning this robot in Gazebo using a ROS launch file and the sapwn_urdf node. Additionally, I have a TF publisher and robot_state_publisher as shown below.

<param name="robot_description"
       command="$(find xacro)/xacro --inorder '$(find myproject)/urdf/sensors/camera_box.urdf.xacro'
                robot_name:=$(arg camera_name)
                camera_name:=$(arg camera_name)
                base_x:=-1.0
                base_y:=2.0
                base_z:=3.0
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
      args="-urdf -param robot_description -model $(arg camera_name)"
      respawn="false" output="screen" />

<!-- Provide transform between the parent and camera's world frame    -->
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg camera_name)_tf_broadcaster"
      args="0   0   0   0   0   0   $(arg parent_frame)   $(arg camera_base_frame)" />

<node name="robot_state_publisher"
      pkg="robot_state_publisher"
      type="robot_state_publisher"
        respawn="false" output="screen"/>

What I'd like to do is be able to move this robot (camera box) to arbitrary positions within Gazebo either from a command-line call or from a self-written node (C++ or Python). The robot could either "jump" instantaneously to another position, or move smoothly.

Is there an easy way to accomplish this? I can move non-robot models within gazebo (such as a piece of furniture) using a service call like:

rostopic pub -1 /gazebo/set_model_state gazebo_msgs/ModelState "model_name: 'chair'

But this doesn't move (or keep moved) the robot.

If I run the TF publisher node from the command-line, I can use it to move the robot within RViz, but it has no effect in Gazebo (as expected).

I assume that I could attach the "root" link of the camera-box robot to a non-fixed joint, attach a transmission, and move it this way. If this is the only way, that's fine. I was just wondering if there is a faster / easier and non-invasive way (especially during prototyping) to accomplish this.

Thanks for your help! Madeleine

edit retag flag offensive close merge delete

Comments

Hi @MadeleineP, I believe you are definitely on the right track. Yes you need to add the 'extras' for Gazebo. Gazebo as a physical simulator, won't be able to differentiate your robot as an inert object otherwise.

The advantage of doing this, it will help you when implementing controls simulation later on. I personally find this the most useful part of the simulation when working with Gazebo.

If you consider this too much work, you may consider PyBullet or Webots as it will be faster for prototyping robot movements.

osilva gravatar image osilva  ( 2022-01-25 12:44:49 -0500 )edit