r2d2.urdf 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418
  1. <?xml version="1.0"?>
  2. <robot name="physics">
  3. <link name="base_link">
  4. <visual>
  5. <geometry>
  6. <cylinder length="0.6" radius="0.2"/>
  7. </geometry>
  8. <material name="blue">
  9. <color rgba="0 0 .8 1"/>
  10. </material>
  11. </visual>
  12. <collision>
  13. <geometry>
  14. <cylinder length="0.6" radius="0.2"/>
  15. </geometry>
  16. </collision>
  17. <inertial>
  18. <mass value="10"/>
  19. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  20. </inertial>
  21. </link>
  22. <link name="right_leg">
  23. <visual>
  24. <geometry>
  25. <box size="0.6 .2 .1"/>
  26. </geometry>
  27. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  28. <material name="white">
  29. <color rgba="1 1 1 1"/>
  30. </material>
  31. </visual>
  32. <collision>
  33. <geometry>
  34. <box size="0.6 .2 .1"/>
  35. </geometry>
  36. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  37. </collision>
  38. <inertial>
  39. <mass value="10"/>
  40. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  41. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  42. </inertial>
  43. </link>
  44. <joint name="base_to_right_leg" type="fixed">
  45. <parent link="base_link"/>
  46. <child link="right_leg"/>
  47. <origin xyz="0.22 0 .25"/>
  48. </joint>
  49. <link name="right_base">
  50. <visual>
  51. <geometry>
  52. <box size=".1 0.4 .1"/>
  53. </geometry>
  54. <material name="white"/>
  55. </visual>
  56. <collision>
  57. <geometry>
  58. <box size=".1 0.4 .1"/>
  59. </geometry>
  60. </collision>
  61. <inertial>
  62. <mass value="10"/>
  63. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  64. </inertial>
  65. </link>
  66. <joint name="right_base_joint" type="fixed">
  67. <parent link="right_leg"/>
  68. <child link="right_base"/>
  69. <origin xyz="0 0 -0.6"/>
  70. </joint>
  71. <link name="right_front_wheel">
  72. <visual>
  73. <geometry>
  74. <cylinder length=".1" radius="0.035"/>
  75. </geometry>
  76. <material name="black">
  77. <color rgba="0.5 0.5 0.5 1"/>
  78. </material>
  79. </visual>
  80. <collision>
  81. <geometry>
  82. <cylinder length=".1" radius="0.035"/>
  83. </geometry>
  84. </collision>
  85. <inertial>
  86. <mass value="1"/>
  87. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  88. </inertial>
  89. </link>
  90. <joint name="right_front_wheel_joint" type="continuous">
  91. <axis xyz="0 0 1"/>
  92. <parent link="right_base"/>
  93. <child link="right_front_wheel"/>
  94. <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
  95. <limit effort="100" velocity="100"/>
  96. <joint_properties damping="0.0" friction="0.0"/>
  97. </joint>
  98. <link name="right_back_wheel">
  99. <visual>
  100. <geometry>
  101. <cylinder length=".1" radius="0.035"/>
  102. </geometry>
  103. <material name="black"/>
  104. </visual>
  105. <collision>
  106. <geometry>
  107. <cylinder length=".1" radius="0.035"/>
  108. </geometry>
  109. </collision>
  110. <inertial>
  111. <mass value="1"/>
  112. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  113. </inertial>
  114. </link>
  115. <joint name="right_back_wheel_joint" type="continuous">
  116. <axis xyz="0 0 1"/>
  117. <parent link="right_base"/>
  118. <child link="right_back_wheel"/>
  119. <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
  120. <limit effort="100" velocity="100"/>
  121. <joint_properties damping="0.0" friction="0.0"/>
  122. </joint>
  123. <link name="left_leg">
  124. <visual>
  125. <geometry>
  126. <box size="0.6 .2 .1"/>
  127. </geometry>
  128. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  129. <material name="white"/>
  130. </visual>
  131. <collision>
  132. <geometry>
  133. <box size="0.6 .2 .1"/>
  134. </geometry>
  135. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  136. </collision>
  137. <inertial>
  138. <mass value="10"/>
  139. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  140. <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
  141. </inertial>
  142. </link>
  143. <joint name="base_to_left_leg" type="fixed">
  144. <parent link="base_link"/>
  145. <child link="left_leg"/>
  146. <origin xyz="-0.22 0 .25"/>
  147. </joint>
  148. <link name="left_base">
  149. <visual>
  150. <geometry>
  151. <box size=".1 0.4 .1"/>
  152. </geometry>
  153. <material name="white"/>
  154. </visual>
  155. <collision>
  156. <geometry>
  157. <box size=".1 0.4 .1"/>
  158. </geometry>
  159. </collision>
  160. <inertial>
  161. <mass value="10"/>
  162. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  163. </inertial>
  164. </link>
  165. <joint name="left_base_joint" type="fixed">
  166. <parent link="left_leg"/>
  167. <child link="left_base"/>
  168. <origin xyz="0 0 -0.6"/>
  169. </joint>
  170. <link name="left_front_wheel">
  171. <visual>
  172. <geometry>
  173. <cylinder length=".1" radius="0.035"/>
  174. </geometry>
  175. <material name="black"/>
  176. </visual>
  177. <collision>
  178. <geometry>
  179. <cylinder length=".1" radius="0.035"/>
  180. </geometry>
  181. </collision>
  182. <inertial>
  183. <mass value="1"/>
  184. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  185. </inertial>
  186. </link>
  187. <joint name="left_front_wheel_joint" type="continuous">
  188. <axis xyz="0 0 1"/>
  189. <parent link="left_base"/>
  190. <child link="left_front_wheel"/>
  191. <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
  192. <limit effort="100" velocity="100"/>
  193. <joint_properties damping="0.0" friction="0.0"/>
  194. </joint>
  195. <link name="left_back_wheel">
  196. <visual>
  197. <geometry>
  198. <cylinder length=".1" radius="0.035"/>
  199. </geometry>
  200. <material name="black"/>
  201. </visual>
  202. <collision>
  203. <geometry>
  204. <cylinder length=".1" radius="0.035"/>
  205. </geometry>
  206. </collision>
  207. <inertial>
  208. <mass value="1"/>
  209. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  210. </inertial>
  211. </link>
  212. <joint name="left_back_wheel_joint" type="continuous">
  213. <axis xyz="0 0 1"/>
  214. <parent link="left_base"/>
  215. <child link="left_back_wheel"/>
  216. <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
  217. <limit effort="100" velocity="100"/>
  218. <joint_properties damping="0.0" friction="0.0"/>
  219. </joint>
  220. <joint name="gripper_extension" type="prismatic">
  221. <axis xyz="1 0 0"/>
  222. <parent link="base_link"/>
  223. <child link="gripper_pole"/>
  224. <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
  225. <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
  226. </joint>
  227. <link name="gripper_pole">
  228. <visual>
  229. <geometry>
  230. <cylinder length="0.2" radius=".01"/>
  231. </geometry>
  232. <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
  233. <material name="Gray">
  234. <color rgba=".7 .7 .7 1"/>
  235. </material>
  236. </visual>
  237. <collision>
  238. <geometry>
  239. <cylinder length="0.2" radius=".01"/>
  240. </geometry>
  241. <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
  242. </collision>
  243. <inertial>
  244. <mass value="0.05"/>
  245. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  246. </inertial>
  247. </link>
  248. <joint name="left_gripper_joint" type="revolute">
  249. <axis xyz="0 0 1"/>
  250. <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  251. <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
  252. <parent link="gripper_pole"/>
  253. <child link="left_gripper"/>
  254. </joint>
  255. <link name="left_gripper">
  256. <visual>
  257. <origin rpy="0.0 0 0" xyz="0 0 0"/>
  258. <geometry>
  259. <mesh filename="l_finger.stl"/>
  260. </geometry>
  261. </visual>
  262. <collision>
  263. <geometry>
  264. <mesh filename="l_finger.stl"/>
  265. </geometry>
  266. <origin rpy="0.0 0 0" xyz="0 0 0"/>
  267. </collision>
  268. <inertial>
  269. <mass value="0.05"/>
  270. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  271. </inertial>
  272. </link>
  273. <joint name="left_tip_joint" type="fixed">
  274. <parent link="left_gripper"/>
  275. <child link="left_tip"/>
  276. </joint>
  277. <link name="left_tip">
  278. <visual>
  279. <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
  280. <geometry>
  281. <mesh filename="l_finger_tip.stl"/>
  282. </geometry>
  283. </visual>
  284. <collision>
  285. <geometry>
  286. <mesh filename="l_finger_tip.stl"/>
  287. </geometry>
  288. <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
  289. </collision>
  290. <inertial>
  291. <mass value="0.05"/>
  292. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  293. </inertial>
  294. </link>
  295. <joint name="right_gripper_joint" type="revolute">
  296. <axis xyz="0 0 -1"/>
  297. <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
  298. <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
  299. <parent link="gripper_pole"/>
  300. <child link="right_gripper"/>
  301. </joint>
  302. <link name="right_gripper">
  303. <visual>
  304. <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
  305. <geometry>
  306. <mesh filename="l_finger.stl"/>
  307. </geometry>
  308. </visual>
  309. <collision>
  310. <geometry>
  311. <mesh filename="l_finger.stl"/>
  312. </geometry>
  313. <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
  314. </collision>
  315. <inertial>
  316. <mass value="0.05"/>
  317. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  318. </inertial>
  319. </link>
  320. <joint name="right_tip_joint" type="fixed">
  321. <parent link="right_gripper"/>
  322. <child link="right_tip"/>
  323. </joint>
  324. <link name="right_tip">
  325. <visual>
  326. <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
  327. <geometry>
  328. <mesh filename="l_finger_tip.stl"/>
  329. </geometry>
  330. </visual>
  331. <collision>
  332. <geometry>
  333. <mesh filename="l_finger_tip.stl"/>
  334. </geometry>
  335. <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
  336. </collision>
  337. <inertial>
  338. <mass value="0.05"/>
  339. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  340. </inertial>
  341. </link>
  342. <link name="head">
  343. <visual>
  344. <geometry>
  345. <sphere radius="0.2"/>
  346. </geometry>
  347. <material name="white"/>
  348. </visual>
  349. <collision>
  350. <geometry>
  351. <sphere radius="0.2"/>
  352. </geometry>
  353. </collision>
  354. <inertial>
  355. <mass value="10"/>
  356. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  357. </inertial>
  358. </link>
  359. <joint name="head_swivel" type="continuous">
  360. <parent link="base_link"/>
  361. <child link="head"/>
  362. <axis xyz="0 0 1"/>
  363. <origin xyz="0 0 0.3"/>
  364. <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
  365. </joint>
  366. <link name="box">
  367. <visual>
  368. <geometry>
  369. <box size=".08 .08 .08"/>
  370. </geometry>
  371. <material name="blue"/>
  372. </visual>
  373. <collision>
  374. <geometry>
  375. <box size=".08 .08 .08"/>
  376. </geometry>
  377. </collision>
  378. <inertial>
  379. <mass value="1"/>
  380. <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
  381. </inertial>
  382. </link>
  383. <joint name="tobox" type="fixed">
  384. <parent link="head"/>
  385. <child link="box"/>
  386. <origin xyz="0 0.1414 0.1414"/>
  387. </joint>
  388. </robot>