diff --git a/pyproject.toml b/pyproject.toml index a66a0efa..48853f59 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", ] 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,