简易小车案例xacro

需求

分析

总共有7个部件，4个轮子，1个板子，2个联轴杆.

抽象成urdf，则是有7个link。

这个7个link是通过6个joint结合在一起的。

其中，板子和2个联轴杆属于固定连接在一起的。

联轴杆和4个轮子是通过旋转方式连接在一起的。

实现

<?xml version="1.0" encoding="UTF-8" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="joint1">

    <material name="black">
        <color rgba="0 0 0 0.8"/>
    </material>

    <material name="grey">
        <color rgba="0.8 0.8 0.8 0.8"/>
    </material>

    <xacro:property name="pi" value="3.1415926"/>
    <xacro:property name="d_180" value="${pi}"/>
    <xacro:property name="d_90" value="${pi/2}"/>

    <xacro:property name="board_x" value="0.4"/>
    <xacro:property name="board_y" value="0.2"/>
    <xacro:property name="board_z" value="0.03"/>

    <xacro:property name="extra_len" value="0.02"/>
    <xacro:property name="extra_radius" value="${board_z/3}"/>

    <xacro:property name="wheel_width" value="0.03" />
    <xacro:property name="wheel_radius" value="0.05" />

    <xacro:macro name="HMBox" params="name xyz='0 0 0' rpy='0 0 0' size color='grey'">
        <link name="${name}">
            <visual>
                <origin xyz="${xyz}" rpy="${rpy}"/>
                <geometry>
                    <box size="${size}"/>
                </geometry>
                <material name="${color}"/>
            </visual>
        </link>
    </xacro:macro>

    <xacro:macro name="HMCylinder" params="name xyz='0 0 0' rpy='0 0 0' length radius color='grey'">
        <link name="${name}">
            <visual>
                <origin xyz="${xyz}" rpy="${rpy}"/>
                <geometry>
                    <cylinder length="${length}" radius="${radius}"/>
                </geometry>
                <material name="${color}"/>
            </visual>
        </link>
    </xacro:macro>

    <xacro:macro name="HMJoint" params="name type='fixed' parent child xyz='0 0 0' rpy='0 0 0' axis='1 0 0'">
        <joint name="${name}" type="${type}">
            <parent link="${parent}"/>
            <child link="${child}"/>
            <origin xyz="${xyz}" rpy="${rpy}"/>
            <axis xyz="${axis}"/>
        </joint>
    </xacro:macro>

    <!-- link -->
    <xacro:HMBox name="base" size="${board_x} ${board_y} ${board_z}"/>

    <!-- link -->
    <xacro:HMCylinder name="front"
                      length="${extra_len * 2 + board_y}"
                      radius="${extra_radius}"
                      rpy="${d_90} 0 0" />

    <xacro:HMJoint name="joint0"
                   parent="base"
                   child="front"
                   xyz="${board_x/2 - extra_len} 0 0" />

    <!-- link -->
    <xacro:HMCylinder name="back"
                      length="${extra_len * 2 + board_y}"
                      radius="${extra_radius}"
                      rpy="${d_90} 0 0" />

    <xacro:HMJoint name="joint1"
                   parent="base"
                   child="back"
                   xyz="-${board_x/2 - extra_len} 0 0" />

    <xacro:macro name="HMWheel" params="name left='true'">
        <xacro:if value="${left}">
            <xacro:HMCylinder name="${name}"
                              length="${wheel_width}"
                              radius="${wheel_radius}"
                              xyz="0 ${wheel_width/2} 0"
                              rpy="${d_90} 0 0"
                              color="black" />
        </xacro:if>
        <xacro:unless value="${left}">
            <xacro:HMCylinder name="${name}"
                              length="${wheel_width}"
                              radius="${wheel_radius}"
                              xyz="0 -${wheel_width/2} 0"
                              rpy="${d_90} 0 0"
                              color="black" />
        </xacro:unless>

    </xacro:macro>

    <xacro:HMWheel name="wheel_front_left"/>

    <xacro:HMJoint name="joint2"
                   parent="front"
                   child="wheel_front_left"
                   type="continuous"
                   xyz="0 ${board_y/2 + extra_len} 0"
                   axis="0 1 0" />

    <xacro:HMWheel name="wheel_front_right" left="false"/>

    <joint name="joint3" type="continuous">
        <parent link="front"/>
        <child link="wheel_front_right"/>
        <origin xyz="0 -0.12 0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
    </joint>

    <xacro:HMWheel name="wheel_back_left"/>

    <joint name="joint4" type="continuous">
        <parent link="back"/>
        <child link="wheel_back_left"/>
        <origin xyz="0 0.12 0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
    </joint>

    <xacro:HMWheel name="wheel_back_right" left="false"/>

    <joint name="joint5" type="continuous">
        <parent link="back"/>
        <child link="wheel_back_right"/>
        <origin xyz="0 -0.12 0" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
    </joint>

</robot>