CM_DOG/src/robot_descriptions/qut/mdog_description/xacro/leg.xacro

98 lines
2.9 KiB
XML

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg" params="name mirror front_hind">
<!-- 定义hip_joint -->
<joint name="${name}_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="${front_hind * 0.23} ${mirror * 0.15} 0"/>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0.1" friction="0.1"/>
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
</joint>
<link name="${name}_hip">
<visual>
<origin rpy="1.5708 0 0" xyz="0 ${mirror * -0.03} 0"/>
<geometry>
<cylinder length="0.05" radius="0.05"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin rpy="1.5708 0 0" xyz="0 ${mirror * -0.03} 0"/>
<geometry>
<cylinder length="0.05" radius="0.10"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- 定义thigh_joint -->
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 -0.7854 0" xyz="0 ${mirror * 0.05} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.1" friction="0.1"/>
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
<geometry>
<box size="0.25 0.05 0.05"/>
</geometry>
<material name="light-grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
<geometry>
<box size="0.25 0.05 0.05"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- 定义calf_joint -->
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 -1.5708 0" xyz="-0.25 0 0"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.1" friction="0.1"/>
<limit effort="20" velocity="10" lower="-1.57" upper="1.57"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
<geometry>
<box size="0.25 0.05 0.05"/>
</geometry>
<material name="grey"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.125 0 0"/>
<geometry>
<box size="0.25 0.05 0.05"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
</xacro:macro>
</robot>