reachy-mini 1.0.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.

Potentially problematic release.


This version of reachy-mini might be problematic. Click here for more details.

Files changed (385) hide show
  1. reachy_mini/__init__.py +4 -0
  2. reachy_mini/apps/__init__.py +24 -0
  3. reachy_mini/apps/app.py +121 -0
  4. reachy_mini/apps/manager.py +183 -0
  5. reachy_mini/apps/sources/__init__.py +4 -0
  6. reachy_mini/apps/sources/hf_space.py +25 -0
  7. reachy_mini/apps/sources/local_common_venv.py +44 -0
  8. reachy_mini/apps/templates/README.md.j2 +1 -0
  9. reachy_mini/apps/templates/main.py.j2 +46 -0
  10. reachy_mini/apps/templates/pyproject.toml.j2 +18 -0
  11. reachy_mini/apps/utils.py +30 -0
  12. reachy_mini/assets/config/hardware_config.yaml +119 -0
  13. reachy_mini/assets/confused1.wav +0 -0
  14. reachy_mini/assets/count.wav +0 -0
  15. reachy_mini/assets/dance1.wav +0 -0
  16. reachy_mini/assets/go_sleep.wav +0 -0
  17. reachy_mini/assets/impatient1.wav +0 -0
  18. reachy_mini/assets/kinematics_data.json +253 -0
  19. reachy_mini/assets/models/fknetwork.dynamic.onnx +0 -0
  20. reachy_mini/assets/models/fknetwork.onnx +0 -0
  21. reachy_mini/assets/models/fknetwork_int8.onnx +0 -0
  22. reachy_mini/assets/models/iknetwork.onnx +0 -0
  23. reachy_mini/assets/wake_up.wav +0 -0
  24. reachy_mini/daemon/__init__.py +1 -0
  25. reachy_mini/daemon/app/__init__.py +1 -0
  26. reachy_mini/daemon/app/bg_job_register.py +142 -0
  27. reachy_mini/daemon/app/dashboard/static/assets/KO-cartoon-static.svg +40 -0
  28. reachy_mini/daemon/app/dashboard/static/assets/awake-cartoon-static.svg +42 -0
  29. reachy_mini/daemon/app/dashboard/static/assets/awake-cartoon.svg +6 -0
  30. reachy_mini/daemon/app/dashboard/static/assets/go-to-sleep-cartoon.svg +6 -0
  31. reachy_mini/daemon/app/dashboard/static/assets/no-wifi-cartoon-static.svg +50 -0
  32. reachy_mini/daemon/app/dashboard/static/assets/no-wifi-cartoon.svg +6 -0
  33. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-awake.svg +18 -0
  34. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-connection-lost-animation.svg +6 -0
  35. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-go-to-sleep-animation.svg +6 -0
  36. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-ko-animation.svg +6 -0
  37. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-ko.svg +22 -0
  38. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-sleeping-static.svg +41 -0
  39. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-sleeping.svg +18 -0
  40. reachy_mini/daemon/app/dashboard/static/assets/reachy-mini-wake-up-animation.svg +6 -0
  41. reachy_mini/daemon/app/dashboard/static/js/3rdparty/gstwebrtc-api-2.0.0.min.js +5 -0
  42. reachy_mini/daemon/app/dashboard/static/js/apps.js +304 -0
  43. reachy_mini/daemon/app/dashboard/static/js/appstore.js +150 -0
  44. reachy_mini/daemon/app/dashboard/static/js/daemon.js +163 -0
  45. reachy_mini/daemon/app/dashboard/static/js/move_player.js +132 -0
  46. reachy_mini/daemon/app/dashboard/static/js/setup_wifi.js +94 -0
  47. reachy_mini/daemon/app/dashboard/static/js/update.js +80 -0
  48. reachy_mini/daemon/app/dashboard/static/style.css +83 -0
  49. reachy_mini/daemon/app/dashboard/templates/base.html +23 -0
  50. reachy_mini/daemon/app/dashboard/templates/index.html +17 -0
  51. reachy_mini/daemon/app/dashboard/templates/sections/apps.html +8 -0
  52. reachy_mini/daemon/app/dashboard/templates/sections/appstore.html +29 -0
  53. reachy_mini/daemon/app/dashboard/templates/sections/daemon.html +36 -0
  54. reachy_mini/daemon/app/dashboard/templates/sections/move_player.html +27 -0
  55. reachy_mini/daemon/app/dashboard/update.html +22 -0
  56. reachy_mini/daemon/app/dashboard/wifi_config.html +41 -0
  57. reachy_mini/daemon/app/dependencies.py +41 -0
  58. reachy_mini/daemon/app/main.py +262 -0
  59. reachy_mini/daemon/app/models.py +147 -0
  60. reachy_mini/daemon/app/routers/apps.py +114 -0
  61. reachy_mini/daemon/app/routers/daemon.py +80 -0
  62. reachy_mini/daemon/app/routers/kinematics.py +57 -0
  63. reachy_mini/daemon/app/routers/motors.py +41 -0
  64. reachy_mini/daemon/app/routers/move.py +257 -0
  65. reachy_mini/daemon/app/routers/state.py +146 -0
  66. reachy_mini/daemon/backend/__init__.py +1 -0
  67. reachy_mini/daemon/backend/abstract.py +750 -0
  68. reachy_mini/daemon/backend/mujoco/__init__.py +36 -0
  69. reachy_mini/daemon/backend/mujoco/backend.py +304 -0
  70. reachy_mini/daemon/backend/mujoco/utils.py +59 -0
  71. reachy_mini/daemon/backend/mujoco/video_udp.py +53 -0
  72. reachy_mini/daemon/backend/robot/__init__.py +8 -0
  73. reachy_mini/daemon/backend/robot/backend.py +535 -0
  74. reachy_mini/daemon/daemon.py +444 -0
  75. reachy_mini/daemon/utils.py +114 -0
  76. reachy_mini/descriptions/reachy_mini/mjcf/additional.xml +6 -0
  77. reachy_mini/descriptions/reachy_mini/mjcf/assets/5w_speaker.part +13 -0
  78. reachy_mini/descriptions/reachy_mini/mjcf/assets/5w_speaker.stl +0 -0
  79. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna.part +13 -0
  80. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna.stl +0 -0
  81. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_body_3dprint.part +13 -0
  82. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_body_3dprint.stl +0 -0
  83. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_holder_l_3dprint.part +13 -0
  84. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_holder_l_3dprint.stl +0 -0
  85. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_holder_r_3dprint.part +13 -0
  86. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_holder_r_3dprint.stl +0 -0
  87. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_interface_3dprint.part +13 -0
  88. reachy_mini/descriptions/reachy_mini/mjcf/assets/antenna_interface_3dprint.stl +0 -0
  89. reachy_mini/descriptions/reachy_mini/mjcf/assets/arducam.part +13 -0
  90. reachy_mini/descriptions/reachy_mini/mjcf/assets/arducam.stl +0 -0
  91. reachy_mini/descriptions/reachy_mini/mjcf/assets/b3b_eh.part +13 -0
  92. reachy_mini/descriptions/reachy_mini/mjcf/assets/b3b_eh.stl +0 -0
  93. reachy_mini/descriptions/reachy_mini/mjcf/assets/b3b_eh_1.part +13 -0
  94. reachy_mini/descriptions/reachy_mini/mjcf/assets/b3b_eh_1.stl +0 -0
  95. reachy_mini/descriptions/reachy_mini/mjcf/assets/bearing_85x110x13.part +13 -0
  96. reachy_mini/descriptions/reachy_mini/mjcf/assets/bearing_85x110x13.stl +0 -0
  97. reachy_mini/descriptions/reachy_mini/mjcf/assets/big_lens_d40.part +13 -0
  98. reachy_mini/descriptions/reachy_mini/mjcf/assets/big_lens_d40.stl +0 -0
  99. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_down_3dprint.part +13 -0
  100. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_down_3dprint.stl +0 -0
  101. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_foot_3dprint.part +13 -0
  102. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_foot_3dprint.stl +0 -0
  103. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_top_3dprint.part +13 -0
  104. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_top_3dprint.stl +0 -0
  105. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_turning_3dprint.part +13 -0
  106. reachy_mini/descriptions/reachy_mini/mjcf/assets/body_turning_3dprint.stl +0 -0
  107. reachy_mini/descriptions/reachy_mini/mjcf/assets/bts2_m2_6x8.part +13 -0
  108. reachy_mini/descriptions/reachy_mini/mjcf/assets/bts2_m2_6x8.stl +0 -0
  109. reachy_mini/descriptions/reachy_mini/mjcf/assets/croissant_1k.blend/croissant_1k.obj +5021 -0
  110. reachy_mini/descriptions/reachy_mini/mjcf/assets/croissant_1k.blend/textures/croissant_diff_1k.png +0 -0
  111. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_b_dummy.part +13 -0
  112. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_b_dummy.stl +0 -0
  113. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_f_dummy.part +13 -0
  114. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_f_dummy.stl +0 -0
  115. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_m_dummy.part +13 -0
  116. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_case_m_dummy.stl +0 -0
  117. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_horn_dummy.part +13 -0
  118. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_horn_dummy.stl +0 -0
  119. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_led_cap2_dummy.part +13 -0
  120. reachy_mini/descriptions/reachy_mini/mjcf/assets/dc15_a01_led_cap2_dummy.stl +0 -0
  121. reachy_mini/descriptions/reachy_mini/mjcf/assets/food_apple_01_1k.blend/food_apple_01_1k.obj +18045 -0
  122. reachy_mini/descriptions/reachy_mini/mjcf/assets/food_apple_01_1k.blend/textures/food_apple_01_diff_1k.png +0 -0
  123. reachy_mini/descriptions/reachy_mini/mjcf/assets/glasses_dolder_3dprint.part +13 -0
  124. reachy_mini/descriptions/reachy_mini/mjcf/assets/glasses_dolder_3dprint.stl +0 -0
  125. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_back_3dprint.part +13 -0
  126. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_back_3dprint.stl +0 -0
  127. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_front_3dprint.part +13 -0
  128. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_front_3dprint.stl +0 -0
  129. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_mic_3dprint.part +13 -0
  130. reachy_mini/descriptions/reachy_mini/mjcf/assets/head_mic_3dprint.stl +0 -0
  131. reachy_mini/descriptions/reachy_mini/mjcf/assets/lens_cap_d30_3dprint.part +13 -0
  132. reachy_mini/descriptions/reachy_mini/mjcf/assets/lens_cap_d30_3dprint.stl +0 -0
  133. reachy_mini/descriptions/reachy_mini/mjcf/assets/lens_cap_d40_3dprint.part +13 -0
  134. reachy_mini/descriptions/reachy_mini/mjcf/assets/lens_cap_d40_3dprint.stl +0 -0
  135. reachy_mini/descriptions/reachy_mini/mjcf/assets/m12_fisheye_lens_1_8mm.part +13 -0
  136. reachy_mini/descriptions/reachy_mini/mjcf/assets/m12_fisheye_lens_1_8mm.stl +0 -0
  137. reachy_mini/descriptions/reachy_mini/mjcf/assets/mp01062_stewart_arm_3.part +13 -0
  138. reachy_mini/descriptions/reachy_mini/mjcf/assets/mp01062_stewart_arm_3.stl +0 -0
  139. reachy_mini/descriptions/reachy_mini/mjcf/assets/neck_reference_3dprint.part +13 -0
  140. reachy_mini/descriptions/reachy_mini/mjcf/assets/neck_reference_3dprint.stl +0 -0
  141. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10.part +13 -0
  142. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10.stl +0 -0
  143. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_1.part +13 -0
  144. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_1.stl +0 -0
  145. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_2.part +13 -0
  146. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_2.stl +0 -0
  147. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_3.part +13 -0
  148. reachy_mini/descriptions/reachy_mini/mjcf/assets/phs_1_7x20_5_dc10_3.stl +0 -0
  149. reachy_mini/descriptions/reachy_mini/mjcf/assets/pp01102_arducam_carter.part +13 -0
  150. reachy_mini/descriptions/reachy_mini/mjcf/assets/pp01102_arducam_carter.stl +0 -0
  151. reachy_mini/descriptions/reachy_mini/mjcf/assets/rubber_duck_toy_1k.blend/rubber_duck_toy_1k.obj +8940 -0
  152. reachy_mini/descriptions/reachy_mini/mjcf/assets/rubber_duck_toy_1k.blend/textures/rubber_duck_toy_diff_1k.png +0 -0
  153. reachy_mini/descriptions/reachy_mini/mjcf/assets/small_lens_d30.part +13 -0
  154. reachy_mini/descriptions/reachy_mini/mjcf/assets/small_lens_d30.stl +0 -0
  155. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_ball.part +13 -0
  156. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_ball.stl +0 -0
  157. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_ball__2.part +13 -0
  158. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_ball__2.stl +0 -0
  159. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_rod.part +13 -0
  160. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_link_rod.stl +0 -0
  161. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_main_plate_3dprint.part +13 -0
  162. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_main_plate_3dprint.stl +0 -0
  163. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_tricap_3dprint.part +13 -0
  164. reachy_mini/descriptions/reachy_mini/mjcf/assets/stewart_tricap_3dprint.stl +0 -0
  165. reachy_mini/descriptions/reachy_mini/mjcf/assets/wooden_table_02_1k.blend/textures/wooden_table_02_diff_1k.png +0 -0
  166. reachy_mini/descriptions/reachy_mini/mjcf/assets/wooden_table_02_1k.blend/wooden_table_02_1k.obj +485 -0
  167. reachy_mini/descriptions/reachy_mini/mjcf/config.json +21 -0
  168. reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml +29 -0
  169. reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml +613 -0
  170. reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml.bak +442 -0
  171. reachy_mini/descriptions/reachy_mini/mjcf/scene.xml +24 -0
  172. reachy_mini/descriptions/reachy_mini/mjcf/scenes/empty.xml +27 -0
  173. reachy_mini/descriptions/reachy_mini/mjcf/scenes/minimal.xml +131 -0
  174. reachy_mini/descriptions/reachy_mini/urdf/assets/5w_speaker.part +13 -0
  175. reachy_mini/descriptions/reachy_mini/urdf/assets/5w_speaker.stl +0 -0
  176. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna.part +13 -0
  177. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna.stl +0 -0
  178. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_body_3dprint.part +13 -0
  179. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_body_3dprint.stl +0 -0
  180. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_holder_l_3dprint.part +13 -0
  181. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_holder_l_3dprint.stl +0 -0
  182. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_holder_r_3dprint.part +13 -0
  183. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_holder_r_3dprint.stl +0 -0
  184. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_interface_3dprint.part +13 -0
  185. reachy_mini/descriptions/reachy_mini/urdf/assets/antenna_interface_3dprint.stl +0 -0
  186. reachy_mini/descriptions/reachy_mini/urdf/assets/arducam.part +13 -0
  187. reachy_mini/descriptions/reachy_mini/urdf/assets/arducam.stl +0 -0
  188. reachy_mini/descriptions/reachy_mini/urdf/assets/arm.part +13 -0
  189. reachy_mini/descriptions/reachy_mini/urdf/assets/arm.stl +0 -0
  190. reachy_mini/descriptions/reachy_mini/urdf/assets/b3b_eh.part +13 -0
  191. reachy_mini/descriptions/reachy_mini/urdf/assets/b3b_eh.stl +0 -0
  192. reachy_mini/descriptions/reachy_mini/urdf/assets/b3b_eh_1.part +13 -0
  193. reachy_mini/descriptions/reachy_mini/urdf/assets/b3b_eh_1.stl +0 -0
  194. reachy_mini/descriptions/reachy_mini/urdf/assets/ball.part +13 -0
  195. reachy_mini/descriptions/reachy_mini/urdf/assets/ball.stl +0 -0
  196. reachy_mini/descriptions/reachy_mini/urdf/assets/bearing_85x110x13.part +13 -0
  197. reachy_mini/descriptions/reachy_mini/urdf/assets/bearing_85x110x13.stl +0 -0
  198. reachy_mini/descriptions/reachy_mini/urdf/assets/big_lens.part +13 -0
  199. reachy_mini/descriptions/reachy_mini/urdf/assets/big_lens.stl +0 -0
  200. reachy_mini/descriptions/reachy_mini/urdf/assets/big_lens_d40.part +13 -0
  201. reachy_mini/descriptions/reachy_mini/urdf/assets/big_lens_d40.stl +0 -0
  202. reachy_mini/descriptions/reachy_mini/urdf/assets/body_down_3dprint.part +13 -0
  203. reachy_mini/descriptions/reachy_mini/urdf/assets/body_down_3dprint.stl +0 -0
  204. reachy_mini/descriptions/reachy_mini/urdf/assets/body_foot_3dprint.part +13 -0
  205. reachy_mini/descriptions/reachy_mini/urdf/assets/body_foot_3dprint.stl +0 -0
  206. reachy_mini/descriptions/reachy_mini/urdf/assets/body_top_3dprint.part +13 -0
  207. reachy_mini/descriptions/reachy_mini/urdf/assets/body_top_3dprint.stl +0 -0
  208. reachy_mini/descriptions/reachy_mini/urdf/assets/body_turning_3dprint.part +13 -0
  209. reachy_mini/descriptions/reachy_mini/urdf/assets/body_turning_3dprint.stl +0 -0
  210. reachy_mini/descriptions/reachy_mini/urdf/assets/bottom_body.part +13 -0
  211. reachy_mini/descriptions/reachy_mini/urdf/assets/bottom_body.stl +0 -0
  212. reachy_mini/descriptions/reachy_mini/urdf/assets/bts2_m2_6x8.part +13 -0
  213. reachy_mini/descriptions/reachy_mini/urdf/assets/bts2_m2_6x8.stl +0 -0
  214. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_b_dummy.part +13 -0
  215. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_b_dummy.stl +0 -0
  216. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_f_dummy.part +13 -0
  217. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_f_dummy.stl +0 -0
  218. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_m_dummy.part +13 -0
  219. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_case_m_dummy.stl +0 -0
  220. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_horn_dummy.part +13 -0
  221. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_horn_dummy.stl +0 -0
  222. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_led_cap2_dummy.part +13 -0
  223. reachy_mini/descriptions/reachy_mini/urdf/assets/dc15_a01_led_cap2_dummy.stl +0 -0
  224. reachy_mini/descriptions/reachy_mini/urdf/assets/drive_palonier__configuration_default.part +14 -0
  225. reachy_mini/descriptions/reachy_mini/urdf/assets/drive_palonier__configuration_default.stl +0 -0
  226. reachy_mini/descriptions/reachy_mini/urdf/assets/drive_palonier__configuration_simple_axe.part +14 -0
  227. reachy_mini/descriptions/reachy_mini/urdf/assets/drive_palonier__configuration_simple_axe.stl +0 -0
  228. reachy_mini/descriptions/reachy_mini/urdf/assets/eye_support.part +13 -0
  229. reachy_mini/descriptions/reachy_mini/urdf/assets/eye_support.stl +0 -0
  230. reachy_mini/descriptions/reachy_mini/urdf/assets/foot.part +13 -0
  231. reachy_mini/descriptions/reachy_mini/urdf/assets/foot.stl +0 -0
  232. reachy_mini/descriptions/reachy_mini/urdf/assets/glasses_dolder_3dprint.part +13 -0
  233. reachy_mini/descriptions/reachy_mini/urdf/assets/glasses_dolder_3dprint.stl +0 -0
  234. reachy_mini/descriptions/reachy_mini/urdf/assets/head_back_3dprint.part +13 -0
  235. reachy_mini/descriptions/reachy_mini/urdf/assets/head_back_3dprint.stl +0 -0
  236. reachy_mini/descriptions/reachy_mini/urdf/assets/head_front_3dprint.part +13 -0
  237. reachy_mini/descriptions/reachy_mini/urdf/assets/head_front_3dprint.stl +0 -0
  238. reachy_mini/descriptions/reachy_mini/urdf/assets/head_head_back.part +13 -0
  239. reachy_mini/descriptions/reachy_mini/urdf/assets/head_head_back.stl +0 -0
  240. reachy_mini/descriptions/reachy_mini/urdf/assets/head_interface.part +13 -0
  241. reachy_mini/descriptions/reachy_mini/urdf/assets/head_interface.stl +0 -0
  242. reachy_mini/descriptions/reachy_mini/urdf/assets/head_mic_3dprint.part +13 -0
  243. reachy_mini/descriptions/reachy_mini/urdf/assets/head_mic_3dprint.stl +0 -0
  244. reachy_mini/descriptions/reachy_mini/urdf/assets/head_shell_front.part +13 -0
  245. reachy_mini/descriptions/reachy_mini/urdf/assets/head_shell_front.stl +0 -0
  246. reachy_mini/descriptions/reachy_mini/urdf/assets/lens_cap_d30_3dprint.part +13 -0
  247. reachy_mini/descriptions/reachy_mini/urdf/assets/lens_cap_d30_3dprint.stl +0 -0
  248. reachy_mini/descriptions/reachy_mini/urdf/assets/lens_cap_d40_3dprint.part +13 -0
  249. reachy_mini/descriptions/reachy_mini/urdf/assets/lens_cap_d40_3dprint.stl +0 -0
  250. reachy_mini/descriptions/reachy_mini/urdf/assets/m12_fisheye_lens_1_8mm.part +13 -0
  251. reachy_mini/descriptions/reachy_mini/urdf/assets/m12_fisheye_lens_1_8mm.stl +0 -0
  252. reachy_mini/descriptions/reachy_mini/urdf/assets/m12_lens.part +13 -0
  253. reachy_mini/descriptions/reachy_mini/urdf/assets/m12_lens.stl +0 -0
  254. reachy_mini/descriptions/reachy_mini/urdf/assets/main_plate.part +13 -0
  255. reachy_mini/descriptions/reachy_mini/urdf/assets/main_plate.stl +0 -0
  256. reachy_mini/descriptions/reachy_mini/urdf/assets/mid_plate.part +13 -0
  257. reachy_mini/descriptions/reachy_mini/urdf/assets/mid_plate.stl +0 -0
  258. reachy_mini/descriptions/reachy_mini/urdf/assets/mp01062_stewart_arm_3.part +13 -0
  259. reachy_mini/descriptions/reachy_mini/urdf/assets/mp01062_stewart_arm_3.stl +0 -0
  260. reachy_mini/descriptions/reachy_mini/urdf/assets/neck_reference_3dprint.part +13 -0
  261. reachy_mini/descriptions/reachy_mini/urdf/assets/neck_reference_3dprint.stl +0 -0
  262. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10.part +13 -0
  263. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10.stl +0 -0
  264. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_1.part +13 -0
  265. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_1.stl +0 -0
  266. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_2.part +13 -0
  267. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_2.stl +0 -0
  268. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_3.part +13 -0
  269. reachy_mini/descriptions/reachy_mini/urdf/assets/phs_1_7x20_5_dc10_3.stl +0 -0
  270. reachy_mini/descriptions/reachy_mini/urdf/assets/plateform.part +13 -0
  271. reachy_mini/descriptions/reachy_mini/urdf/assets/plateform.stl +0 -0
  272. reachy_mini/descriptions/reachy_mini/urdf/assets/pp00xxx_stewart_rod.part +13 -0
  273. reachy_mini/descriptions/reachy_mini/urdf/assets/pp00xxx_stewart_rod.stl +0 -0
  274. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01062_stewart_arm.part +13 -0
  275. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01062_stewart_arm.stl +0 -0
  276. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01063_stewart_plateform.part +13 -0
  277. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01063_stewart_plateform.stl +0 -0
  278. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01064_stewart_main_plate.part +13 -0
  279. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01064_stewart_main_plate.stl +0 -0
  280. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01065_stewart_side_plate.part +13 -0
  281. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01065_stewart_side_plate.stl +0 -0
  282. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01066_stewart_mid_plate.part +13 -0
  283. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01066_stewart_mid_plate.stl +0 -0
  284. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01067_bottom_body.part +13 -0
  285. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01067_bottom_body.stl +0 -0
  286. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01068_top_body.part +13 -0
  287. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01068_top_body.stl +0 -0
  288. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01069_head_shell_front.part +13 -0
  289. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01069_head_shell_front.stl +0 -0
  290. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01070_head_head_back.part +13 -0
  291. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01070_head_head_back.stl +0 -0
  292. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01071_turning_bowl.part +13 -0
  293. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01071_turning_bowl.stl +0 -0
  294. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01072_turning_end.part +13 -0
  295. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01072_turning_end.stl +0 -0
  296. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01078_glasses.part +13 -0
  297. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01078_glasses.stl +0 -0
  298. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01079_back_big_eye.part +13 -0
  299. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01079_back_big_eye.stl +0 -0
  300. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01080_back_small_eye.part +13 -0
  301. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01080_back_small_eye.stl +0 -0
  302. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01102_arducam_carter.part +13 -0
  303. reachy_mini/descriptions/reachy_mini/urdf/assets/pp01102_arducam_carter.stl +0 -0
  304. reachy_mini/descriptions/reachy_mini/urdf/assets/rod.part +13 -0
  305. reachy_mini/descriptions/reachy_mini/urdf/assets/rod.stl +0 -0
  306. reachy_mini/descriptions/reachy_mini/urdf/assets/shape.part +13 -0
  307. reachy_mini/descriptions/reachy_mini/urdf/assets/shape.stl +0 -0
  308. reachy_mini/descriptions/reachy_mini/urdf/assets/side_plate.part +13 -0
  309. reachy_mini/descriptions/reachy_mini/urdf/assets/side_plate.stl +0 -0
  310. reachy_mini/descriptions/reachy_mini/urdf/assets/small_lens.part +13 -0
  311. reachy_mini/descriptions/reachy_mini/urdf/assets/small_lens.stl +0 -0
  312. reachy_mini/descriptions/reachy_mini/urdf/assets/small_lens_d30.part +13 -0
  313. reachy_mini/descriptions/reachy_mini/urdf/assets/small_lens_d30.stl +0 -0
  314. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_ball.part +13 -0
  315. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_ball.stl +0 -0
  316. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_ball__2.part +13 -0
  317. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_ball__2.stl +0 -0
  318. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_rod.part +13 -0
  319. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_link_rod.stl +0 -0
  320. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_main_plate_3dprint.part +13 -0
  321. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_main_plate_3dprint.stl +0 -0
  322. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_tricap_3dprint.part +13 -0
  323. reachy_mini/descriptions/reachy_mini/urdf/assets/stewart_tricap_3dprint.stl +0 -0
  324. reachy_mini/descriptions/reachy_mini/urdf/assets/test_antenna.part +13 -0
  325. reachy_mini/descriptions/reachy_mini/urdf/assets/test_antenna.stl +0 -0
  326. reachy_mini/descriptions/reachy_mini/urdf/assets/test_antenna_body.part +13 -0
  327. reachy_mini/descriptions/reachy_mini/urdf/assets/test_antenna_body.stl +0 -0
  328. reachy_mini/descriptions/reachy_mini/urdf/assets/top_body.part +13 -0
  329. reachy_mini/descriptions/reachy_mini/urdf/assets/top_body.stl +0 -0
  330. reachy_mini/descriptions/reachy_mini/urdf/assets/turning_bowl.part +13 -0
  331. reachy_mini/descriptions/reachy_mini/urdf/assets/turning_bowl.stl +0 -0
  332. reachy_mini/descriptions/reachy_mini/urdf/assets/uc_a37_rev_a_step.part +13 -0
  333. reachy_mini/descriptions/reachy_mini/urdf/assets/uc_a37_rev_a_step.stl +0 -0
  334. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0122topcabinetcase_95__configuration_default.part +14 -0
  335. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0122topcabinetcase_95__configuration_default.stl +0 -0
  336. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0122topcabinetcase_95__configuration_simple_axe.part +14 -0
  337. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0122topcabinetcase_95__configuration_simple_axe.stl +0 -0
  338. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0123middlecase_56__configuration_default.part +14 -0
  339. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0123middlecase_56__configuration_default.stl +0 -0
  340. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0123middlecase_56__configuration_simple_axe.part +14 -0
  341. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0123middlecase_56__configuration_simple_axe.stl +0 -0
  342. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0124bottomcase_45__configuration_default.part +14 -0
  343. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0124bottomcase_45__configuration_default.stl +0 -0
  344. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0124bottomcase_45__configuration_simple_axe.part +14 -0
  345. reachy_mini/descriptions/reachy_mini/urdf/assets/wj_wk00_0124bottomcase_45__configuration_simple_axe.stl +0 -0
  346. reachy_mini/descriptions/reachy_mini/urdf/config.json +18 -0
  347. reachy_mini/descriptions/reachy_mini/urdf/robot.urdf +3281 -0
  348. reachy_mini/descriptions/reachy_mini/urdf/robot.urdf.bak +3282 -0
  349. reachy_mini/descriptions/reachy_mini/urdf/robot_no_collision.urdf +2316 -0
  350. reachy_mini/io/__init__.py +1 -0
  351. reachy_mini/io/abstract.py +70 -0
  352. reachy_mini/io/protocol.py +44 -0
  353. reachy_mini/io/zenoh_client.py +258 -0
  354. reachy_mini/io/zenoh_server.py +183 -0
  355. reachy_mini/kinematics/__init__.py +68 -0
  356. reachy_mini/kinematics/analytical_kinematics.py +102 -0
  357. reachy_mini/kinematics/nn_kinematics.py +100 -0
  358. reachy_mini/kinematics/placo_kinematics.py +666 -0
  359. reachy_mini/media/__init__.py +1 -0
  360. reachy_mini/media/audio_base.py +163 -0
  361. reachy_mini/media/audio_gstreamer.py +195 -0
  362. reachy_mini/media/audio_sounddevice.py +226 -0
  363. reachy_mini/media/audio_utils.py +27 -0
  364. reachy_mini/media/camera_base.py +63 -0
  365. reachy_mini/media/camera_constants.py +13 -0
  366. reachy_mini/media/camera_gstreamer.py +162 -0
  367. reachy_mini/media/camera_opencv.py +61 -0
  368. reachy_mini/media/camera_utils.py +60 -0
  369. reachy_mini/media/media_manager.py +194 -0
  370. reachy_mini/motion/__init__.py +4 -0
  371. reachy_mini/motion/goto.py +71 -0
  372. reachy_mini/motion/move.py +36 -0
  373. reachy_mini/motion/recorded_move.py +132 -0
  374. reachy_mini/reachy_mini.py +705 -0
  375. reachy_mini/utils/__init__.py +46 -0
  376. reachy_mini/utils/constants.py +9 -0
  377. reachy_mini/utils/interpolation.py +227 -0
  378. reachy_mini/utils/parse_urdf_for_kinematics.py +110 -0
  379. reachy_mini/utils/rerun.py +546 -0
  380. reachy_mini-1.0.0.dist-info/METADATA +286 -0
  381. reachy_mini-1.0.0.dist-info/RECORD +385 -0
  382. reachy_mini-1.0.0.dist-info/WHEEL +5 -0
  383. reachy_mini-1.0.0.dist-info/entry_points.txt +3 -0
  384. reachy_mini-1.0.0.dist-info/licenses/LICENSE +201 -0
  385. reachy_mini-1.0.0.dist-info/top_level.txt +1 -0
