From c6c43cc4096ae440ddd90f7d93ae3f0224dd0242 Mon Sep 17 00:00:00 2001
From: David Weis <davidmichaelweis@gmail.com>
Date: Wed, 15 Nov 2017 07:16:22 +0100
Subject: [PATCH] Update relaxed poses to keep with the new coordiante system

---
 Quadruped/FunctionalGaitEngine.cs    | 14 ++++----------
 Quadruped/InterpolationController.cs | 14 ++++----------
 Quadruped/QuadrupedIkDriver.cs       | 16 ++++++++--------
 3 files changed, 16 insertions(+), 28 deletions(-)

diff --git a/Quadruped/FunctionalGaitEngine.cs b/Quadruped/FunctionalGaitEngine.cs
index 3bc7bcb..9d3c0c6 100644
--- a/Quadruped/FunctionalGaitEngine.cs
+++ b/Quadruped/FunctionalGaitEngine.cs
@@ -25,10 +25,10 @@ public class FunctionalGaitEngine : QuadrupedGaitEngine
 
         private LegPositions RelaxedStance => new LegPositions
         (
-            new Vector3(-LegDistance, LegDistance, LegHeight),
             new Vector3(LegDistance, LegDistance, LegHeight),
-            new Vector3(-LegDistance, -LegDistance, LegHeight),
-            new Vector3(LegDistance, -LegDistance, LegHeight)
+            new Vector3(LegDistance, -LegDistance, LegHeight),
+            new Vector3(-LegDistance, LegDistance, LegHeight),
+            new Vector3(-LegDistance, -LegDistance, LegHeight)
         );
 
 
@@ -42,13 +42,7 @@ public FunctionalGaitEngine(QuadrupedIkDriver driver) : base(driver)
                              currentPosition.RightRear.Z) / 4;
             if (average > -9)
             {
-                Driver.MoveLegsSynced(new LegPositions
-                (
-                    new Vector3(-LegDistance, LegDistance, 0),
-                    new Vector3(LegDistance, LegDistance, 0),
-                    new Vector3(-LegDistance, -LegDistance, 0),
-                    new Vector3(LegDistance, -LegDistance, 0)
-                ));
+                Driver.MoveLegsSynced(RelaxedStance.Transform(new Vector3(0, 0, -LegHeight)));
                 Thread.Sleep(1000);
             }
             _lastWrittenPosition = RelaxedStance;
diff --git a/Quadruped/InterpolationController.cs b/Quadruped/InterpolationController.cs
index c6c3255..e572e2b 100644
--- a/Quadruped/InterpolationController.cs
+++ b/Quadruped/InterpolationController.cs
@@ -22,10 +22,10 @@ public int Speed
 
         public LegPositions OriginalRelaxedStance => new LegPositions
         (
-            new Vector3(-LegDistanceLateral, LegDistanceLongitudinal, LegHeight),
             new Vector3(LegDistanceLateral, LegDistanceLongitudinal, LegHeight),
-            new Vector3(-LegDistanceLateral, -LegDistanceLongitudinal, LegHeight),
-            new Vector3(LegDistanceLateral, -LegDistanceLongitudinal, LegHeight)
+            new Vector3(LegDistanceLateral, -LegDistanceLongitudinal, LegHeight),
+            new Vector3(-LegDistanceLateral, LegDistanceLongitudinal, LegHeight),
+            new Vector3(-LegDistanceLateral, -LegDistanceLongitudinal, LegHeight)
         );
 
         public LegPositions RelaxedStance { get; set; }
@@ -47,13 +47,7 @@ public void EnqueueInitialStandup()
                              currentPositions.RightRear.Z) / 4;
             if (average > LegHeight + 2)
             {
-                _engine.AddStep(new LegPositions
-                (
-                    new Vector3(-LegDistanceLateral, LegDistanceLongitudinal, 0),
-                    new Vector3(LegDistanceLateral, LegDistanceLongitudinal, 0),
-                    new Vector3(-LegDistanceLateral, -LegDistanceLongitudinal, 0),
-                    new Vector3(LegDistanceLateral, -LegDistanceLongitudinal, 0)
-                ));
+                _engine.AddStep(RelaxedStance.Transform(new Vector3(0, 0, -LegHeight)));
             }
 
             _engine.AddStep(RelaxedStance);
diff --git a/Quadruped/QuadrupedIkDriver.cs b/Quadruped/QuadrupedIkDriver.cs
index d5f5722..a9e8faf 100644
--- a/Quadruped/QuadrupedIkDriver.cs
+++ b/Quadruped/QuadrupedIkDriver.cs
@@ -62,10 +62,10 @@ public void MoveRelativeCenterMass(Vector3 transform)
 
         public void MoveAbsoluteCenterMass(Vector3 transform, float legDistance, float height)
         {
-            MoveLeftFrontLeg(new Vector3(-legDistance, legDistance, height) - transform);
-            MoveRightFrontLeg(new Vector3(legDistance, legDistance, height) - transform);
-            MoveLeftRearLeg(new Vector3(-legDistance, -legDistance, height) - transform);
-            MoveRightRearLeg(new Vector3(legDistance, -legDistance, height) - transform);
+            MoveLeftFrontLeg(new Vector3(legDistance, legDistance, height) - transform);
+            MoveRightFrontLeg(new Vector3(legDistance, -legDistance, height) - transform);
+            MoveLeftRearLeg(new Vector3(-legDistance, legDistance, height) - transform);
+            MoveRightRearLeg(new Vector3(-legDistance, -legDistance, height) - transform);
         }
 
         public void DisableMotors()
@@ -140,10 +140,10 @@ public LegPositions ReadCurrentLegPositions()
 
         public void MoveToHeight(float height, float distance)
         {
-            MoveLeg(new Vector3(-distance, distance, height), LeftFront);
-            MoveLeg(new Vector3(distance, distance, height), RightFront);
-            MoveLeg(new Vector3(-distance, -distance, height), LeftRear);
-            MoveLeg(new Vector3(distance, -distance, height), RightRear);
+            MoveLeg(new Vector3(distance, distance, height), LeftFront);
+            MoveLeg(new Vector3(distance, -distance, height), RightFront);
+            MoveLeg(new Vector3(-distance, distance, height), LeftRear);
+            MoveLeg(new Vector3(-distance, -distance, height), RightRear);
         }
 
         public void Dispose()