Driving a simple robot model (1.5 Building a Robot with SDF Tutorial)
Hello,
I'm trying to follow the tutorial that describes how to create a robot model from model.sdf file on Gazebo 1.5. I intend to make my own robot, and be able to drive it, differential or skid steering, doesn't matter. But I think there is something wrong with the robot provided, I'm not able to drive it by subscribing to /gazebo/cmd[underscore]vel topic, or using the differential drive plugin (for /gz/cmd[underscore]vel/ topic). I've narrowed the problems down and now I think the .sdf file provided has some problems (maybe a missing tag, link, or dummy container, or some wrong parameters). Since I don't know how exactly a sdf file is constructed and described, I need some help revising the model.sdf file below:
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="myrobot">
<static>false</static>
<link name="chassis">
<pose>0 0 .1 0 0 0</pose>
<inertial>
<mass>1.0</mass>
<inertia>
<ixx>1.0</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>1.0</iyy>
<iyz>0.0</iyz>
<izz>1.0</izz>
</inertia>
</inertial>
<self_collide>1</self_collide>
<collision name="collision">
<geometry>
<box>
<size>.4 .2 .1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>.4 .2 .1</size>
</box>
</geometry>
</visual>
<collision name="caster_collision">
<pose>-0.15 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
<slip1>1.0</slip1>
<slip2>1.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="caster_visual">
<pose>-0.15 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="left_wheel">
<pose>0.1 0.13 0.1 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_wheel">
<pose>0.1 -0.13 0.1 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
<joint type="revolute" name="left_wheel_hinge">
<pose>0 0 -0.03 0 0 0</pose>
<child>left_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint type="revolute" name="right_wheel_hinge">
<pose>0 0 0.03 0 0 0</pose>
<child>right_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<!-- Sensor 3 -->
<include>
<uri>model://hokuyo</uri>
<pose>-0.2 0 0.2 0 0 15.75</pose>
</include>
<joint name="hokuyo_joint" type="revolute">
<child>hokuyo::link</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 -1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- -->
<!-- Sensor1 -->
<include>
<uri>model://hokuyo2</uri>
<pose>-0.2 0.04 0.2 0 0 15.15</pose>
</include>
<joint name="hokuyo_joint2" type ...
<plugin name="differentialdrivecontroller" filename="libmyrobotdiffdrive.so"> <alwaysOn><tr> </tr>ue</alwaysOn> Maybe this is the problem?
Try using continous joints instead of revolute s. http://wiki.ros.org/urdf/XML/joint revolute - a hinge joint that rotates along the axis and has a limited range specified by the upper and lower limits. continuous - a continuous hinge joint that rotates around the axis and has no upper and lower limits