Revert "Add matrices and types necessary to construct a least squares objective"

This reverts commit 22431d47.

src/Mathematics/intermediate-qp-matrices.cpp

@@ -36,7 +36,7 @@ using namespace PatternGeneratorJRL;
 
 IntermedQPMat::IntermedQPMat()
 {
-
+  //TODO:
 }
 
 
@@ -47,44 +47,32 @@ IntermedQPMat::~IntermedQPMat()
 
 
 void 
-IntermedQPMat::getTermMatrices(standard_ls_form_t & TMat, int ObjectiveType)
+IntermedQPMat::getLSTerm(ObjectiveTerm_t LST, int ObjectiveType)
 {
+  //TODO:
+}
+	  
+	
+//
+//Private methods
+//
+void 
+IntermedQPMat::weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_MATRIX (&M2, double), MAL_MATRIX (&M3, double))
+{
+  //TODO:
+}
+	  
 
-  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;
-  }
+void 
+IntermedQPMat::weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_MATRIX (&M2, double), MAL_VECTOR (&V, double))
+{
+  //TODO:
 }
+	  
 
+void 
+IntermedQPMat::weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_VECTOR (&M2, double), MAL_VECTOR (&M3, double), MAL_VECTOR (&V, double))
+{
+  //TODO:
+}
 	  

src/Mathematics/intermediate-qp-matrices.hh

@@ -31,7 +31,6 @@
 //# include <boost/numeric/ublas/matrix.hpp>
 //# include <boost/numeric/ublas/vector.hpp>
 #include <jrl/mal/matrixabstractlayer.hh>
-
 #include <privatepgtypes.h>
 
 
@@ -42,45 +41,14 @@ namespace PatternGeneratorJRL
   class  IntermedQPMat
   {
     //
-    //Public types
+    //Public constants
     //
   public:
     const static int MEAN_VELOCITY = 0;
     const static int INSTANT_VELOCITY = 1;
-    const static int COP_CENTERING = 2;
+    const static int FOOT_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$
-    struct standard_ls_form_s
-    {
-      double weight;
-      /// \brief Matrix of the quadratic part
-      MAL_MATRIX(U,double);
-      MAL_MATRIX(UT,double);
-      MAL_MATRIX(V,double);
-      MAL_MATRIX(VT,double);
-
-      /// \brief Reference
-      MAL_VECTOR(Sc_x,double);
-      MAL_VECTOR(Sc_y,double);
-
-      /// \brief Reference
-      MAL_VECTOR(ref_x,double);
-      MAL_VECTOR(ref_y,double);
-
-      /// \brief Final products (the ones that are added to the final QP)
-      MAL_MATRIX(weightM1TM2,double);
-      MAL_VECTOR(weightM1TV1,double);
-
-      /// \brief Number of previewed steps
-      int nSt;
-    };
-    typedef standard_ls_form_s standard_ls_form_t;
-    /// \}
-
     //
     //Public methods
     //
@@ -91,74 +59,69 @@ namespace PatternGeneratorJRL
     ~IntermedQPMat();
     /// \}
 
-    /// \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 Get matrices corresponding to the desired Least Squares term
+    void getLSTerm(ObjectiveTerm_t LST, int ObjectiveType);
+	
+    //
+    //Private methods
+    //
+  private:
+
+    /// \brief \f$ M1 = \alpha M2^{\top}M3 \f$
+    void weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_MATRIX (&M2, double), MAL_MATRIX (&M3, double));
+	  
+    /// \brief \f$ M1 = \alpa M2^{\top}V \f$
+    void weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_MATRIX (&M2, double), MAL_VECTOR (&V, double));  
+	  
+    /// \brief \f$ M1 = \alpa M2^{\top}M3V \f$
+    void weightedTransProd(MAL_MATRIX (&M1, double), double alpha, MAL_VECTOR (&M2, double), MAL_VECTOR (&M3, double), MAL_VECTOR (&V, double));    
+	  
 	  
     //
     //Private members
     //
   private:
 
-    /// \name Least square objective's dependent elements
-    /// \{
-    struct invariant_objective_part_s
-    {
-      double weight;
-      /// \brief Matrix of the quadratic part
-      MAL_MATRIX(U,double);
-      MAL_MATRIX(UT,double);
-
-      /// \brief Matrix of the linear part
-      MAL_MATRIX(S,double);
-    };
-    typedef invariant_objective_part_s invariant_objective_part_t;
-    /// \}
-    invariant_objective_part_t
-      m_MeanVelocity,
-      m_InstantVelocity,
-      m_COPCentering,
-      m_Jerk;
+	  
+    /// \brief Current state of the point mass model.
+    MAL_VECTOR(m_xk,double);
+	  
+    /// \brief Matrix relating the command and the CoM position.
+    MAL_MATRIX(m_Up,double);
 
-    /// \name QP elements that are objective independent
-    /// \{
-    struct variant_objective_part_s
-    {
-      /// 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);
-
-      /// \brief Selection matrix for the previewed and current feet positions.
-      MAL_MATRIX(V,double);
-      MAL_MATRIX(VT,double);
-      MAL_VECTOR(Vc,double);
-
-      /// \brief Current support foot position
-      double fx, fy;
-
-      /// \brief Number of free steps in the preview window
-      int NbStepsPrw;
-      /// \}
-    };
-    typedef variant_objective_part_s variant_objective_part_t;
-    /// \}
-    variant_objective_part_t m_StateMatrices;
+    /// \brief Matrix relating the command and the ZMP position.
+    MAL_MATRIX(m_Uz,double);
+
+    /// \brief Matrix relating the command and the CoM speed.
+    MAL_MATRIX(m_Uv,double);
+
+    /// \brief Matrix relating the CoM state and the CoM position.
+    MAL_MATRIX(m_Sp,double);
+
+    /// \brief Matrix relating the CoM state and the ZMP position.
+    MAL_MATRIX(m_Sz,double);
 
+    /// \brief Matrix relating the CoM state and the CoM speed.
+    MAL_MATRIX(m_Sv,double);
 
-    /// \brief Cholesky decomposition of the initial objective function $Q$
+    /// \brief Selection matrix for the previewed feet positions.
+    MAL_MATRIX(m_U,double);
+
+    /// \brief Selection matrix for the support feet.
+    MAL_VECTOR(m_Uc,double);
+
+    /// \brief Cholesky decomposition of the initial objective function $Q$ 
     MAL_MATRIX(m_LQ,double);
-    /// \brief Cholesky decomposition of the initial objective function $Q$
+
+    /// \brief Cholesky decomposition of the initial objective function $Q$ 
     MAL_MATRIX(m_iLQ,double);
-    /// \brief Constant part of the constraint matrices.
+
+    /// \brief Constant part of the constraint matrices. 
     MAL_MATRIX(m_iDu,double);
+	  
     /// \brief Constant part of the constraint matrices.
     MAL_MATRIX(m_Ds,double);
+
     /// \brief Sub matrices to compute the linear part of the objective function $p^{\top}_k$.
     MAL_MATRIX(m_OptA,double);
     MAL_MATRIX(m_OptB,double);
@@ -169,7 +132,7 @@ namespace PatternGeneratorJRL
     enum MatrixType  { M_UP, M_UZ, M_UV, M_SP,
 		       M_SZ, M_SV, M_U,
 		       M_LQ, M_ILQ};
-    enum VectorType  { M_UC,M_OPTA, M_OPTB, M_OPTC, M_OPTD };
+    enum VectorType  { m_UC,M_OPTA, M_OPTB, M_OPTC, M_OPTD };
 
 
   };