Merge pull request #4 from pollen-robotics/fix_erreur_safe_ik

error in sign

Author: apirrone

Cargo.lock

@@ -1,6 +1,6 @@
 [package]
 name = "reachy_mini_rust_kinematics"
-version = "1.0.2"
+version = "1.0.3"
 edition = "2024"
 
 [lib]

Cargo.toml

@@ -237,21 +237,21 @@ impl Kinematics {
         let mut body_yaw_target = 0.0;
         // if body yaw is specified, rotate the platform accordingly
         if body_yaw.is_some() {
-            body_yaw_target = body_yaw.unwrap();
+            body_yaw_target = -body_yaw.unwrap();
             // first verify if the body yaw is within the allowed limits
             // relative yaw is the yaw difference between the current platform yaw and body yaw
             // it should stays within +/- max_relative_yaw
             if let Some(max_rel_yaw) = max_relative_yaw {
                 let current_yaw = t_world_platform[(0, 1)].atan2(t_world_platform[(0, 0)]);
                 let relative_yaw = body_yaw_target - current_yaw;
                 body_yaw_target = current_yaw + relative_yaw.clamp(-max_rel_yaw, max_rel_yaw);
-                body_yaw_target = -body_yaw_target;
             }
             // then clamp the body yaw within +/- max_body_yaw
             // this is physically limited by the mechanical design
             if let Some(max_body_yaw) = max_body_yaw {
                 body_yaw_target = body_yaw_target.clamp(-max_body_yaw, max_body_yaw);
             }
+            body_yaw_target = -body_yaw_target;
         }
 
         // construct the joint angles vector