From 846be4ab58282d87ef679a59ad7aa05316c464e6 Mon Sep 17 00:00:00 2001
From: Francois Keith <keith@lirmm.fr>
Date: Fri, 18 Apr 2014 17:15:34 +0200
Subject: [PATCH] Correct OrientationsPreview: the order of left/right feet is
 no longer hard coded.

---
 .../OrientationsPreview.cpp                   | 22 +++++++++++++------
 .../OrientationsPreview.hh                    |  4 ++--
 .../ZMPVelocityReferencedQP.cpp               |  2 +-
 3 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
index 07748ff8..951d55c0 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
@@ -37,27 +37,35 @@ using namespace std;
 
 const double OrientationsPreview::EPS_ = 0.00000001;
 
-OrientationsPreview::OrientationsPreview( CjrlJoint *aRootJoint)
+OrientationsPreview::OrientationsPreview( CjrlHumanoidDynamicRobot *aHS)
 {
+  CjrlJoint * waist     = aHS->waist();
 
-  lLimitLeftHipYaw_ = aRootJoint->childJoint(1)->lowerBound(0);//-30.0/180.0*M_PI;
-  uLimitLeftHipYaw_  = aRootJoint->childJoint(1)->upperBound(0);//45.0/180.0*M_PI;
+  // left hip
+  CjrlJoint * leftFoot  = aHS->leftFoot()->associatedAnkle();
+  CjrlJoint * leftHip   = aHS->jointsBetween(*waist, *leftFoot)[0];
+
+  lLimitLeftHipYaw_  = leftHip->lowerBound(0);//-30.0/180.0*M_PI;
+  uLimitLeftHipYaw_  = leftHip->upperBound(0);//45.0/180.0*M_PI;
   if (lLimitLeftHipYaw_==  uLimitLeftHipYaw_)
     {
       lLimitLeftHipYaw_ = -30.0/180.0*M_PI;
       uLimitLeftHipYaw_ = 45.0/180.0*M_PI;
     }
 
-  lLimitRightHipYaw_ = aRootJoint->childJoint(0)->lowerBound(0);//-45.0/180.0*M_PI;
-  uLimitRightHipYaw_ = aRootJoint->childJoint(0)->upperBound(0);//30.0/180.0*M_PI;
+  // right hip
+  CjrlJoint * rightFoot = aHS->rightFoot()->associatedAnkle();
+  CjrlJoint * rightHip  = aHS->jointsBetween(*waist, *rightFoot)[0];
+
+  lLimitRightHipYaw_ = rightHip->lowerBound(0);//-45.0/180.0*M_PI;
+  uLimitRightHipYaw_ = rightHip->upperBound(0);//30.0/180.0*M_PI;
   if (lLimitRightHipYaw_==  uLimitRightHipYaw_)
     {
       lLimitRightHipYaw_ = -30.0/180.0*M_PI;
       uLimitRightHipYaw_ = 45.0/180.0*M_PI;
     }
 
-
-  uvLimitFoot_ = fabs(aRootJoint->childJoint(0)->upperVelocityBound(0));
+  uvLimitFoot_ = fabs(leftHip->upperVelocityBound(0));
 
   //Acceleration limit not given by HRP2JRLmain.wrl
   uaLimitHipYaw_ = 0.1;
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
index b6cf8a7f..05a402c1 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
@@ -37,7 +37,7 @@
 
 #include <privatepgtypes.hh>
 #include <jrl/walkgen/pgtypes.hh>
-#include <abstract-robot-dynamics/joint.hh>
+#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
 
 namespace PatternGeneratorJRL
 {
@@ -51,7 +51,7 @@ namespace PatternGeneratorJRL
 
     /// \name Accessors
     /// \{
-    OrientationsPreview( CjrlJoint *aLeg );
+    OrientationsPreview( CjrlHumanoidDynamicRobot *aHS );
     ~OrientationsPreview();
     /// \}
 
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 0b384738..c9162421 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -80,7 +80,7 @@ ZMPVelocityReferencedQP::ZMPVelocityReferencedQP(SimplePluginManager *SPM,
   SupportFSM_->SamplingPeriod( QP_T_ );
 
   // Create and initialize preview of orientations
-  OrientPrw_ = new OrientationsPreview( aHS->rootJoint() );
+  OrientPrw_ = new OrientationsPreview( aHS );
   OrientPrw_->SamplingPeriod( QP_T_ );
   OrientPrw_->NbSamplingsPreviewed( QP_N_ );
   OrientPrw_->SSLength( SupportFSM_->StepPeriod() );