- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
- <xacro:macro name="create_frame_plate" params="z_offset">
- <visual>
- <origin xyz="0 0 ${z_offset}"/>
- <geometry>
- <box size="0.100 0.180 0.003"/>
- </geometry>
- <material name="black"/>
- </visual>
- </xacro:macro>
-
- <xacro:macro name="create_coxa_joint_mount" params="xy_offset">
- <visual>
- <origin xyz="${xy_offset} -0.0325"/>
- <geometry>
- <cylinder radius="0.0125" length="0.006"/>
- </geometry>
- <material name="black"/>
- </visual>
- <visual>
- <origin xyz="${xy_offset} 0.0325"/>
- <geometry>
- <cylinder radius="0.0125" length="0.006"/>
- </geometry>
- <material name="black"/>
- </visual>
- </xacro:macro>
-
- <xacro:macro name="create_coxa" params="name side xy_offset">
- <link name="${name}_link">
- <!-- coxa bounding box -->
- <visual>
- <geometry>
- <box size="0.045 0.045 0.045"/>
- </geometry>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.015 -0.005 0"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.015 -0.005 0"/></xacro:if>
- <material name="blue"/>
- </visual>
- <!-- coxa rotation axis -->
- <visual>
- <geometry>
- <cylinder radius="0.005" length="0.065"/>
- </geometry>
- <material name="blue"/>
- </visual>
- <!-- femur rotation axis -->
- <visual>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
- <geometry>
- <cylinder radius="0.005" length="0.065"/>
- </geometry>
- <material name="blue"/>
- </visual>
- </link>
- <joint name="${name}_joint" type="continuous">
- <parent link="base_link"/>
- <child link="${name}_link"/>
- <origin xyz="${xy_offset} 0"/>
- <axis xyz="0 0 1"/>
- </joint>
- </xacro:macro>
-
- <xacro:macro name="create_femur" params="name parent_name side">
- <link name="${name}_link">
- <visual>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.040 0 0"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 0 0"/></xacro:if>
- <geometry>
- <box size="0.100 0.003 0.010"/>
- </geometry>
- <material name="green"/>
- </visual>
- <!--
- <visual>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
- <geometry>
- <cylinder radius="0.0125" length="0.006"/>
- </geometry>
- <material name="green"/>
- </visual>
- -->
- </link>
- <joint name="${name}_joint" type="continuous">
- <parent link="${parent_name}_link"/>
- <child link="${name}_link"/>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
- <axis xyz="0 1 0"/>
- </joint>
- </xacro:macro>
-
- <xacro:macro name="create_tibia" params="name parent_name side">
- <link name="${name}_link">
- <!-- servo box -->
- <visual>
- <origin xyz="0 -0.005 0.010"/>
- <geometry>
- <box size="0.020 0.035 0.040"/>
- </geometry>
- <material name="red"/>
- </visual>
- <!-- tibia top -->
- <visual>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 0.0025"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 0.0025"/></xacro:if>
- <geometry>
- <box size="0.040 0.003 0.075"/>
- </geometry>
- <material name="red"/>
- </visual>
- <!-- tibia middle -->
- <visual>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 -0.055"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 -0.055"/></xacro:if>
- <origin xyz="0 0 -0.055"/>
- <geometry>
- <box size="0.030 0.003 0.050"/>
- </geometry>
- <material name="red"/>
- </visual>
- <!-- tibia bottom -->
- <visual>
- <origin xyz=" 0.0 0 -0.105"/>
- <origin xyz="0 0 -0.105"/>
- <geometry>
- <box size="0.010 0.003 0.050"/>
- </geometry>
- <material name="red"/>
- </visual>
- <!-- tibia rotation axis -->
- <visual>
- <origin xyz="0 0 0" rpy="-1.57 0 0"/>
- <geometry>
- <cylinder radius="0.005" length="0.065"/>
- </geometry>
- <material name="red"/>
- </visual>
-
- </link>
- <joint name="${name}_joint" type="continuous">
- <parent link="${parent_name}_link"/>
- <child link="${name}_link"/>
- <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
- <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
- <axis xyz="0 1 0"/>
- </joint>
- </xacro:macro>
-
- <xacro:macro name="create_end" params="name side parent_name">
- <link name="${name}_link">
- <visual>
- <geometry>
- <sphere radius="0.004"/>
- </geometry>
- <material name="cyan"/>
- </visual>
- </link>
- <joint name="${name}_joint" type="fixed">
- <parent link="${parent_name}_link"/>
- <child link="${name}_link"/>
- <origin xyz="0 0 -0.130"/>
- </joint>
- </xacro:macro>
-
- <xacro:macro name="create_leg" params="number side xy_offset">
- <xacro:create_coxa name="c${number}" side="${side}" xy_offset="${xy_offset}"/>
- <xacro:create_femur name="f${number}" side="${side}" parent_name="c${number}"/>
- <xacro:create_tibia name="t${number}" side="${side}" parent_name="f${number}"/>
- <xacro:create_end name="e${number}" side="${side}" parent_name="t${number}"/>
- </xacro:macro>
-
- </robot>
|