Integrate new CoM type

Add overloaded accessor functions

src/Mathematics/intermediate-qp-matrices.cpp

@@ -49,42 +49,53 @@ 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.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;
-  }
+    {
+    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;
+    }
 }
 
-	  
+
+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,7 +50,6 @@ 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$
@@ -81,6 +80,7 @@ namespace PatternGeneratorJRL
     typedef standard_ls_form_s standard_ls_form_t;
     /// \}
 
+
     //
     //Public methods
     //
@@ -94,7 +94,12 @@ 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
     //
@@ -115,24 +120,23 @@ 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_objective_part_s
+    struct variant_obj_mat_s
     {
-      /// reference values for the whole preview window
+      /// \brief 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
-      MAL_VECTOR(CoMX,double);
-      MAL_VECTOR(CoMY,double);
-      MAL_VECTOR(CoMZ,double);
+      com_t CoM;
 
       /// \brief Selection matrix for the previewed and current feet positions.
       MAL_MATRIX(V,double);
@@ -146,9 +150,9 @@ namespace PatternGeneratorJRL
       int NbStepsPrw;
       /// \}
     };
-    typedef variant_objective_part_s variant_objective_part_t;
+    typedef variant_obj_mat_s variant_obj_mat_t;
     /// \}
-    variant_objective_part_t m_StateMatrices;
+    variant_obj_mat_t m_StateMatrices;
 
 
     /// \brief Cholesky decomposition of the initial objective function $Q$