diff --git a/examples/gui_demos/mini_head_lookat_gui.py b/examples/gui_demos/mini_head_lookat_gui.py new file mode 100644 index 00000000..fd45f296 --- /dev/null +++ b/examples/gui_demos/mini_head_lookat_gui.py @@ -0,0 +1,100 @@ +"""Reachy Mini Head Position GUI Example.""" + +import time +import tkinter as tk + +import numpy as np + +from reachy_mini import ReachyMini +from reachy_mini.utils import create_head_pose + + +def main(): + """Run a GUI to set the head position and orientation of Reachy Mini.""" + with ReachyMini(media_backend="no_media") as mini: + t0 = time.time() + + root = tk.Tk() + root.title("Set Look At XYZ Position") + + # Add sliders for X, Y, Z position + x_var = tk.DoubleVar(value=0.0) + y_var = tk.DoubleVar(value=0.0) + z_var = tk.DoubleVar(value=0.0) + + tk.Label(root, text="X (m):").grid(row=0, column=0) + tk.Scale( + root, + variable=x_var, + from_=-0.2, + to=0.2, + resolution=0.001, + orient=tk.HORIZONTAL, + length=200, + ).grid(row=0, column=1) + tk.Label(root, text="Y (m):").grid(row=1, column=0) + tk.Scale( + root, + variable=y_var, + from_=-0.2, + to=0.2, + resolution=0.001, + orient=tk.HORIZONTAL, + length=200, + ).grid(row=1, column=1) + tk.Label(root, text="Z (m):").grid(row=2, column=0) + tk.Scale( + root, + variable=z_var, + from_=-0.2, + to=0.2, + resolution=0.001, + orient=tk.HORIZONTAL, + length=200, + ).grid(row=2, column=1) + + tk.Label(root, text="Body Yaw (deg):").grid(row=3, column=0) + body_yaw_var = tk.DoubleVar(value=0.0) + tk.Scale( + root, + variable=body_yaw_var, + from_=-180, + to=180, + resolution=1.0, + orient=tk.HORIZONTAL, + length=200, + ).grid(row=3, column=1) + + mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0) + + # Run the GUI in a non-blocking way + root.update() + + try: + while True: + t = time.time() - t0 + target = np.deg2rad(30) * np.sin(2 * np.pi * 0.5 * t) + + head = np.eye(4) + head[:3, 3] = [0, 0, 0.0] + + head = mini.look_at_world(x_var.get(), y_var.get(), z_var.get(), duration=0.3, perform_movement=False) + + root.update() + + mini.set_target( + head=head, + body_yaw=np.deg2rad(body_yaw_var.get()), + antennas=np.array([target, -target]), + ) + + + + except KeyboardInterrupt: + pass + finally: + root.destroy() + + +if __name__ == "__main__": + main() diff --git a/examples/mini_head_position_gui.py b/examples/gui_demos/mini_head_position_gui.py similarity index 93% rename from examples/mini_head_position_gui.py rename to examples/gui_demos/mini_head_position_gui.py index 7b0f732d..d6df8dbd 100644 --- a/examples/mini_head_position_gui.py +++ b/examples/gui_demos/mini_head_position_gui.py @@ -83,11 +83,6 @@ def main(): length=200, ).grid(row=6, column=1) - # add a checkbox to enable/disable collision checking - collision_check_var = tk.BooleanVar(value=False) - tk.Checkbutton(root, text="Check Collision", variable=collision_check_var).grid( - row=7, column=1 - ) mini.goto_target(create_head_pose(), antennas=[0.0, 0.0], duration=1.0) diff --git a/pyproject.toml b/pyproject.toml index 1ffcfb2e..e49a08e3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,7 +24,7 @@ dependencies = [ "huggingface-hub==0.34.4", "sounddevice==0.5.1", "soundfile==0.13.1", - "reachy-mini-rust-kinematics>=1.0.1", + "reachy-mini-rust-kinematics>=1.0.2", "asgiref", "aiohttp", "log-throttling==0.0.3", diff --git a/src/reachy_mini/daemon/backend/mujoco/backend.py b/src/reachy_mini/daemon/backend/mujoco/backend.py index aba69eaa..6decdb55 100644 --- a/src/reachy_mini/daemon/backend/mujoco/backend.py +++ b/src/reachy_mini/daemon/backend/mujoco/backend.py @@ -92,11 +92,6 @@ def __init__( self.current_head_pose = np.eye(4) - # print("Joints in the model:") - # for i in range(self.model.njoint): - # name = mujoco.mj_id2joint(self.model, i) - # print(f" {i}: {name}") - self.joint_names = get_actuator_names(self.model) self.joint_ids = [ @@ -105,6 +100,14 @@ def __init__( self.joint_qpos_addr = [ get_joint_addr_from_name(self.model, n) for n in self.joint_names ] + + self.col_inds = [] + for i,type in enumerate(self.model.geom_contype): + if type != 0: + self.col_inds.append(i) + self.model.geom_contype[i] = 0 + self.model.geom_conaffinity[i] = 0 + def rendering_loop(self) -> None: """Offline Rendering loop for the Mujoco simulation. @@ -162,6 +165,18 @@ def run(self) -> None: # recompute all kinematics, collisions, etc. mujoco.mj_forward(self.model, self.data) + for i in range(100): + mujoco.mj_step(self.model, self.data) + + # enable collisions + for i in self.col_inds: + self.model.geom_contype[i] = 1 + self.model.geom_conaffinity[i] = 1 + + + for i in range(100): + mujoco.mj_step(self.model, self.data) + # one more frame so the viewer shows your startup pose mujoco.mj_step(self.model, self.data) if not self.headless: @@ -170,6 +185,11 @@ def run(self) -> None: rendering_thread = Thread(target=self.rendering_loop, daemon=True) rendering_thread.start() + # Update the internal states of the IK and FK to the current configuration + # This is important to avoid jumps when starting the robot (beore wake-up) + self.head_kinematics.ik(self.get_mj_present_head_pose(), no_iterations=20) + self.head_kinematics.fk(self.get_present_head_joint_positions(), no_iterations=20) + # 3) now enter your normal loop while not self.should_stop.is_set(): start_t = time.time() @@ -230,7 +250,7 @@ def run(self) -> None: took = time.time() - start_t time.sleep(max(0, self.model.opt.timestep - took)) - # print(f"Step {step}: took {took*1000:.1f}ms") + #print(f"Step {step}: took {took*1e6:.1f}us") step += 1 if not self.headless: diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m new file mode 100644 index 00000000..edda0bc7 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/README.m @@ -0,0 +1 @@ +Created with: https://github.com/pollen-robotics/reachy_mini_stl_convexify \ No newline at end of file diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl new file mode 100644 index 00000000..b134982c --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8f92539090c8fbef3a292ffbdc6a1019828ad5e33a5b87354ad08032a59d0c35 +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl new file mode 100644 index 00000000..dc98ea93 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9186fdc67813c3b7f3fba721cbfbb14d5e6f20bcabe1897d488913b6879bfd78 +size 4784 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl new file mode 100644 index 00000000..592ecb1e --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2db1d2726afd373e145a7b3cb8340983eeff49c07f8e4057e2d4eec7f91c35df +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl new file mode 100644 index 00000000..2bea29e7 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_back_3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e7916bfcdccd254c51143774575c7a0924a1a1939fd9a4755ae3b8f2c920bdc +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl new file mode 100644 index 00000000..728659d3 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94d9c386d10344ae4fa03baae3ea0ca935c8ab2a5d41352a4b4c59c3ac2fd9e1 +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl new file mode 100644 index 00000000..d296a17e --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e893cc8cd9581722ffe7798031076190cdc2264edc70694d925be5cf326b8bc2 +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl new file mode 100644 index 00000000..d990c548 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/body_top_3dprint_collider_front_2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ec16d901b544cd898eb1836d4f61fc16855919bd856c82f0fd23ac180e684af +size 4884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl new file mode 100644 index 00000000..b5a03639 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/coarse/head_one_3dprint_collider_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9ba0e5a4dc95869e16212e69fa3712ea7f021378cf76db0b1a8d2300eaab3c64 +size 6884 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl new file mode 100644 index 00000000..6e405fc6 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7c22bb171ab47155a4fe2946cd267f8e43cba901dd11d3106082bbe1848f90b4 +size 190084 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl new file mode 100644 index 00000000..34019f99 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9a2c6189ab48a1b296ab5e0695363ab20021154dc7823d3eef8a188b992c1682 +size 195784 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl new file mode 100644 index 00000000..67603dad --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cbdf0d34da912a29174148bd78b90e3541755f9bc20d2a5cc624e3565aef3d94 +size 78684 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl new file mode 100644 index 00000000..37d62560 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_back_3.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4e7c26eb2033477e711d6ea535b955a91f443b74d21a4d8e5ed1bfb72c10c401 +size 245084 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl new file mode 100644 index 00000000..f0f3c8da --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:27dcf3bcfc55a11f5fe3a6a5f01fe253fb5e9cf9d736aa9b83a44d286aeef38c +size 185384 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl new file mode 100644 index 00000000..ce58be8b --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_1.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:89940a91f8e1bf1d1b7c4844bc58a8a03f5c3983cfc174573e8f60f6a47c1109 +size 182484 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl new file mode 100644 index 00000000..8964aa93 --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/body_top_3dprint_collider_front_2.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ccf48cd480051da28caee941ad1e130f74cebfd2c474d5791d5e014e10fcdfd +size 219184 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl new file mode 100644 index 00000000..1d636e5f --- /dev/null +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/assets/collision/fine/head_one_3dprint_collider_0.stl @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5a28add53d3bc49c5b7f6d9e8f8f0884bbfa1ad9fe81af5ca58f01d08a072ed9 +size 1359584 diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml index 0d7260b4..7a166408 100644 --- a/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/joints_properties.xml @@ -2,8 +2,8 @@ - - + + diff --git a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml index bf5dafa5..bb32faf5 100644 --- a/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml +++ b/src/reachy_mini/descriptions/reachy_mini/mjcf/reachy_mini.xml @@ -11,7 +11,7 @@ - + @@ -19,8 +19,8 @@ - - + + @@ -75,6 +75,14 @@ + + + + + + + + @@ -400,6 +408,7 @@ + @@ -472,8 +481,10 @@ fovy="80" /> + + @@ -550,6 +561,17 @@ + + + + + + + + + + + diff --git a/src/reachy_mini/kinematics/analytical_kinematics.py b/src/reachy_mini/kinematics/analytical_kinematics.py index 0691b7d8..43c4fcde 100644 --- a/src/reachy_mini/kinematics/analytical_kinematics.py +++ b/src/reachy_mini/kinematics/analytical_kinematics.py @@ -67,10 +67,17 @@ def ik( """ _pose = pose.copy() _pose[:3, 3][2] += self.head_z_offset - - stewart_joints = self.kin.inverse_kinematics(_pose, body_yaw) # type: ignore[arg-type] - - return np.array([body_yaw] + stewart_joints) + + # inverse kinematics solution that modulates the body yaw to + # stay within the mechanical limits (max_body_yaw) + # additionally it makes sure the the relative yaw between the body and the head + # stays within the mechanical limits (max_relative_yaw) + reachy_joints = self.kin.inverse_kinematics_safe(_pose, # type: ignore[arg-type] + body_yaw = body_yaw, + max_relative_yaw = np.deg2rad(65), + max_body_yaw = np.deg2rad(160)) + + return np.array(reachy_joints) def fk( self, diff --git a/src/reachy_mini/kinematics/placo_kinematics.py b/src/reachy_mini/kinematics/placo_kinematics.py index 01896d90..3c977bdd 100644 --- a/src/reachy_mini/kinematics/placo_kinematics.py +++ b/src/reachy_mini/kinematics/placo_kinematics.py @@ -89,7 +89,7 @@ def __init__( # This will allow independent control of the torso and the head yaw # until this constraint is reached yaw_constraint = self.ik_solver.add_yaw_constraint( - "dummy_torso_yaw", "head", np.deg2rad(65.0) + "dummy_torso_yaw", "head", np.deg2rad(55.0) ) yaw_constraint.configure("rel_yaw", "hard") @@ -105,18 +105,18 @@ def __init__( # Add a cone constraint for the head to not exceed a certain angle # This is to avoid the head from looking too far up or down self.fk_cone = self.ik_solver.add_cone_constraint( - "body_foot_3dprint", "head", np.deg2rad(40.0) + "body_foot_3dprint", "head", np.deg2rad(35.0) ) self.fk_cone.configure("cone", "hard") self.fk_yaw_constraint = self.fk_solver.add_yaw_constraint( - "dummy_torso_yaw", "head", np.deg2rad(65.0) + "dummy_torso_yaw", "head", np.deg2rad(55.0) ) self.fk_yaw_constraint.configure("rel_yaw", "hard") # Add a cone constraint for the head to not exceed a certain angle # This is to avoid the head from looking too far up or down fk_cone = self.fk_solver.add_cone_constraint( - "body_foot_3dprint", "head", np.deg2rad(40.0) + "body_foot_3dprint", "head", np.deg2rad(35.0) ) fk_cone.configure("cone", "hard") @@ -142,7 +142,7 @@ def __init__( # regularization self.ik_yaw_joint_task = self.ik_solver.add_joints_task() self.ik_yaw_joint_task.set_joints({"yaw_body": 0}) - if self.automatic_body_yaw: + if not self.automatic_body_yaw: self.ik_yaw_joint_task.configure("joints", "soft", 5e-5) else: self.ik_yaw_joint_task.configure("joints", "soft", 1.0)