Merge branch 'topic/vel-ref-gen-cleaning-restructuring'

Conflicts:
	src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
	src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h

src/Mathematics/FootConstraintsAsLinearSystemForVelRef.h

@@ -45,7 +45,7 @@
 #include <PGTypes.h>
 #include <Mathematics/ConvexHull.h>
 #include <SimplePlugin.h>
-#include <PreviewControl/SupportState.h>
+#include <PreviewControl/SupportFSM.h>
 
 namespace PatternGeneratorJRL
 {

src/PreviewControl/SupportState.cpp

-/*
- * Copyright 2010, 
- *
- * Andrei  Herdt
- * Olivier Stasse
- *
- * JRL, CNRS/AIST
- *
- * This file is part of walkGenJrl.
- * walkGenJrl is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * walkGenJrl is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Lesser Public License for more details.
- * You should have received a copy of the GNU Lesser General Public License
- * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
- *
- *  Research carried out within the scope of the 
- *  Joint Japanese-French Robotics Laboratory (JRL)
- */
-/* TODO 3: Restructure the class
- *  setSupportState.cpp
- */
-
-#include <iostream>
-#include <fstream>
-
-#include <PreviewControl/SupportState.h>
-#include <Debug.h>
-
-using namespace PatternGeneratorJRL;
-using namespace std;
-
-SupportState::SupportState(const double &SamplingPeriod)
-{
-  SSPeriod = 0.8; 	  //Duration of one step
-  DSDuration = 1e9;       //Duration of the DS phase
-  DSSSDuration = 0.8;
-  NbOfStepsSSDS = 200;
-
-  m_T = SamplingPeriod;
-  //Initial current state
-  CurrentSupportPhase = 0;
-  CurrentSupportFoot = 1;
-  CurrentTimeLimit = 1000000000;
-  CurrentStepsLeft = 1;
-  StartSupportFoot = 1;
-
-  eps = 0.00000001;
-  m_StateChanged = false;
-
-  m_FullDebug = 0;
-
-}
- 
-
-SupportState::~SupportState()
-{
-}
-
-void SupportState::setSupportState(const double &Time, const int &pi,  const ReferenceAbsoluteVelocity & RefVel)
-
-{
-
-  m_StateChanged = false;
-
-  if(pi==0) {
-    m_SupportPhase = &CurrentSupportPhase;
-    m_SupportFoot = &CurrentSupportFoot;
-    m_SupportStepsLeft = &CurrentStepsLeft;
-    m_SupportTimeLimit = &CurrentTimeLimit;
-    StepNumber = 0;
-    SSSS = 0;
-  }
-  else {
-    m_SupportPhase = &PrwSupportPhase;
-    m_SupportFoot = &PrwSupportFoot;
-    m_SupportStepsLeft = &PrwStepsLeft;
-    m_SupportTimeLimit = &PrwTimeLimit;
-  }
-
-  
-  ReferenceGiven = -1;
-  if(fabs(RefVel.x)>0||fabs(RefVel.y)>0)
-    ReferenceGiven = 1;
-    
-  if(ReferenceGiven == 1 && *m_SupportPhase == 0 && (*m_SupportTimeLimit-Time-eps)>DSSSDuration)
-    {
-      *m_SupportTimeLimit = Time+DSSSDuration;
-    }
- 
-   
-  //FSM
-  if(Time+eps+pi*m_T >= *m_SupportTimeLimit)
-    {
-      //SS->DS
-      if(*m_SupportPhase == 1  && ReferenceGiven == -1 && *m_SupportStepsLeft==0)
-	{
-	  *m_SupportPhase = 0;
-	  *m_SupportTimeLimit = Time+pi*m_T + DSDuration;
-	  m_StateChanged = true;
-	}
-      //DS->SS
-      else if(*m_SupportPhase == 0 && ReferenceGiven == 1)
-	{
-	  *m_SupportPhase = 1;
-	  *m_SupportTimeLimit = Time+pi*m_T + SSPeriod;
-	  *m_SupportStepsLeft = NbOfStepsSSDS;
-	  m_StateChanged = true;
-	}
-      //SS->SS
-      else if(((*m_SupportPhase == 1) && (*m_SupportStepsLeft>0)) ||
-	      ((*m_SupportStepsLeft==0) && (ReferenceGiven == 1)))
-	{
-	  *m_SupportFoot = -1**m_SupportFoot;
-	  m_StateChanged = true;
-	  *m_SupportTimeLimit = Time+pi*m_T + SSPeriod;
-	  StepNumber++;
-	  SSSS = 1;
-	  if (ReferenceGiven == -1)
-	    *m_SupportStepsLeft = *m_SupportStepsLeft-1;
-	}
-    }
-
-
-  if(pi==0)
-    initializePreviewedState();
-
-  
-  if(m_FullDebug>0)
-    {
-      ofstream aof;
-      aof.open("SupportStates.dat", ios::app);
-      aof << "Time: "<<Time<<" PrwTime: "<<Time+pi*m_T<<" CSP: "<<CurrentSupportPhase
-  	  <<" CSF: "<<CurrentSupportFoot<<" CTL: "<<CurrentTimeLimit
-  	  <<" CSL: "<<CurrentStepsLeft<<" PrwSP: "<<PrwSupportPhase
-  	  <<" PrwSF: "<<PrwSupportFoot<<" PrwTL: "<<PrwTimeLimit
-  	  <<" PrwSL: "<<PrwStepsLeft<<" *SF: "<<*m_SupportFoot
-  	  <<" *SSL: "<<*m_SupportStepsLeft<<" SN: "<<StepNumber;
-      aof << endl;
-      aof.close();
-    }
-
-}
-
-void SupportState::initializePreviewedState()
-{ 
-  PrwSupportPhase = CurrentSupportPhase;
-  PrwSupportFoot = CurrentSupportFoot;
-  PrwStepsLeft = CurrentStepsLeft;
-  PrwTimeLimit = CurrentTimeLimit;
-  StepNumber = 0;
-}

src/PreviewControl/SupportState.h

-/*
- * Copyright 2010, 
- *
- * Andrei  Herdt
- *
- * JRL, CNRS/AIST
- *
- * This file is part of walkGenJrl.
- * walkGenJrl is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * walkGenJrl is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Lesser Public License for more details.
- * You should have received a copy of the GNU Lesser General Public License
- * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
- *
- *  Research carried out within the scope of the 
- *  Joint Japanese-French Robotics Laboratory (JRL)
- */
-/* This object provides the finite state machine to determine the support parameters. */
-
-#ifndef _SUPPORT_STATE_
-#define _SUPPORT_STATE_
-
-#include <walkGenJrl/PGTypes.h>
-
-namespace PatternGeneratorJRL
-{
-  class  SupportState
-  {
-  public:
-    /*! Constructor */
-    SupportState(const double &SamplingPeriod);
-	
-    /*! Destructor */
-    ~SupportState();
-	
-    /*! \brief Initialize the previewed state. */
-    void setSupportState(const double &Time, const int &pi,  const ReferenceAbsoluteVelocity & RefVel);
-	
-
-	
-    ///*! \brief Numerical precision */
-    double eps;
-	
-    /*! \brief constants for the durations in the support phases */
-    double DSDuration, SSPeriod, DSSSDuration;
-
-    /*! \brief First support foot */
-    int StartSupportFoot;
-	
-    /*! \brief Current support state */
-    int CurrentSupportPhase, CurrentSupportFoot, CurrentStepsLeft;
-
-    int SSSS;
-
-    double CurrentTimeLimit;
-	
-    /*! \brief Future support state */
-    int PrwSupportPhase, PrwSupportFoot, PrwStepsLeft;
-
-    double  PrwTimeLimit;
-
-    bool m_StateChanged;
-
-    int StepNumber;
-
-    //Number of steps done before DS
-    unsigned int NbOfStepsSSDS;
-
- 
-  private: 
-	
-    /*! \Brief Sampling duration */
-    double m_T;
-
-    /*! \Brief Support state */
-    int *m_SupportPhase, *m_SupportFoot, *m_SupportStepsLeft;
-
-    double *m_SupportTimeLimit;
-    void initializePreviewedState();
-
-    int ReferenceGiven;
-
-    int m_FullDebug;
-
-
-    ///*! Getter for the support state */
-    //const void SupportState::getSupportState () const;
-	
-    
-  };
-};
-
-#endif /* _SUPPORT_STATE_ */

src/ZMPRefTrajectoryGeneration/OrientationsPreview.h

@@ -37,7 +37,7 @@
 #include <deque>
 
 /*! Framework includes */
-#include <PreviewControl/SupportState.h>
+#include <PreviewControl/SupportFSM.h>
 
 /*! Framework includes */
 #include <walkGenJrl/PGTypes.h>

src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.h

@@ -25,7 +25,7 @@
  *  Joint Japanese-French Robotics Laboratory (JRL)
  */
 /*! This object provides the generation of ZMP and CoM trajectory
-   using a new formulation of the stability problem.
+  using a new formulation of the stability problem.
 */
 
 #ifndef _ZMPVELOCITYREFERENCEDQP_WITH_CONSTRAINT_H_
@@ -38,7 +38,7 @@
 #include <Mathematics/OptCholesky.h>
 #include <ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.h>
 #include <Mathematics/PLDPSolverHerdt.h>
-#include <PreviewControl/SupportState.h>
+#include <PreviewControl/SupportFSM.h>
 #include <FootTrajectoryGeneration/FootTrajectoryGenerationStandard.h>
 #include <ZMPRefTrajectoryGeneration/OrientationsPreview.h>
 
@@ -61,7 +61,7 @@ namespace PatternGeneratorJRL
 
     /*! \name Methods to build the optimization problem
       @{
-     */
+    */
 
     /*! \brief Compute the constant matrices over all the instances of the problem.
       This means \f$P_{pu}, P_{px}, P_{vs}, P_{vu}\f$.
@@ -88,7 +88,7 @@ namespace PatternGeneratorJRL
 
     /*! \brief Call the two previous methods
       \return A negative value in case of a problem 0 otherwise.
-     */
+    */
     int InitConstants();
 
     void initFeet();
@@ -224,7 +224,7 @@ namespace PatternGeneratorJRL
     /*! Uses a Finite State Machine to simulate the evolution of the support states. */
     SupportState * m_Support;
 
-    /*! Uses a Finite State Machine to simulate the evolution of the support states. */
+    /*! Deecoupled optimization problem to compute the evolution of feet angles. */
     OrientationsPreview * m_OP;
 
     /*! \brief Object creating Linear inequalities constraints
@@ -246,15 +246,16 @@ namespace PatternGeneratorJRL
     /*! Orientations of the feet previewed over the whole horizon length*/
     //deque<double> PreviewedSupportAngles;
 
-    /*! Current state of the trunk */
+    /*! Current state of the trunk and the trunk state after m_QP_T*/
     COMState m_TrunkState, m_TrunkStateT;
 
     deque<COMState> m_QueueOfTrunkStates;
 
-    MAL_VECTOR(m_PerturbationAcceleration,double);
-
     double m_a, m_TrunkPolCoeffB, m_c, m_d, m_TrunkPolCoeffE;
 
+    //Additional term on the acceleration of the CoM
+    MAL_VECTOR(m_PerturbationAcceleration,double);
+
     /*! Sampling of the QP. */
     double m_QP_T;
 
@@ -267,11 +268,11 @@ namespace PatternGeneratorJRL
     //Final optimization problem
     struct Problem_s
     {
-    	int m, me, mmax, n, nmax, mnn;
-    	double *Q, *D, *DU, *DS, *XL, *XU, *X, *NewX, *U, *war;//For COM
-    	int *iwar;
-    	int iout, ifail, iprint, lwar, liwar;
-    	double Eps;
+      int m, me, mmax, n, nmax, mnn;
+      double *Q, *D, *DU, *DS, *XL, *XU, *X, *NewX, *U, *war;//For COM
+      int *iwar;
+      int iout, ifail, iprint, lwar, liwar;
+      double Eps;
     };
     typedef struct Problem_s Problem;
 
@@ -287,7 +288,6 @@ namespace PatternGeneratorJRL
 
     SupportState_t m_CurrentSupport;
 
-
     /*! \name Variables related to the QP
       @{ */
     /*! \brief Matrix relating the command and the CoM position. */
@@ -333,6 +333,7 @@ namespace PatternGeneratorJRL
 
     /*! \name Parameters of the objective function
     @{ */
+
     /*! Putting weight on the velocity */
     double m_Beta;
 
@@ -366,17 +367,16 @@ namespace PatternGeneratorJRL
     void computeCholeskyOfQ(double * OptA);
 
     void computeObjective(deque<LinearConstraintInequalityFreeFeet_t> & QueueOfLConstraintInequalitiesFreeFeet,
-		    deque<SupportFeet_t> & QueueOfSupportFeet,
-		    int NbOfConstraints, int NbOfEqConstraints,
-		    int & CriteriaToMaximize, MAL_VECTOR(& xk,double), double time);
-
+			  deque<SupportFeet_t> & QueueOfSupportFeet,
+			  int NbOfConstraints, int NbOfEqConstraints,
+			  int & CriteriaToMaximize, MAL_VECTOR(& xk,double), double time);
 
     void interpolateTrunkState(double time, int CurrentIndex,
-    		deque<COMState> & FinalCOMStates);
+			       deque<COMState> & FinalCOMStates);
 
     void interpolateFeetPositions(double time, int CurrentIndex,
-    				     deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
-    				     deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions);
+				  deque<FootAbsolutePosition> &FinalLeftFootAbsolutePositions,
+				  deque<FootAbsolutePosition> &FinalRightFootAbsolutePositions);
 
     int dumpProblem(double * Q,
 		    double * D,
@@ -397,7 +397,7 @@ namespace PatternGeneratorJRL
     /*! Methods to comply with the initial interface of ZMPRefTrajectoryGeneration.
       TODO: Change the internal structure to make those methods not mandatory
       for compiling.
-     */
+    */
 
     void GetZMPDiscretization(std::deque<ZMPPosition> & ZMPPositions,
 			      std::deque<COMState> & COMStates,