@@ -0,0 +1,666 @@
1
+ """Placo Kinematics for Reachy Mini.
2
+
3
+ This module provides the PlacoKinematics class for performing inverse and forward kinematics based on the Reachy Mini robot URDF using the Placo library.
4
+ """
5
+
6
+ import logging
7
+ from typing import Annotated, List, Optional
8
+
9
+ import numpy as np
10
+ import numpy.typing as npt
11
+ import pinocchio as pin
12
+ import placo
13
+
14
+
15
+ class PlacoKinematics:
16
+ """Placo Kinematics class for Reachy Mini.
17
+
18
+ This class provides methods for inverse and forward kinematics using the Placo library and a URDF model of the Reachy Mini robot.
19
+ """
20
+
21
+ def __init__(
22
+ self,
23
+ urdf_path: str,
24
+ dt: float = 0.02,
25
+ automatic_body_yaw: bool = False,
26
+ check_collision: bool = False,
27
+ log_level: str = "INFO",
28
+ ) -> None:
29
+ """Initialize the PlacoKinematics class.
30
+
31
+ Args:
32
+ urdf_path (str): Path to the URDF file of the Reachy Mini robot.
33
+ dt (float): Time step for the kinematics solver. Default is 0.02 seconds.
34
+ automatic_body_yaw (bool): If True, the body yaw will be used to compute the IK and FK. Default is False.
35
+ check_collision (bool): If True, checks for collisions after solving IK. (default: False)
36
+ log_level (str): Logging level for the kinematics computations.
37
+
38
+ """
39
+ self.fk_reached_tol = np.deg2rad(
40
+ 0.1
41
+ ) # 0.1 degrees tolerance for the FK reached condition
42
+
43
+ if not urdf_path.endswith(".urdf"):
44
+ urdf_path = f"{urdf_path}/{'robot.urdf' if check_collision else 'robot_no_collision.urdf'}"
45
+
46
+ self.robot = placo.RobotWrapper(
47
+ urdf_path, placo.Flags.ignore_collisions + placo.Flags.collision_as_visual
48
+ )
49
+ self.robot_ik = placo.RobotWrapper(
50
+ urdf_path, placo.Flags.ignore_collisions + placo.Flags.collision_as_visual
51
+ )
52
+
53
+ self.ik_solver = placo.KinematicsSolver(self.robot_ik)
54
+ self.ik_solver.mask_fbase(True)
55
+
56
+ self.fk_solver = placo.KinematicsSolver(self.robot)
57
+ self.fk_solver.mask_fbase(True)
58
+
59
+ self.automatic_body_yaw = automatic_body_yaw
60
+ self.check_collision = check_collision
61
+
62
+ self._logger = logging.getLogger(__name__)
63
+ self._logger.setLevel(log_level)
64
+
65
+ # we could go to soft limits to avoid over-constraining the IK
66
+ # but the current implementation works robustly with hard limits
67
+ # so we keep the hard limits for now
68
+ constrant_type = "hard" # "hard" or "soft"
69
+
70
+ # IK closing tasks
71
+ ik_closing_tasks = []
72
+ for i in range(1, 6):
73
+ ik_closing_task = self.ik_solver.add_relative_position_task(
74
+ f"closing_{i}_1", f"closing_{i}_2", np.zeros(3)
75
+ )
76
+ ik_closing_task.configure(f"closing_{i}", constrant_type, 1.0)
77
+ ik_closing_tasks.append(ik_closing_task)
78
+
79
+ # FK closing tasks
80
+ fk_closing_tasks = []
81
+ for i in range(1, 6):
82
+ fk_closing_task = self.fk_solver.add_relative_position_task(
83
+ f"closing_{i}_1", f"closing_{i}_2", np.zeros(3)
84
+ )
85
+ fk_closing_task.configure(f"closing_{i}", constrant_type, 1.0)
86
+ fk_closing_tasks.append(fk_closing_task)
87
+
88
+ # Add the constraint between the rotated torso and the head
89
+ # This will allow independent control of the torso and the head yaw
90
+ # until this constraint is reached
91
+ yaw_constraint = self.ik_solver.add_yaw_constraint(
92
+ "dummy_torso_yaw", "head", np.deg2rad(65.0)
93
+ )
94
+ yaw_constraint.configure("rel_yaw", "hard")
95
+
96
+ # Add the constraint to avoid the head from looking too far behind
97
+ # Mostly due to some numerical problems 180 is always a bit tricky
98
+ # Not really constraining because the this 180 pose is almost not
99
+ # reachable with the real robot anyway
100
+ yaw_constraint_abs = self.ik_solver.add_yaw_constraint(
101
+ "body_foot_3dprint", "head", np.deg2rad(179.0)
102
+ )
103
+ yaw_constraint_abs.configure("abs_yaw", "hard")
104
+
105
+ # Add a cone constraint for the head to not exceed a certain angle
106
+ # This is to avoid the head from looking too far up or down
107
+ self.fk_cone = self.ik_solver.add_cone_constraint(
108
+ "body_foot_3dprint", "head", np.deg2rad(40.0)
109
+ )
110
+ self.fk_cone.configure("cone", "hard")
111
+ self.fk_yaw_constraint = self.fk_solver.add_yaw_constraint(
112
+ "dummy_torso_yaw", "head", np.deg2rad(65.0)
113
+ )
114
+ self.fk_yaw_constraint.configure("rel_yaw", "hard")
115
+
116
+ # Add a cone constraint for the head to not exceed a certain angle
117
+ # This is to avoid the head from looking too far up or down
118
+ fk_cone = self.fk_solver.add_cone_constraint(
119
+ "body_foot_3dprint", "head", np.deg2rad(40.0)
120
+ )
121
+ fk_cone.configure("cone", "hard")
122
+
123
+ # Z offset for the head to make it easier to compute the IK and FK
124
+ # This is the height of the head from the base of the robot
125
+ self.head_z_offset = 0.177 # offset for the head height
126
+
127
+ # IK head task
128
+ self.head_starting_pose = np.eye(4)
129
+ self.head_starting_pose[:3, 3][2] = self.head_z_offset
130
+ self.head_frame = self.ik_solver.add_frame_task("head", self.head_starting_pose)
131
+ # equivalance to ~1cm = 1deg weights
132
+ # set to 5 to be higher than the 1.0 for the body yaw
133
+ self.head_frame.configure(
134
+ "head",
135
+ "soft",
136
+ 5.0, # in meters # 1m
137
+ 5.0, # in radians # 1rad
138
+ )
139
+
140
+ self.head_frame.T_world_frame = self.head_starting_pose
141
+
142
+ # regularization
143
+ self.ik_yaw_joint_task = self.ik_solver.add_joints_task()
144
+ self.ik_yaw_joint_task.set_joints({"yaw_body": 0})
145
+ if self.automatic_body_yaw:
146
+ self.ik_yaw_joint_task.configure("joints", "soft", 5e-5)
147
+ else:
148
+ self.ik_yaw_joint_task.configure("joints", "soft", 1.0)
149
+
150
+ # joint limit tasks (values form URDF)
151
+ self.ik_solver.enable_velocity_limits(True)
152
+ self.ik_solver.enable_joint_limits(True)
153
+ self.ik_solver.dt = dt
154
+
155
+ # FK joint task
156
+ self.head_joints_task = self.fk_solver.add_joints_task()
157
+ self.head_joints_task.configure("joints", "soft", 5.0)
158
+ # joint limit tasks (values form URDF)
159
+ self.fk_solver.enable_velocity_limits(True)
160
+ self.fk_solver.enable_joint_limits(True)
161
+ self.fk_solver.dt = dt
162
+
163
+ # Actuated DoFs
164
+ self.joints_names = [
165
+ "yaw_body",
166
+ "stewart_1",
167
+ "stewart_2",
168
+ "stewart_3",
169
+ "stewart_4",
170
+ "stewart_5",
171
+ "stewart_6",
172
+ ]
173
+
174
+ # Passive DoFs to eliminate with constraint jacobian
175
+ self.passive_joints_names = [
176
+ "passive_1_x",
177
+ "passive_1_y",
178
+ "passive_2_x",
179
+ "passive_2_y",
180
+ "passive_3_x",
181
+ "passive_3_y",
182
+ "passive_4_x",
183
+ "passive_4_y",
184
+ "passive_5_x",
185
+ "passive_5_y",
186
+ "passive_6_x",
187
+ "passive_6_y",
188
+ "passive_7_x",
189
+ "passive_7_y",
190
+ "passive_7_z",
191
+ ]
192
+
193
+ # Retrieving indexes in the jacobian
194
+ self.passives_idx = [
195
+ self.robot.get_joint_v_offset(dof) for dof in self.passive_joints_names
196
+ ]
197
+ self.actives_idx = [
198
+ self.robot.get_joint_v_offset(dof)
199
+ for dof in self.robot.joint_names()
200
+ if dof not in self.passive_joints_names
201
+ ]
202
+ self.actuated_idx = [
203
+ self.robot.get_joint_v_offset(dof)
204
+ for dof in self.robot.joint_names()
205
+ if dof in self.joints_names
206
+ ]
207
+
208
+ # actuated dof indexes in active dofs
209
+ self.actuated_idx_in_active = [
210
+ i for i, idx in enumerate(self.actives_idx) if idx in self.actuated_idx
211
+ ]
212
+
213
+ # set velocity limits to be artificially high
214
+ # to enable faster convergence of the IK/FK solver
215
+ max_vel = 13.0 # rad/s
216
+ for joint_name in self.joints_names:
217
+ if joint_name != "yaw_body":
218
+ self.robot.set_velocity_limit(joint_name, max_vel)
219
+ self.robot_ik.set_velocity_limit(joint_name, max_vel)
220
+
221
+ self.robot.set_joint_limits("yaw_body", -2.8, 2.8)
222
+ self.robot_ik.set_joint_limits("yaw_body", -2.8, 2.8)
223
+
224
+ # initial state
225
+ self._inital_q = self.robot.state.q.copy()
226
+ self._inital_qd = np.zeros_like(self.robot.state.qd)
227
+ self._inital_qdd = np.zeros_like(self.robot.state.qdd)
228
+
229
+ # initial FK to set the head pose
230
+ for _ in range(10):
231
+ self.ik_solver.solve(True) # False to not update the kinematics
232
+ self.robot_ik.update_kinematics()
233
+
234
+ # last good q to revert to in case of collision
235
+ self._inital_q = self.robot_ik.state.q.copy()
236
+ self._last_good_q = self.robot_ik.state.q.copy()
237
+
238
+ # update the robot state to the initial state
239
+ self._update_state_to_initial(self.robot) # revert to the inital state
240
+ self.robot.update_kinematics()
241
+
242
+ if self.check_collision:
243
+ # setup the collision model
244
+ self.config_collision_model()
245
+
246
+ def _update_state_to_initial(self, robot: placo.RobotWrapper) -> None:
247
+ """Update the robot state to the initial state.
248
+
249
+ It does not call update_kinematics, so the robot state is not updated.
250
+
251
+ Args:
252
+ robot (placo.RobotWrapper): The robot wrapper instance to update.
253
+
254
+ """
255
+ robot.state.q = self._inital_q
256
+ robot.state.qd = self._inital_qd
257
+ robot.state.qdd = self._inital_qdd
258
+
259
+ def _pose_distance(
260
+ self, pose1: npt.NDArray[np.float64], pose2: npt.NDArray[np.float64]
261
+ ) -> tuple[float, float]:
262
+ """Compute the orientation distance between two poses.
263
+
264
+ Args:
265
+ pose1 (np.ndarray): The first pose (4x4 homogeneous transformation matrix).
266
+ pose2 (np.ndarray): The second pose (4x4 homogeneous transformation matrix).
267
+
268
+ Returns:
269
+ float: The Euler distance between the two poses.
270
+
271
+ """
272
+ euler1 = pin.rpy.matrixToRpy(pose1[:3, :3])
273
+ euler2 = pin.rpy.matrixToRpy(pose2[:3, :3])
274
+ p1 = pose1[:3, 3]
275
+ p2 = pose2[:3, 3]
276
+ return float(np.linalg.norm(euler1 - euler2)), float(np.linalg.norm(p1 - p2))
277
+
278
+ def _closed_loop_constraints_valid(
279
+ self, robot: placo.RobotWrapper, tol: float = 1e-2
280
+ ) -> bool:
281
+ """Check if all closed-loop constraints are satisfied.
282
+
283
+ Args:
284
+ robot (placo.RobotWrapper): The robot wrapper instance to check.
285
+ tol (float): The tolerance for checking constraints (default: 1e-2).
286
+
287
+ Returns:
288
+ bool: True if all constraints are satisfied, False otherwise.
289
+
290
+ """
291
+ for i in range(1, 6):
292
+ pos1 = robot.get_T_world_frame(f"closing_{i}_1")[:3, 3]
293
+ pos2 = robot.get_T_world_frame(f"closing_{i}_2")[:3, 3]
294
+ if not np.allclose(pos1, pos2, atol=tol):
295
+ return False
296
+ return True
297
+
298
+ def _get_joint_values(self, robot: placo.RobotWrapper) -> List[float]:
299
+ """Get the joint values from the robot state.
300
+
301
+ Args:
302
+ robot (placo.RobotWrapper): The robot wrapper instance to get joint values from.
303
+
304
+ Returns:
305
+ List[float]: A list of joint values.
306
+
307
+ """
308
+ joints = []
309
+ for joint_name in self.joints_names:
310
+ joint = robot.get_joint(joint_name)
311
+ joints.append(joint)
312
+ return joints
313
+
314
+ def ik(
315
+ self,
316
+ pose: npt.NDArray[np.float64],
317
+ body_yaw: float = 0.0,
318
+ no_iterations: int = 2,
319
+ ) -> Annotated[npt.NDArray[np.float64], (7,)] | None:
320
+ """Compute the inverse kinematics for the head for a given pose.
321
+
322
+ Args:
323
+ pose (np.ndarray): A 4x4 homogeneous transformation matrix
324
+ representing the desired position and orientation of the head.
325
+ body_yaw (float): Body yaw angle in radians.
326
+ no_iterations (int): Number of iterations to perform (default: 2). The higher the value, the more accurate the solution.
327
+
328
+ Returns:
329
+ List[float]: A list of joint angles for the head.
330
+
331
+ """
332
+ _pose = pose.copy()
333
+ # set the head pose
334
+ _pose[:3, 3][2] += self.head_z_offset # offset the height of the head
335
+ self.head_frame.T_world_frame = _pose
336
+ # update the body_yaw task
337
+ self.ik_yaw_joint_task.set_joints({"yaw_body": body_yaw})
338
+
339
+ # check the starting configuration
340
+ # if the poses are too far start from the initial configuration
341
+ _dist_o, _dist_p = self._pose_distance(
342
+ _pose, self.robot_ik.get_T_world_frame("head")
343
+ )
344
+ # if distance too small 0.1mm and 0.1 deg and the QP has converged (almost 0 velocity)
345
+ _dist_by = np.abs(body_yaw - self.robot_ik.get_joint("yaw_body"))
346
+ if (
347
+ _dist_p < 0.1e-4
348
+ and _dist_o < np.deg2rad(0.01)
349
+ and _dist_by < np.deg2rad(0.01)
350
+ and np.linalg.norm(self.robot_ik.state.qd) < 1e-4
351
+ ):
352
+ # no need to recalculate - return the current joint values
353
+ return np.array(
354
+ self._get_joint_values(self.robot_ik)
355
+ ) # no need to solve IK
356
+ if _dist_o >= np.pi:
357
+ # distance too big between the current and the target pose
358
+ # start the optim from zero position
359
+ #
360
+ # TO INVESTIGATE: Another way to do this would be not to start from 0 but
361
+ # to set the target pose not to the actual target but to some intermediate pose
362
+ self._update_state_to_initial(self.robot_ik)
363
+ self.robot_ik.update_kinematics()
364
+ self._logger.debug("IK: Poses too far, starting from initial configuration")
365
+
366
+ done = True
367
+ # do the inital ik
368
+ for i in range(no_iterations):
369
+ try:
370
+ self.ik_solver.solve(True) # False to not update the kinematics
371
+ except Exception as e:
372
+ self._logger.debug(f"IK solver failed: {e}, retrying...")
373
+ done = False
374
+ break
375
+ self.robot_ik.update_kinematics()
376
+
377
+ # if no problem in solving the IK check for constraint violation
378
+ if done and (not self._closed_loop_constraints_valid(self.robot_ik)):
379
+ self._logger.debug(
380
+ "IK: Not all equality constraints are satisfied in IK, retrying..."
381
+ )
382
+ done = False
383
+
384
+ # if there was an issue start from scratch
385
+ if not done:
386
+ # set the initial pose
387
+ self._update_state_to_initial(self.robot_ik)
388
+ self.robot_ik.update_kinematics()
389
+
390
+ no_iterations += 2 # add a few more iterations
391
+ # do the inital ik with 10 iterations
392
+ for i in range(no_iterations):
393
+ try:
394
+ self.ik_solver.solve(True) # False to not update the kinematics
395
+ except Exception as e:
396
+ self._logger.warning(f"IK solver failed: {e}, no solution found!")
397
+ return None
398
+ self.robot_ik.update_kinematics()
399
+
400
+ # verify that there is no collision
401
+ if self.check_collision and self.compute_collision():
402
+ self._logger.warning(
403
+ "IK: Collision detected, using the previous configuration..."
404
+ )
405
+ self._update_state_to_initial(self.robot_ik) # revert to the inital state
406
+ self.robot_ik.update_kinematics()
407
+ return None
408
+
409
+ # Get the joint angles
410
+ return np.array(self._get_joint_values(self.robot_ik))
411
+
412
+ def fk(
413
+ self,
414
+ joints_angles: Annotated[npt.NDArray[np.float64], (7,)],
415
+ no_iterations: int = 2,
416
+ ) -> Optional[npt.NDArray[np.float64]]:
417
+ """Compute the forward kinematics for the head given joint angles.
418
+
419
+ Args:
420
+ joints_angles (List[float]): A list of joint angles for the head.
421
+ no_iterations (int): The number of iterations to use for the FK solver. (default: 2), the higher the more accurate the result.
422
+
423
+ Returns:
424
+ np.ndarray: A 4x4 homogeneous transformation matrix
425
+
426
+ """
427
+ # check if we're already there
428
+ _current_joints = self._get_joint_values(self.robot)
429
+ # if the joint angles are the same (tol 1e-4) and teh QP has converged (max speed is 1e-4rad/s)
430
+ # no need to compute the FK
431
+ if (
432
+ np.linalg.norm(np.array(_current_joints) - np.array(joints_angles))
433
+ < self.fk_reached_tol
434
+ and self.robot.state.qd.max() < 1e-4
435
+ ):
436
+ # no need to compute FK
437
+ T_world_head: npt.NDArray[np.float64] = self.robot.get_T_world_frame("head")
438
+ T_world_head[:3, 3][2] -= (
439
+ self.head_z_offset
440
+ ) # offset the height of the head
441
+ return T_world_head
442
+
443
+ # update the main task
444
+ self.head_joints_task.set_joints(
445
+ {
446
+ "yaw_body": joints_angles[0],
447
+ "stewart_1": joints_angles[1],
448
+ "stewart_2": joints_angles[2],
449
+ "stewart_3": joints_angles[3],
450
+ "stewart_4": joints_angles[4],
451
+ "stewart_5": joints_angles[5],
452
+ "stewart_6": joints_angles[6],
453
+ }
454
+ )
455
+
456
+ # save the initial configuration
457
+ q = self.robot.state.q.copy()
458
+
459
+ done = True
460
+ # do the inital ik with 2 iterations
461
+ for i in range(no_iterations):
462
+ try:
463
+ self.fk_solver.solve(True) # False to not update the kinematics
464
+ except Exception as e:
465
+ self._logger.debug(f"FK solver failed: {e}, retrying...")
466
+ done = False
467
+ break
468
+ self.robot.update_kinematics()
469
+
470
+ if done and (not self._closed_loop_constraints_valid(self.robot)):
471
+ self._logger.debug(
472
+ "FK: Not all equality constraints are satisfied in FK, retrying..."
473
+ )
474
+ done = False
475
+
476
+ if not done:
477
+ self._update_state_to_initial(self.robot) # revert to the previous state
478
+ self.robot.update_kinematics()
479
+
480
+ no_iterations += 2 # add a few more iterations
481
+ # do the inital ik with 10 iterations
482
+ for i in range(no_iterations):
483
+ try:
484
+ self.fk_solver.solve(True) # False to not update the kinematics
485
+ except Exception as e:
486
+ self._logger.warning(f"FK solver failed: {e}, no solution found!")
487
+ return None
488
+ self.robot.update_kinematics()
489
+
490
+ if self.check_collision and self.compute_collision():
491
+ self._logger.warning("FK: Collision detected, using the previous config...")
492
+ self._update_state_to_initial(self.robot) # revert to the previous state
493
+ self.robot.state.q = q
494
+ self.robot.update_kinematics()
495
+ # return None
496
+
497
+ # Get the head frame transformation
498
+ T_world_head = self.robot.get_T_world_frame("head")
499
+ T_world_head[:3, 3][2] -= self.head_z_offset # offset the height of the head
500
+
501
+ return T_world_head
502
+
503
+ def config_collision_model(self) -> None:
504
+ """Configure the collision model for the robot.
505
+
506
+ Add collision pairs between the torso and the head colliders.
507
+ """
508
+ geom_model = self.robot.collision_model
509
+
510
+ # name_torso_collider = "dc15_a01_case_b_dummy_10"
511
+ # names_head_colliders = ["pp01063_stewart_plateform_7", "pp01063_stewart_plateform_11"]
512
+
513
+ id_torso_collider = 12 # geom_model.getGeometryObjectId(name_torso_collider)
514
+ id_head_colliders = [
515
+ 74,
516
+ 78,
517
+ ] # [geom_model.getGeometryObjectId(name) for name in names_head_colliders]
518
+
519
+ for i in id_head_colliders:
520
+ geom_model.addCollisionPair(
521
+ pin.CollisionPair(id_torso_collider, i)
522
+ ) # torso with head colliders
523
+
524
+ def compute_collision(self, margin: float = 0.005) -> bool:
525
+ """Compute the collision between the robot and the environment.
526
+
527
+ Args:
528
+ margin (float): The margin to consider for collision detection (default: 5mm).
529
+
530
+ Returns:
531
+ True if there is a collision, False otherwise.
532
+
533
+ """
534
+ collision_data = self.robot.collision_model.createData()
535
+ data = self.robot.model.createData()
536
+
537
+ # pin.computeCollisions(
538
+ pin.computeDistances(
539
+ self.robot.model,
540
+ data,
541
+ self.robot.collision_model,
542
+ collision_data,
543
+ self.robot.state.q,
544
+ )
545
+
546
+ # Iterate over all collision pairs
547
+ for distance_result in collision_data.distanceResults:
548
+ if distance_result.min_distance <= margin:
549
+ return True # Something is too close or colliding!
550
+
551
+ return False # Safe
552
+
553
+ def compute_jacobian(
554
+ self, q: Optional[npt.NDArray[np.float64]] = None
555
+ ) -> npt.NDArray[np.float64]:
556
+ """Compute the Jacobian of the head frame with respect to the actuated DoFs.
557
+
558
+ The jacobian in local world aligned.
559
+
560
+ Args:
561
+ q (np.ndarray, optional): Joint angles of the robot. If None, uses the current state of the robot. (default: None)
562
+
563
+ Returns:
564
+ np.ndarray: The Jacobian matrix.
565
+
566
+ """
567
+ # If q is provided, use it to compute the forward kinematics
568
+ if q is not None:
569
+ self.fk(q, no_iterations=20)
570
+
571
+ # Computing the platform Jacobian
572
+ # dx = Jp.dq
573
+ Jp: npt.NDArray[np.float64] = self.robot.frame_jacobian(
574
+ "head", "local_world_aligned"
575
+ )
576
+
577
+ # Computing the constraints Jacobian
578
+ # 0 = Jc.dq
579
+ constraints = []
580
+ for i in range(1, 6):
581
+ Jc = self.robot.relative_position_jacobian(
582
+ f"closing_{i}_1", f"closing_{i}_2"
583
+ )
584
+ constraints.append(Jc)
585
+ Jc = np.vstack(constraints)
586
+
587
+ # Splitting jacobians as
588
+ # Jp_a.dq_a + Jp_p.dq_p = dx
589
+ Jp_a = Jp[:, self.actives_idx]
590
+ Jp_p = Jp[:, self.passives_idx]
591
+ # Jc_a.dq_a + Jc_p.dq_p = 0
592
+ Jc_a = Jc[:, self.actives_idx]
593
+ Jc_p = Jc[:, self.passives_idx]
594
+
595
+ # Computing effector jacobian under constraints
596
+ # Because constraint equation
597
+ # Jc_a.dq_a + Jc_p.dq_p = 0
598
+ # can be written as:
599
+ # dq_p = - (Jc_p)^(⁻1) @ Jc_a @ dq_a
600
+ # Then we can substitute dq_p in the first equation and get
601
+ # This new jacobian
602
+ J: npt.NDArray[np.float64] = Jp_a - Jp_p @ np.linalg.inv(Jc_p) @ Jc_a
603
+
604
+ return J[:, self.actuated_idx_in_active]
605
+
606
+ def compute_gravity_torque(
607
+ self, q: Optional[npt.NDArray[np.float64]] = None
608
+ ) -> npt.NDArray[np.float64]:
609
+ """Compute the gravity torque vector for the actuated joints of the robot.
610
+
611
+ This method uses the static gravity compensation torques from the robot's dictionary.
612
+
613
+ Args:
614
+ q (np.ndarray, optional): Joint angles of the robot. If None, uses the current state of the robot. (default: None)
615
+
616
+ Returns:
617
+ np.ndarray: The gravity torque vector.
618
+
619
+ """
620
+ # If q is provided, use it to compute the forward kinematics
621
+ if q is not None:
622
+ self.fk(q)
623
+
624
+ # Get the static gravity compensation torques for all joints
625
+ # except the mobile base 6dofs
626
+ grav_torque_all_joints = np.array(
627
+ list(
628
+ self.robot.static_gravity_compensation_torques_dict(
629
+ "body_foot_3dprint"
630
+ ).values()
631
+ )
632
+ )
633
+
634
+ # See the paper for more info (equations 4-9):
635
+ # https://hal.science/hal-03379538/file/BriotKhalil_SpringerEncyclRob_bookchapterPKMDyn.pdf#page=4
636
+ #
637
+ # Basically to compute the actuated torques necessary to compensate the gravity, we need to compute the
638
+ # the equivalent wrench in the head frame that would be created if all the joints were actuated.
639
+ # wrench_eq = np.linalg.pinv(J_all_joints.T) @ torque_all_joints
640
+ # And then we can compute the actuated torques as:
641
+ # torque_actuated = J_actuated.T @ wrench_eq
642
+ J_all_joints: npt.NDArray[np.float64] = self.robot.frame_jacobian(
643
+ "head", "local_world_aligned"
644
+ )[:, 6:] # all joints except the mobile base 6dofs
645
+ J_actuated = self.compute_jacobian()
646
+ # using a single matrix G to compute the actuated torques
647
+ G = J_actuated.T @ np.linalg.pinv(J_all_joints.T)
648
+
649
+ # torques of actuated joints
650
+ grav_torque_actuated: npt.NDArray[np.float64] = G @ grav_torque_all_joints
651
+
652
+ # Compute the gravity torque
653
+ return grav_torque_actuated
654
+
655
+ def set_automatic_body_yaw(self, body_yaw: float) -> None:
656
+ """Set the automatic body yaw.
657
+
658
+ Args:
659
+ body_yaw (float): The yaw angle of the body.
660
+
661
+ """
662
+ self.start_body_yaw = body_yaw
663
+
664
+ def get_joint(self, joint_name: str) -> float:
665
+ """Get the joint object by its name."""
666
+ return float(self.robot.get_joint(joint_name))
@@ -0,0 +1 @@
1
+ """Media module."""