add mutator : setAlgorithm

include/centroidal-dynamics-lib/centroidal_dynamics.hh

@@ -119,6 +119,8 @@ public:
 
   EquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
 
+  void setAlgorithm(EquilibriumAlgorithm algorithm);
+
   /**
    * @brief Specify a new set of contacts.
    * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.

src/centroidal_dynamics.cpp

@@ -130,6 +130,13 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
     return true;
 }
 
+void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
+    if(algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
+      SEND_DEBUG_MSG("Cannot set algorithm to PP after setting contacts, you should call again setNewContact with PP algorithm");
+    else
+      m_algorithm = algorithm;
+}
+
 bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
                                        const double frictionCoefficient, const EquilibriumAlgorithm alg)
 {