ROS Components for hexapod_robot
No puede seleccionar más de 25 temas Los temas deben comenzar con una letra o número, pueden incluir guiones ('-') y pueden tener hasta 35 caracteres de largo.

180 líneas
7.0KB

  1. <?xml version="1.0"?>
  2. <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  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="revolute">
  57. <parent link="base_link"/>
  58. <child link="${name}_link"/>
  59. <origin xyz="${xy_offset} 0"/>
  60. <axis xyz="0 0 1"/>
  61. <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
  62. </joint>
  63. </xacro:macro>
  64. <xacro:macro name="create_femur" params="name parent_name side">
  65. <link name="${name}_link">
  66. <visual>
  67. <xacro:if value="${side == 'l'}"><origin xyz="-0.040 0 0"/></xacro:if>
  68. <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 0 0"/></xacro:if>
  69. <geometry>
  70. <box size="0.100 0.003 0.010"/>
  71. </geometry>
  72. <material name="green"/>
  73. </visual>
  74. <!--
  75. <visual>
  76. <xacro:if value="${side == 'l'}"><origin xyz="-0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
  77. <xacro:if value="${side == 'r'}"><origin xyz=" 0.040 -0.003 0" rpy="-1.57 0 0"/></xacro:if>
  78. <geometry>
  79. <cylinder radius="0.0125" length="0.006"/>
  80. </geometry>
  81. <material name="green"/>
  82. </visual>
  83. -->
  84. </link>
  85. <joint name="${name}_joint" type="revolute">
  86. <parent link="${parent_name}_link"/>
  87. <child link="${name}_link"/>
  88. <xacro:if value="${side == 'l'}"><origin xyz="-0.020 0.03 -0.0075"/></xacro:if>
  89. <xacro:if value="${side == 'r'}"><origin xyz=" 0.020 0.03 -0.0075"/></xacro:if>
  90. <axis xyz="0 -1 0"/>
  91. <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
  92. </joint>
  93. </xacro:macro>
  94. <xacro:macro name="create_tibia" params="name parent_name side">
  95. <link name="${name}_link">
  96. <!-- servo box -->
  97. <visual>
  98. <origin xyz="0 -0.005 0.010"/>
  99. <geometry>
  100. <box size="0.020 0.035 0.040"/>
  101. </geometry>
  102. <material name="red"/>
  103. </visual>
  104. <!-- tibia top -->
  105. <visual>
  106. <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 0.0025"/></xacro:if>
  107. <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 0.0025"/></xacro:if>
  108. <geometry>
  109. <box size="0.040 0.003 0.075"/>
  110. </geometry>
  111. <material name="red"/>
  112. </visual>
  113. <!-- tibia middle -->
  114. <visual>
  115. <xacro:if value="${side == 'l'}"><origin xyz="-0.005 0 -0.055"/></xacro:if>
  116. <xacro:if value="${side == 'r'}"><origin xyz=" 0.005 0 -0.055"/></xacro:if>
  117. <origin xyz="0 0 -0.055"/>
  118. <geometry>
  119. <box size="0.030 0.003 0.050"/>
  120. </geometry>
  121. <material name="red"/>
  122. </visual>
  123. <!-- tibia bottom -->
  124. <visual>
  125. <origin xyz=" 0.0 0 -0.105"/>
  126. <origin xyz="0 0 -0.105"/>
  127. <geometry>
  128. <box size="0.010 0.003 0.050"/>
  129. </geometry>
  130. <material name="red"/>
  131. </visual>
  132. <!-- tibia rotation axis -->
  133. <visual>
  134. <origin xyz="0 0 0" rpy="-1.57 0 0"/>
  135. <geometry>
  136. <cylinder radius="0.005" length="0.065"/>
  137. </geometry>
  138. <material name="red"/>
  139. </visual>
  140. </link>
  141. <joint name="${name}_joint" type="revolute">
  142. <parent link="${parent_name}_link"/>
  143. <child link="${name}_link"/>
  144. <xacro:if value="${side == 'l'}"><origin xyz="-0.080 -0.03 0"/></xacro:if>
  145. <xacro:if value="${side == 'r'}"><origin xyz=" 0.080 -0.03 0"/></xacro:if>
  146. <axis xyz="0 1 0"/>
  147. <limit effort="30" velocity="100" lower="-1.57" upper="1.57"/>
  148. </joint>
  149. </xacro:macro>
  150. <xacro:macro name="create_end" params="name side parent_name">
  151. <link name="${name}_link">
  152. <visual>
  153. <geometry>
  154. <sphere radius="0.004"/>
  155. </geometry>
  156. <material name="cyan"/>
  157. </visual>
  158. </link>
  159. <joint name="${name}_joint" type="fixed">
  160. <parent link="${parent_name}_link"/>
  161. <child link="${name}_link"/>
  162. <origin xyz="0 0 -0.130"/>
  163. </joint>
  164. </xacro:macro>
  165. <xacro:macro name="create_leg" params="number side xy_offset">
  166. <xacro:create_coxa name="c${number}" side="${side}" xy_offset="${xy_offset}"/>
  167. <xacro:create_femur name="f${number}" side="${side}" parent_name="c${number}"/>
  168. <xacro:create_tibia name="t${number}" side="${side}" parent_name="f${number}"/>
  169. <xacro:create_end name="e${number}" side="${side}" parent_name="t${number}"/>
  170. </xacro:macro>
  171. </robot>