Merge pull request #22 from pollen-robotics/21-range-of-neck-angles-different-from-real-robot

Set angles in range -180,180

Author: FabienDanieau

Packages/ReachySimulator/Scripts/JointServiceServer.cs

@@ -383,20 +383,21 @@ public override Task<FullBodyCartesianCommandAck> SendFullBodyCartesianCommands(
                         (float)fullBodyCartesianCommand.Neck.Q.W);
 
                     Vector3 neck_commands = Mathf.Deg2Rad * headRotation.eulerAngles;
+
                     jointCommandList.Add(new JointCommand
                     {
                         Id = new JointId { Name = "neck_roll" },
-                        GoalPosition = (float?)neck_commands[2],
+                        GoalPosition = (float?)ChangeAngleRange(neck_commands[2]),
                     });
                     jointCommandList.Add(new JointCommand
                     {
                         Id = new JointId { Name = "neck_pitch" },
-                        GoalPosition = (float?)neck_commands[0],
+                        GoalPosition = (float?)ChangeAngleRange(neck_commands[0]),
                     });
                     jointCommandList.Add(new JointCommand
                     {
                         Id = new JointId { Name = "neck_yaw" },
-                        GoalPosition = (float?)neck_commands[1],
+                        GoalPosition = -(float?)ChangeAngleRange(neck_commands[1]),
                     });
                 }
 
@@ -447,6 +448,13 @@ public override async Task<FullBodyCartesianCommandAck> StreamFullBodyCartesianC
                 NeckCommandSuccess = true
             });
         }
+
+        private float ChangeAngleRange(float orbita_angle)
+        {
+            float modified_angle = orbita_angle % (2.0f * (float)Math.PI);
+            modified_angle = modified_angle > (float)Math.PI ? modified_angle - (2.0f * (float)Math.PI) : modified_angle;
+            return modified_angle;
+        }
     }
 
     public class ArmKinematicsImpl : ArmKinematics.ArmKinematicsBase

Packages/ReachySimulator/Scripts/ReachyController.cs

@@ -269,7 +269,7 @@ public void HandleCommand(Dictionary<JointId, float> commands)
                 if (motorName == "neck_yaw")
                 {
                     containNeckCommand = true;
-                    headOrientation[2] = kvp.Value;
+                    headOrientation[2] = -kvp.Value;
                 }
             }