Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
265 lines (245 sloc) 7.21 KB
<?xml version="1.0"?>
<robot name="simple_arm">
<link name="world"/>
<!-- all inertia param below set like Izz = (1/12)*(x^2+y^2), that approximately as box
-->
<link name="base">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.005"/>
<geometry>
<cylinder length="0.01" radius="0.02"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.005"/>
<geometry>
<cylinder length="0.01" radius="0.02"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.005"/>
<mass value="1.0"/>
<inertia ixx="0.00014" ixy="0.0" ixz="0.0"
iyy="0.00014" iyz="0.0"
izz="0.000266"/>
</inertial>
</link>
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<box size="0.02 0.02 0.03"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<box size="0.02 0.02 0.03"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="0.000108" ixy="0.0" ixz="0.0"
iyy="0.000108" iyz="0.0"
izz="0.000066"/>
</inertial>
</link>
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.045"/>
<geometry>
<box size="0.02 0.02 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.045"/>
<geometry>
<box size="0.02 0.02 0.1"/>
</geometry>
<material name="gray">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.6"/>
<inertia ixx="0.00052" ixy="0.0" ixz="0.0"
iyy="0.00052" iyz="0.0"
izz="0.00004"/>
</inertial>
</link>
<link name="link3">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.045"/>
<geometry>
<box size="0.02 0.02 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.045"/>
<geometry>
<box size="0.02 0.02 0.1"/>
</geometry>
<material name="gray">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.6"/>
<inertia ixx="0.00052" ixy="0.0" ixz="0.0"
iyy="0.00052" iyz="0.0"
izz="0.00004"/>
</inertial>
</link>
<link name="link4">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.005"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.005"/>
<geometry>
<box size="0.02 0.02 0.02"/>
</geometry>
<material name="gray">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.3"/>
<inertia ixx="0.00002" ixy="0.0" ixz="0.0"
iyy="0.00002" iyz="0.0"
izz="0.00002"/>
</inertial>
</link>
<link name="link5">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.003"/>
<geometry>
<cylinder length="0.006" radius="0.01"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.003"/>
<geometry>
<cylinder length="0.006" radius="0.01"/>
</geometry>
<material name="gray">
<color rgba="1 1 1 0.5"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.2"/>
<inertia ixx="0.00000726" ixy="0.0" ixz="0.0"
iyy="0.00000726" iyz="0.0"
izz="0.0000133"/>
</inertial>
</link>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base"/>
</joint>
<joint name="joint1" type="revolute">
<parent link="base"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<limit effort="10000" lower="-3.14" upper="3.14" velocity="0.5"/>
</joint>
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 0 0.02"/>
<limit effort="10000" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>
<joint name="joint3" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 0 0.09"/>
<limit effort="10000" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>
<joint name="joint4" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 0 0.09"/>
<limit effort="10000" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>
<joint name="joint5" type="revolute">
<parent link="link4"/>
<child link="link5"/>
<axis xyz="0 0 1"/>
<origin rpy="0 0 0" xyz="0 0 0.013"/>
<limit effort="10000" lower="-3.14" upper="3.14" velocity="0.5"/>
</joint>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/simple_arm</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint4">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor4">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran5">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint5">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor5">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>