Note: Adding a GazeboRosForce gazebo ros plugin to a simulated object.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Adding a GazeboRosForce gazebo ros plugin to a simulated object

Description:

Keywords: gazebo plugins GazeboRosForce

Tutorial Level:

GazeboRosForce Example

Create a Simple Box URDF using the GazeboRosForce plugin

Create an URDF for an object and save it as object.urdf:

<?xml version="1.0" ?>
<robot name="single_joint">
  <gazebo reference="base">
    <material>Gazebo/GrassFloor</material>
  </gazebo>
  <link name="world"/>
  <joint name="link_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <parent link="world"/>
    <origin rpy="0 0 0" xyz="0 0 1"/>
    <child link="link"/>
    <limit effort="100" velocity="100" k_velocity="0" />
    <joint_properties damping="0.1" friction="1.0" />
  </joint>
  <link name="link">
    <inertial>
      <mass value="10"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
    <visual>
      <origin rpy="0 0 1" xyz="0 0 0"/>
      <geometry>
        <box size="0.2 1.0 0.2"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 1" xyz="0 0 0"/>
      <geometry>
        <box size="0.2 1.0 0.2"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="link">
    <material>Gazebo/GrassFloor</material>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  
   <gazebo>
    <controller:gazebo_ros_force name="gazebo_ros_force" plugin="libgazebo_ros_force.so">
      <alwaysOn>true</alwaysOn>
      <update>100</update>
      <updateRate>100.0</updateRate>
      <bodyName>link</bodyName>
      <topicName>force</topicName>
    </controller:gazebo_ros_force>
  </gazebo>
</robot>

Spawn Model in Simulation

Start an empty world simulation

rosmake gazebo_worlds
roslaunch gazebo_worlds empty_world.launch

To spawn above URDF

rosrun gazebo spawn_model -file object.urdf -urdf -z 1 -model my_object

You will see the /force topic listed with

rostopic list

You can applaiy a torque

rostopic pub /force geometry_msgs/Wrench  '{force:  {x: 0.0, y: 0.0, z: 0.0}, torque: {x: 0.0,y: 0.0,z: 0.6}}'

You should see the box rotating

Wiki: pr2_gazebo_plugins/Tutorials/Attaching a GazeboRosForce pugin to Objects in Simulation (last edited 2011-12-13 19:44:43 by Markus Bader)