ROS Components for hexapod_robot
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

177 rindas
6.8KB

  1. <?xml version="1.0"?>
  2. <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hexapod_robot">
  3. <xacro:macro name="create_frame_plate" params="z_offset">
  4. <visual>
  5. <origin xyz="0 0 ${z_offset}"/>
  6. <geometry>
  7. <box size="0.100 0.180 0.003"/>
  8. </geometry>
  9. <material name="black"/>
  10. </visual>
  11. </xacro:macro>
  12. <xacro:macro name="create_coxa_joint_mount" params="xy_offset">
  13. <visual>
  14. <origin xyz="${xy_offset} -0.0325"/>
  15. <geometry>
  16. <cylinder radius="0.0125" length="0.006"/>
  17. </geometry>
  18. <material name="black"/>
  19. </visual>
  20. <visual>
  21. <origin xyz="${xy_offset} 0.0325"/>
  22. <geometry>
  23. <cylinder radius="0.0125" length="0.006"/>
  24. </geometry>
  25. <material name="black"/>
  26. </visual>
  27. </xacro:macro>
  28. <xacro:macro name="create_coxa" params="name side xy_offset">
  29. <link name="${name}_link">
  30. <!-- coxa bounding box -->
  31. <visual>
  32. <geometry>
  33. <box size="0.045 0.045 0.045"/>
  34. </geometry>
  35. <xacro:if value="${side == 'l'}"><origin xyz="-0.015 -0.005 0"/></xacro:if>
  36. <xacro:if value="${side == 'r'}"><origin xyz=" 0.015 -0.005 0"/></xacro:if>
  37. <material name="blue"/>
  38. </visual>
  39. <!-- coxa rotation axis -->
  40. <visual>
  41. <geometry>
  42. <cylinder radius="0.005" length="0.065"/>
  43. </geometry>
  44. <material name="blue"/>
  45. </visual>
  46. <!-- femur rotation axis -->
  47. <visual>
  48. <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
  49. <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0 -0.0075" rpy="-1.57 0 0"/></xacro:if>
  50. <geometry>
  51. <cylinder radius="0.005" length="0.065"/>
  52. </geometry>
  53. <material name="blue"/>
  54. </visual>
  55. </link>
  56. <joint name="${name}_joint" type="continuous">
  57. <parent link="base_link"/>
  58. <child link="${name}_link"/>
  59. <origin xyz="${xy_offset} 0"/>
  60. <axis xyz="0 0 1"/>
  61. </joint>
  62. </xacro:macro>
  63. <xacro:macro name="create_femur" params="name parent_name side">
  64. <link name="${name}_link">
  65. <visual>
  66. <xacro:if value="${side == 'l'}"><origin xyz="-0.040 0 0"/></xacro:if>
  67. <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 0 0"/></xacro:if>
  68. <geometry>
  69. <box size="0.100 0.003 0.010"/>
  70. </geometry>
  71. <material name="green"/>
  72. </visual>
  73. <!--
  74. <visual>
  75. <xacro:if value="${side == 'l'}"><origin xyz="-0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
  76. <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
  77. <geometry>
  78. <cylinder radius="0.0125" length="0.006"/>
  79. </geometry>
  80. <material name="green"/>
  81. </visual>
  82. -->
  83. </link>
  84. <joint name="${name}_joint" type="continuous">
  85. <parent link="${parent_name}_link"/>
  86. <child link="${name}_link"/>
  87. <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
  88. <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
  89. <axis xyz="0 1 0"/>
  90. </joint>
  91. </xacro:macro>
  92. <xacro:macro name="create_tibia" params="name parent_name side">
  93. <link name="${name}_link">
  94. <!-- servo box -->
  95. <visual>
  96. <origin xyz="0 -0.005 0.010"/>
  97. <geometry>
  98. <box size="0.020 0.035 0.040"/>
  99. </geometry>
  100. <material name="red"/>
  101. </visual>
  102. <!-- tibia top -->
  103. <visual>
  104. <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 0.0025"/></xacro:if>
  105. <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 0.0025"/></xacro:if>
  106. <geometry>
  107. <box size="0.040 0.003 0.075"/>
  108. </geometry>
  109. <material name="red"/>
  110. </visual>
  111. <!-- tibia middle -->
  112. <visual>
  113. <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 -0.055"/></xacro:if>
  114. <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 -0.055"/></xacro:if>
  115. <origin xyz="0 0 -0.055"/>
  116. <geometry>
  117. <box size="0.030 0.003 0.050"/>
  118. </geometry>
  119. <material name="red"/>
  120. </visual>
  121. <!-- tibia bottom -->
  122. <visual>
  123. <origin xyz=" 0.0 0 -0.105"/>
  124. <origin xyz="0 0 -0.105"/>
  125. <geometry>
  126. <box size="0.010 0.003 0.050"/>
  127. </geometry>
  128. <material name="red"/>
  129. </visual>
  130. <!-- tibia rotation axis -->
  131. <visual>
  132. <origin xyz="0 0 0" rpy="-1.57 0 0"/>
  133. <geometry>
  134. <cylinder radius="0.005" length="0.065"/>
  135. </geometry>
  136. <material name="red"/>
  137. </visual>
  138. </link>
  139. <joint name="${name}_joint" type="continuous">
  140. <parent link="${parent_name}_link"/>
  141. <child link="${name}_link"/>
  142. <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
  143. <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
  144. <axis xyz="0 1 0"/>
  145. </joint>
  146. </xacro:macro>
  147. <xacro:macro name="create_end" params="name side parent_name">
  148. <link name="${name}_link">
  149. <visual>
  150. <geometry>
  151. <sphere radius="0.004"/>
  152. </geometry>
  153. <material name="cyan"/>
  154. </visual>
  155. </link>
  156. <joint name="${name}_joint" type="fixed">
  157. <parent link="${parent_name}_link"/>
  158. <child link="${name}_link"/>
  159. <origin xyz="0 0 -0.130"/>
  160. </joint>
  161. </xacro:macro>
  162. <xacro:macro name="create_leg" params="number side xy_offset">
  163. <xacro:create_coxa name="c${number}" side="${side}" xy_offset="${xy_offset}"/>
  164. <xacro:create_femur name="f${number}" side="${side}" parent_name="c${number}"/>
  165. <xacro:create_tibia name="t${number}" side="${side}" parent_name="f${number}"/>
  166. <xacro:create_end name="e${number}" side="${side}" parent_name="t${number}"/>
  167. </xacro:macro>
  168. </robot>