Implement the generation of constraints in GenVelRef

- Add buildConstraintInequalities

src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp

@@ -28,6 +28,7 @@
 
 #include <ZMPRefTrajectoryGeneration/generator-vel-ref.hh>
 
+using namespace std;
 using namespace PatternGeneratorJRL;
 
 
@@ -252,6 +253,90 @@ GeneratorVelRef::generateFeetPosConstraints (CjrlFoot & Foot,
 }
 
 
+void
+GeneratorVelRef::buildConstraintInequalities( std::deque< FootAbsolutePosition> & LeftFootAbsolutePositions,
+    std::deque<FootAbsolutePosition> & RightFootAbsolutePositions,
+    std::deque<LinearConstraintInequalityFreeFeet_t> & ZMPInequalitiesDeque,
+    std::deque<LinearConstraintInequalityFreeFeet_t> & FeetPosInequalitiesDeque,
+    std::deque<support_state_t> & deqSupportStates,
+    std::deque<double> & PreviewedSupportAngles, int & NbConstraints,
+    FootConstraintsAsLinearSystemForVelRef * FCALS)
+{
+
+  std::vector<CH_Point> ZMPConstrVertices;
+  ZMPConstrVertices.resize(4);
+  std::vector<CH_Point> FeetPosConstrVertices;
+  FeetPosConstrVertices.resize(5);
+
+  //determine the current support angle
+  std::deque<FootAbsolutePosition>::iterator FAP_it;
+
+  support_state_t & CurrentSupport = deqSupportStates.front();
+
+  //define the current support angle
+  if( CurrentSupport.Foot==1 )
+    {
+      FAP_it = LeftFootAbsolutePositions.end();
+      FAP_it--;
+    }
+  else
+    {
+      FAP_it = RightFootAbsolutePositions.end();
+      FAP_it--;
+    }
+  double CurrentSupportAngle = FAP_it->theta*M_PI/180.0;
+
+  double ZMPConvHullOrientation = CurrentSupportAngle;
+  double FPConvHullOrientation = CurrentSupportAngle;
+
+  //set current constraints
+  FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
+               ZMPConvHullOrientation, FPConvHullOrientation,
+               CurrentSupport );
+
+
+  //set constraints for the whole preview window
+  for( int i=1;i<=m_N;i++ )
+    {
+
+      support_state_t & PrwSupport = deqSupportStates[i];
+      if( PrwSupport.StateChanged )
+        FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
+                     ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
+
+      if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 )
+          FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-1];
+
+      //foot positioning constraints
+      if( PrwSupport.StateChanged && PrwSupport.StepNumber>0 && PrwSupport.Phase != 0)
+        {
+          ZMPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-1];
+
+          if( PrwSupport.StepNumber == 1 )
+              FPConvHullOrientation = CurrentSupportAngle;
+          else
+              FPConvHullOrientation = PreviewedSupportAngles[PrwSupport.StepNumber-2];
+
+          FCALS->setVertices( ZMPConstrVertices, FeetPosConstrVertices,
+                       ZMPConvHullOrientation, FPConvHullOrientation, PrwSupport );
+
+          LinearConstraintInequalityFreeFeet_t aLCIFP;
+          FCALS->computeLinearSystem( FeetPosConstrVertices, aLCIFP.D, aLCIFP.Dc, PrwSupport );
+          aLCIFP.StepNumber = PrwSupport.StepNumber;
+          FeetPosInequalitiesDeque.push_back( aLCIFP );
+          NbConstraints += MAL_MATRIX_NB_ROWS( aLCIFP.D );
+        }
+
+      LinearConstraintInequalityFreeFeet_t aLCI;
+      FCALS->computeLinearSystem( ZMPConstrVertices, aLCI.D, aLCI.Dc, PrwSupport );
+      aLCI.StepNumber = PrwSupport.StepNumber;
+      ZMPInequalitiesDeque.push_back( aLCI );
+      NbConstraints += MAL_MATRIX_NB_ROWS( aLCI.D );
+    }
+
+}
+
+
 void 
 GeneratorVelRef::buildInvariantPart(QPProblem & Pb, IntermedQPMat & Matrices)
 {

src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh

@@ -31,12 +31,15 @@
 
 #include <ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh>
 
-#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
 #include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
+#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
 #include <PreviewControl/SupportFSM.h>
-#include <privatepgtypes.h>
-#include <Mathematics/intermediate-qp-matrices.hh>
 #include <PreviewControl/LinearizedInvertedPendulum2D.h>
+#include <Mathematics/intermediate-qp-matrices.hh>
+#include <Mathematics/FootConstraintsAsLinearSystemForVelRef.h>
+
+#include <privatepgtypes.h>
+
 
 namespace PatternGeneratorJRL
 {
@@ -60,10 +63,9 @@ namespace PatternGeneratorJRL
 
     /// \brief Set the weights on the objective terms
     ///
-    /// \param alpha
-    /// \param beta
-    /// \param gamma
-    /// \param delta
+    /// \param Matrices
+    /// \param weight
+    /// \param objective
     void setPonderation( IntermedQPMat & Matrices, double weight, int objective );
 
     /// \brief Set the velocity reference from string
@@ -73,6 +75,7 @@ namespace PatternGeneratorJRL
 
     /// \brief Preview support state for the whole preview period
     ///
+    /// \param Matrices
     /// \param FSM
     /// \param deqSupportStates
     void preview(IntermedQPMat Matrices,
@@ -136,6 +139,14 @@ namespace PatternGeneratorJRL
 				     support_state_t &,
 				     QPProblem & Pb);
 
+    void buildConstraintInequalities( std::deque< FootAbsolutePosition> &LeftFootAbsolutePositions,
+        std::deque<FootAbsolutePosition> &RightFootAbsolutePositions,
+        std::deque<LinearConstraintInequalityFreeFeet_t> & ZMPInequalitiesDeque,
+        std::deque<LinearConstraintInequalityFreeFeet_t> & FeetPosInequalitiesDeque,
+        std::deque<support_state_t> & deqSupportStates,
+        std::deque<double> &PreviewedSupportAngles, int & NbConstraints,
+        FootConstraintsAsLinearSystemForVelRef * FCALS);
+
     /// \brief Build the constant part of the objective
     ///
     /// \param Pb