Correct OrientationsPreview: the order of left/right feet is no longe…

…r hard coded.
jrl-umi3218 · Apr 18, 2014 · 846be4a · 846be4a
1 parent ce74280
commit 846be4a
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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() );