Merge pull request #24 from pollen-robotics/19-goal-position-kept-when-turning-robot-compliant

19 goal position kept when turning robot compliant

Author: FabienDanieau

Packages/ReachySimulator/Scripts/CameraServiceServer.cs

@@ -109,7 +109,7 @@ public override async Task StreamImage(StreamImageRequest imageRequest, Grpc.Cor
             }
             catch (OperationCanceledException e)
             {
-
+                Debug.LogWarning(e);
             }
         }
 

Packages/ReachySimulator/Scripts/JointServiceServer.cs

@@ -132,7 +132,7 @@ public override async Task<JointsCommandAck> StreamJointsCommands(IAsyncStreamRe
             }
             catch (OperationCanceledException e)
             {
-
+                Debug.LogWarning(e);
             }
             return (new JointsCommandAck { Success = true });
         }
@@ -306,7 +306,7 @@ public override async Task StreamJointsState(StreamJointsRequest jointRequest, G
             }
             catch (OperationCanceledException e)
             {
-
+                Debug.LogWarning(e);
             }
         }
 
@@ -438,7 +438,7 @@ public override async Task<FullBodyCartesianCommandAck> StreamFullBodyCartesianC
             }
             catch (OperationCanceledException e)
             {
-
+                Debug.LogWarning(e);
             }
 
             return (new FullBodyCartesianCommandAck
@@ -651,7 +651,7 @@ public override async Task StreamSensorStates(StreamSensorsStateRequest stateReq
             }
             catch (OperationCanceledException e)
             {
-
+                Debug.LogWarning(e);
             }
         }
     }

Packages/ReachySimulator/Scripts/ReachyController.cs

@@ -161,7 +161,10 @@ void Update()
                 if (!m.name.StartsWith("neck"))
                 {
                     JointController joint = m.gameObject.GetComponent<JointController>();
-                    joint.RotateTo(m.targetPosition);
+                    if (m.isCompliant)
+                        joint.RotateTo(m.presentPosition);
+                    else
+                        joint.RotateTo(m.targetPosition);
                     joint.IsCompliant(m.isCompliant);
 
                     m.presentPosition = joint.GetPresentPosition();