Error adding controller script in simple urdf model to be simulated on GAZEBO.
I have URDF model of the single leg of a robot. My goal is to be able to give the joints a effort command and make it stand eventually. My URDF file is as follows.
<?xml version="1.0" ?>
<robot name="huboleg" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="Body_LHP">
<inertial>
<mass value="2.8201"/>
<inertia ixx="0.0295102" ixy="0.000184399" ixz="-0.000370291" iyy="0.0273771" iyz="0.00065658" izz="0.00838035"/>
<origin xyz="0.0195049 -0.0595775 -0.175202"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LHP_vis.dae"/>
</geometry>
<material name="random">
<color rgba="0.800000 0.800000 0.500000 1"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LHP_col.dae"/>
</geometry>
</collision>
</link>
<link name="Body_LKP">
<inertial>
<mass value="1.80912"/>
<inertia ixx="0.0232156" ixy="0.000251648" ixz="-0.00129343" iyy="0.0208342" iyz="0.00278068" izz="0.0059204"/>
<origin xyz="0.0128254 -0.00727567 -0.171431"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LKP_vis.dae"/>
</geometry>
<material name="random">
<color rgba="0.800000 0.800000 0.500000 1"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LKP_col.dae"/>
</geometry>
</collision>
</link>
<link name="Body_LAP">
<inertial>
<mass value="1.63501"/>
<inertia ixx="0.00231659" ixy="1.87402e-05" ixz="0.000369899" iyy="0.00330411" iyz="6.38153e-05" izz="0.00279495"/>
<origin xyz="0.0198702 -0.0459693 0.0115069"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LAP_vis.dae"/>
</geometry>
<material name="random">
<color rgba="0.800000 0.800000 0.500000 1"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LAP_col.dae"/>
</geometry>
</collision>
</link>
<link name="Body_LAR">
<inertial>
<mass value="1.20318"/>
<inertia ixx="0.00295183" ixy="3.23211e-05" ixz="0.000141769" iyy="0.00524792" iyz="5.95404e-05" izz="0.00516817"/>
<origin xyz="-0.0515094 0.00216398 -0.0693881"/>
</inertial>
<visual>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LAR_vis.dae"/>
</geometry>
<material name="random">
<color rgba="0.800000 0.800000 0.500000 1"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://hubo_leg/meshes/Body_LAR_col.dae"/>
</geometry>
</collision>
</link>
<joint name="LKP" type="revolute">
<origin rpy="0 -0 0" xyz="0.000766364 -0.0445011 -0.280007"/>
<parent link="Body_LHP"/>
<child link="Body_LKP"/>
<axis xyz="0 1 0"/>
<limit effort="10.0" lower="-0.0698132" upper="2.60054" velocity="1.0"/>
</joint>
<joint name="LAP" type="revolute">
<origin rpy="0 -0 0" xyz="7.20248e-06 0.0247555 -0.279942"/>
<parent link="Body_LKP"/>
<child link="Body_LAP"/>
<axis xyz="0 1 0"/>
<limit effort="10.0" lower="-1.29154" upper="1.69297" velocity="1.0"/>
</joint>
<joint name="LAR" type="revolute">
<origin rpy="0 -0 0" xyz="0.0711787 -0.0466006 -1.04e-10"/>
<parent link="Body_LAP"/>
<child link="Body_LAR"/>
<axis xyz="1 0 0"/>
<limit effort="10.0" lower="-0.191986" upper="0.191986" velocity="1.0"/>
</joint>
<gazebo>
<!-- robot model offset -->
<pose>0 0 .66 0 0 0</pose>
</gazebo>
<transmission name="LKP_trans" type="pr2_mechanism_model/SimpleTransmission">
<actuator name="LKP_motor" />
<joint name="LKP" />
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<transmission name="LAP_trans" type ...
I have the same problem with Gazebo 1.5: http://answers.gazebosim.org/question/2389/plugins-via-urdf-not-parsing/ I believe it is because Gazebo > V1.3 doesn't parse urdf plugins like it use to so it fails parsing the file as a urdf and then attempts to parse as a sdf instead (and fails).