Merge OrientationsPreview.

src/ZMPRefTrajectoryGeneration/OrientationsPreview.cpp

@@ -74,11 +74,9 @@ OrientationsPreview::OrientationsPreview(const double & SamplingPeriod,
       aof.close();
     }
 
-  //	RESETDEBUG4("OrientationsPreview.dat");
 }
 
 OrientationsPreview::~OrientationsPreview() {
-  //  cout<<"Leaving ~OrientationsPreview()"<<endl;
 }
 
 
@@ -86,7 +84,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
 					      deque<double> &PreviewedSupportAngles,
 					      const COMState &TrunkState, 
 					      COMState &TrunkStateT,
-					      const SupportState * Support,
+					      const SupportFSM * SupportFSM ,SupportState_t CurrentSupport,
 					      deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
 					      deque<FootAbsolutePosition> &RightFootAbsolutePositions)
 {
@@ -126,7 +124,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
     {
 
       //Initialize the preview loop
-      if (Support->CurrentSupportFoot == 1)
+      if (CurrentSupport.Foot == 1)
 	{
 	  m_CurrentSupportAngle = LeftFootAbsolutePositions[0].theta*M_PI/180.0;
 
@@ -140,7 +138,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
 
 
 
-      if(Support->CurrentSupportPhase != 0)
+      if(CurrentSupport.Phase != 0)
 	{
 	  m_TrunkAngleOK = false;
 	  while(!m_TrunkAngleOK)
@@ -179,19 +177,18 @@ void OrientationsPreview::previewOrientations(const double &Time,
 
 		}
 	      //Compute the trunk angle at the end of the support phase
-	      m_SupportTimePassed = Support->CurrentTimeLimit-Time;
+	      m_SupportTimePassed = CurrentSupport.TimeLimit-Time;
 	      m_PreviewedTrunkAngleEnd = TrunkStateT.yaw[0] + TrunkStateT.yaw[1]*(m_SupportTimePassed-m_T);
 
 
 	      //Verify the angle between the support foot and the trunk at the end of the current support period
-	      m_TrunkAngleOK = verifyAngleOfHipJoint(
-						     Support, TrunkState, TrunkStateT, m_CurrentSupportAngle, StepNumber);
+	      m_TrunkAngleOK = verifyAngleOfHipJoint(CurrentSupport, TrunkState, TrunkStateT, m_CurrentSupportAngle, StepNumber);
 	    }
 
 	}
       else//The trunk does not rotate in the DS phase
 	{
-	  m_SupportTimePassed = Support->CurrentTimeLimit+m_SSPeriod-Time;
+	  m_SupportTimePassed = CurrentSupport.TimeLimit+m_SSPeriod-Time;
 	  m_FirstPreviewedFoot = 1;
 	  PreviewedSupportAngles.push_back(m_CurrentSupportAngle);
 	  TrunkStateT.yaw[0] = m_PreviewedTrunkAngleEnd = TrunkState.yaw[0];
@@ -212,7 +209,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
 	}
 
       m_PreviousSupportAngle = m_CurrentSupportAngle;
-      m_PreviewedSupportFoot = Support->CurrentSupportFoot;
+      m_PreviewedSupportFoot = CurrentSupport.Foot;
 
 
       if(m_FullDebug>2)
@@ -231,7 +228,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
 
       //Preview
       for(StepNumber = m_FirstPreviewedFoot; 
-	  StepNumber <= (unsigned int)((int)ceil((m_N+1)*m_T/Support->SSPeriod)); 
+	  StepNumber <= (unsigned int)((int)ceil((m_N+1)*m_T/SupportFSM->m_SSPeriod));
 	  StepNumber++)
 	{
 	  m_PreviewedSupportFoot = -m_PreviewedSupportFoot;
@@ -248,12 +245,10 @@ void OrientationsPreview::previewOrientations(const double &Time,
 	    }
 
 	  verifyVelocityOfHipJoint(Time, TrunkStateT,
-				   m_PreviewedSupportFoot, StepNumber, Support,
+				   m_PreviewedSupportFoot, StepNumber, CurrentSupport,
 				   CurrentRightFootAngle, CurrentLeftFootAngle,
 				   CurrentLeftFootVelocity, CurrentRightFootVelocity);
 
-	  //verifyVelocityOfHipJoint(Ref, TrunkStateT.yaw[1], TrunkState, Support, StepNumber);
-
 	  //Check the feet angles to avoid self-collision:
 	  if ((double)m_PreviewedSupportFoot*(m_PreviousSupportAngle-m_PreviewedSupportAngle)-M_EPS > m_uLimitFeet)
 	    {
@@ -284,7 +279,7 @@ void OrientationsPreview::previewOrientations(const double &Time,
 	    }
 
 	  m_TrunkAngleOK = verifyAngleOfHipJoint(
-						 Support, TrunkState, TrunkStateT,
+						 CurrentSupport, TrunkState, TrunkStateT,
 						 m_CurrentSupportAngle, StepNumber);
 	  if(!m_TrunkAngleOK){
 	    PreviewedSupportAngles.clear();
@@ -323,9 +318,9 @@ void OrientationsPreview::previewOrientations(const double &Time,
 void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVelocity_t &Ref,
 						       const COMState &TrunkState, 
 						       COMState &TrunkStateT, 
-						       const SupportState * Support)
+						       SupportState_t CurrentSupport)
 {
-  if(Support->CurrentSupportPhase!=0)
+  if(CurrentSupport.Phase!=0)
     {
       //Verify change in velocity against the maximal acceleration
       if(fabs(Ref.dYaw-TrunkState.yaw[1]) > 2.0/3.0*m_T*m_uaLimitHipYaw)
@@ -357,7 +352,7 @@ void OrientationsPreview::verifyAccelerationOfHipJoint(const ReferenceAbsoluteVe
 }
 
 
-bool OrientationsPreview::verifyAngleOfHipJoint(const SupportState * Support,
+bool OrientationsPreview::verifyAngleOfHipJoint(SupportState_t CurrentSupport,
 						const COMState &TrunkState, COMState &TrunkStateT,
 						double CurrentSupportFootAngle,
 						unsigned int StepNumber)
@@ -365,7 +360,7 @@ bool OrientationsPreview::verifyAngleOfHipJoint(const SupportState * Support,
 
   //Which limitation is relevant in the current situation?
   double uJointLimit, lJointLimit, JointLimit;
-  if(Support->CurrentSupportFoot == 1)
+  if(CurrentSupport.Foot == 1)
     {
       uJointLimit = m_uLimitLeftHipYaw;
       lJointLimit = m_lLimitLeftHipYaw;
@@ -403,7 +398,7 @@ void OrientationsPreview::verifyVelocityOfHipJoint(const double &Time,
 						   COMState &TrunkStateT,
 						   const double &PreviewedSupportFoot, 
 						   const unsigned int &StepNumber,
-						   const SupportState * Support,
+						   SupportState_t CurrentSupport,
 						   const double &CurrentRightFootAngle, 
 						   const double &CurrentLeftFootAngle,
 						   const double &CurrentLeftFootVelocity,
@@ -418,7 +413,7 @@ void OrientationsPreview::verifyVelocityOfHipJoint(const double &Time,
   double a,b,c,d,T;
   //To be implemented
   //For the
-  if(StepNumber>0 && Support->CurrentSupportPhase==1)
+  if(StepNumber>0 && CurrentSupport.Phase==1)
     {
       //verify the necessary, maximal, relative foot velocity
       m_MeanFootVelDifference = (m_PreviewedSupportAngle-CurrentAngle)/(m_SSPeriod-m_T);
@@ -430,10 +425,10 @@ void OrientationsPreview::verifyVelocityOfHipJoint(const double &Time,
 	  m_PreviewedSupportAngle = CurrentAngle+m_MeanFootVelDifference*(m_SSPeriod-m_T);
 	}
     }
-  else if((StepNumber==0 && Support->CurrentSupportPhase==1) || (StepNumber==1 && Support->CurrentSupportPhase==0))
+  else if((StepNumber==0 && CurrentSupport.Phase==1) || (StepNumber==1 && CurrentSupport.Phase==0))
     {
 
-      T = Support->CurrentTimeLimit-Time-m_T;
+      T = CurrentSupport.TimeLimit-Time-m_T;
       //Previewed polynome
       a = CurrentAngle;
       if(PreviewedSupportFoot==1)

src/ZMPRefTrajectoryGeneration/OrientationsPreview.h

@@ -55,13 +55,13 @@ namespace PatternGeneratorJRL
     void previewOrientations(const double &Time,
 			     std::deque<double> &PreviewedSupportAngles,
 			     const COMState &TrunkState, COMState &TrunkStateT,
-			     const SupportState * Support,
+			     SupportState_t CurrentSupport,
 			     std::deque<FootAbsolutePosition> &LeftFootAbsolutePositions,
 			     std::deque<FootAbsolutePosition> &RightFootAbsolutePositions);
 
     void verifyAccelerationOfHipJoint(const ReferenceAbsoluteVelocity_t &Ref,
 				      const COMState &TrunkState, COMState &TrunkStateT,
-				      const SupportState * Support);
+				      SupportState_t CurrentSupport);
 
   private:
     /*! Angular limitations of the hip joints*/