Manipulating Position of Conveyor Belt relative to platform in Gazebo and applying body wrench
Hello Everyone,
I created a simple conveyor model in RVIZ (see URDF below)
<?xml version="1.0"?>
<robot name="multipleshapes">
<link name="base_link">
<visual>
<geometry>
<box size="1 4 .01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="1 4 .01"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="1 4 .01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="1 4 .01"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="belt">
<visual>
<geometry>
<box size="1 4 .01"/>
</geometry>
<material name="blue">
<color rgba="0 0 .8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="1 4 .01"/>
</geometry>
</collision>
<inertial>
<mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="2 0 0"/>
</joint>
<joint name="base_to_belt" type="prismatic">
<parent link="base_link"/>
<child link="belt"/>
<axis xyz="0 1 0"/>
<limit effort="1000.0" lower="0" upper="1" velocity="0.5"/>
<origin xyz="1 0 0"/>
</joint>
</robot>
The plan was to have the blue strip much longer in the y direction so that when it moved, it simulated a conveyor for a long period of time. When I simulated this in RVIZ with the slide for the prismatic joint, the model worked and the belt moved.
Now I need to make a conveyor simulation in Gazebo and I accomplished this before by exerting a force on a different belt, see answer to this question:
http://answers.ros.org/question/52309/defining-joints-in-solidworks-to-urdf-exporter-for-a-conveyor-belt/
Trying to do what I did before, I spawned this Object in gazebo (added in gazebo reference for color):
and applied the wrench on the blue belt:
rosservice call /gazebo/apply_body_wrench '{body_name: "my_object::belt", reference_frame: "my_object::belt", wrench: { force: { x: 0, y: 1, z: 0 } }, start_time: 0, duration: -1 }'
However, it would not move.
I need to have this belt move and effect other robots in gazebo (so that the robots do not move it, but it moves other robots), and I do not know what the best way to have this done is.
I also looked into plugin tutorials, but when I tried it, I was getting errors, so before I go that route, I want to know if there is a simpler way of doing this.
I figured if I have the white platforms surrounding the belt, the robots on the platform/belt will not move the belt relative to the world, but will be effected by the belt's movement.
Am I applying the wrench wrong/making some mistake or is there no simple ...