Revert "Integrate new CoM type"

This reverts commit fd4dcaf1.

src/Mathematics/intermediate-qp-matrices.cpp

@@ -49,53 +49,42 @@ IntermedQPMat::~IntermedQPMat()
 void 
 IntermedQPMat::getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType)
 {
+
   TMat.nSt = m_StateMatrices.NbStepsPrw;
 
   switch(ObjectiveType)
-    {
-    case MEAN_VELOCITY:
-      TMat.U = m_MeanVelocity.U;
-      TMat.UT = m_MeanVelocity.UT;
-      TMat.Sc_x = MAL_RET_A_by_B(m_MeanVelocity.S,m_StateMatrices.CoM.x);
-      TMat.Sc_x = MAL_RET_A_by_B(m_MeanVelocity.S,m_StateMatrices.CoM.y);
-      TMat.weight = m_MeanVelocity.weight;
-      TMat.ref_x = m_StateMatrices.RefX;
-      TMat.ref_y = m_StateMatrices.RefY;
-    case INSTANT_VELOCITY:
-      TMat.U = m_InstantVelocity.U;
-      TMat.UT = m_InstantVelocity.UT;
-      TMat.Sc_x = MAL_RET_A_by_B(m_InstantVelocity.S,m_StateMatrices.CoM.x);
-      TMat.Sc_y = MAL_RET_A_by_B(m_InstantVelocity.S,m_StateMatrices.CoM.y);
-      TMat.weight = m_InstantVelocity.weight;
-      TMat.ref_x = m_StateMatrices.RefX;
-      TMat.ref_y = m_StateMatrices.RefY;
-    case COP_CENTERING:
-      TMat.U = m_COPCentering.U;
-      TMat.UT = m_COPCentering.UT;
-      TMat.V = m_StateMatrices.V;
-      TMat.VT = m_StateMatrices.VT;
-      TMat.Sc_x = MAL_RET_A_by_B(m_COPCentering.S,m_StateMatrices.CoM.x);
-      TMat.Sc_x = MAL_RET_A_by_B(m_COPCentering.S,m_StateMatrices.CoM.y);
-      TMat.weight = m_COPCentering.weight;
-      TMat.ref_x = m_StateMatrices.Vc*m_StateMatrices.fx;
-      TMat.ref_y = m_StateMatrices.Vc*m_StateMatrices.fy;
-    case JERK:
-      TMat.U = m_Jerk.U;
-      TMat.UT = m_Jerk.UT;
-      TMat.Sc_x = MAL_RET_A_by_B(m_Jerk.S,m_StateMatrices.CoM.x);
-      TMat.Sc_x = MAL_RET_A_by_B(m_Jerk.S,m_StateMatrices.CoM.y);
-      TMat.weight = m_Jerk.weight;
-      TMat.ref_x = 0.0*m_StateMatrices.RefX;
-      TMat.ref_y = 0.0*m_StateMatrices.RefY;
-    }
+  {
+  case MEAN_VELOCITY:
+    TMat.U = m_MeanVelocity.U;
+    TMat.UT = m_MeanVelocity.UT;
+    TMat.Sc_x = MAL_RET_A_by_B(m_MeanVelocity.S,m_StateMatrices.RefX);
+    TMat.weight = m_MeanVelocity.weight;
+    TMat.ref_x = m_StateMatrices.RefX;
+    TMat.ref_y = m_StateMatrices.RefY;
+  case INSTANT_VELOCITY:
+    TMat.U = m_InstantVelocity.U;
+    TMat.UT = m_InstantVelocity.UT;
+    TMat.Sc_x = MAL_RET_A_by_B(m_InstantVelocity.S,m_StateMatrices.RefX);
+    TMat.weight = m_InstantVelocity.weight;
+    TMat.ref_x = m_StateMatrices.RefX;
+    TMat.ref_y = m_StateMatrices.RefY;
+  case COP_CENTERING:
+    TMat.U = m_COPCentering.U;
+    TMat.UT = m_COPCentering.UT;
+    TMat.V = m_StateMatrices.V;
+    TMat.VT = m_StateMatrices.VT;
+    TMat.Sc_x = MAL_RET_A_by_B(m_COPCentering.S,m_StateMatrices.RefX);
+    TMat.weight = m_COPCentering.weight;
+    TMat.ref_x = m_StateMatrices.Vc*m_StateMatrices.fx;
+    TMat.ref_y = m_StateMatrices.Vc*m_StateMatrices.fy;
+  case JERK:
+    TMat.U = m_Jerk.U;
+    TMat.UT = m_Jerk.UT;
+    TMat.Sc_x = MAL_RET_A_by_B(m_Jerk.S,m_StateMatrices.RefX);
+    TMat.weight = m_Jerk.weight;
+    TMat.ref_x = 0.0*m_StateMatrices.RefX;
+    TMat.ref_y = 0.0*m_StateMatrices.RefY;
+  }
 }
 
-
-com_t IntermedQPMat::operator ()() const
-{
-  return m_StateMatrices.CoM;
-}
-void IntermedQPMat::operator ()( com_t CoM )
-{
-  m_StateMatrices.CoM = CoM;
-}
+	  

src/Mathematics/intermediate-qp-matrices.hh

@@ -50,6 +50,7 @@ namespace PatternGeneratorJRL
     const static int COP_CENTERING = 2;
     const static int JERK = 3;
 
+
     /// \name Standardized least square elements
     /// \{
     /// \brief Standard form: \f$ Q = \alpha M1^{\top}M2, p = \alpha U^{\top}(S*c-ref) \f$
@@ -80,7 +81,6 @@ namespace PatternGeneratorJRL
     typedef standard_ls_form_s standard_ls_form_t;
     /// \}
 
-
     //
     //Public methods
     //
@@ -94,12 +94,7 @@ namespace PatternGeneratorJRL
     /// \brief Get matrices in the standard Least Squares form:
     /// \f$ Q = \alpha U^{\top}U, p = \alpha U^{\top}(S*c-ref) \f$
     void getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType);
-
-    /// \brief Accessors for the CoM
-    com_t operator ()() const;
-    void operator ()( com_t CoM );
-
-
+	  
     //
     //Private members
     //
@@ -120,23 +115,24 @@ namespace PatternGeneratorJRL
     typedef invariant_objective_part_s invariant_objective_part_t;
     /// \}
     invariant_objective_part_t
-    m_MeanVelocity,
+      m_MeanVelocity,
       m_InstantVelocity,
       m_COPCentering,
       m_Jerk;
 
-
     /// \name QP elements that are objective independent
     /// \{
-    struct variant_obj_mat_s
+    struct variant_objective_part_s
     {
-      /// \brief reference values for the whole preview window
+      /// reference values for the whole preview window
       MAL_VECTOR(RefX,double);
       MAL_VECTOR(RefY,double);
       MAL_VECTOR(RefTheta,double);
 
       /// \brief State of the Center of Mass
-      com_t CoM;
+      MAL_VECTOR(CoMX,double);
+      MAL_VECTOR(CoMY,double);
+      MAL_VECTOR(CoMZ,double);
 
       /// \brief Selection matrix for the previewed and current feet positions.
       MAL_MATRIX(V,double);
@@ -150,9 +146,9 @@ namespace PatternGeneratorJRL
       int NbStepsPrw;
       /// \}
     };
-    typedef variant_obj_mat_s variant_obj_mat_t;
+    typedef variant_objective_part_s variant_objective_part_t;
     /// \}
-    variant_obj_mat_t m_StateMatrices;
+    variant_objective_part_t m_StateMatrices;
 
 
     /// \brief Cholesky decomposition of the initial objective function $Q$