quadruped_ros2_control/descriptions/unitree/a1_description/xacro/robot.xacro

151 lines
5.4 KiB
XML

<?xml version="1.0"?>
<robot name="a1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:arg name="GAZEBO" default="false"/>
<xacro:include filename="$(find a1_description)/xacro/const.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/leg.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
<xacro:if value="$(arg GAZEBO)">
<xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
</xacro:if>
<xacro:unless value="$(arg GAZEBO)">
<xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
</xacro:unless>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
<!-- Rollover Protection mode will add an additional stick on the top, use "true" or "false" to switch it. -->
<xacro:property name="rolloverProtection" value="false"/>
<!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
<xacro:if value="$(arg DEBUG)">
<link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint>
</xacro:if>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find a1_description)/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
<mass value="${trunk_mass}"/>
<inertia
ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
iyy="${trunk_iyy}" iyz="${trunk_iyz}"
izz="${trunk_izz}"/>
</inertial>
</link>
<xacro:if value="${(rolloverProtection == 'true')}">
<joint name="stick_joint" type="fixed">
<parent link="trunk"/>
<child link="stick_link"/>
<origin rpy="0 0 0" xyz="${0.18} 0 ${stick_length/2.0+0.08}"/>
</joint>
<link name="stick_link">
<visual>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="${stick_length}" radius="${stick_radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="${stick_mass}"/>
<inertia
ixx="${stick_mass / 2.0 * (stick_radius*stick_radius)}" ixy="0.0" ixz="0.0"
iyy="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}" iyz="0.0"
izz="${stick_mass / 12.0 * (3*stick_radius*stick_radius + stick_length*stick_length)}"/>
</inertial>
</link>
</xacro:if>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:leg>
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:leg>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
</robot>