Extend InverseDynamicsFormulationAccForce to work also for robot manipulators,...

Extend InverseDynamicsFormulationAccForce to work also for robot manipulators, which are fully-actuated, so they do not need to specify the unactuated base dynamics as equality constraints in the TSID optimization problem.

include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp

@@ -159,6 +159,7 @@ namespace tsid
     double m_t;         /// time
     unsigned int m_k;   /// number of contact-force variables
     unsigned int m_v;   /// number of acceleration variables
+    unsigned int m_u;   /// number of unactuated DoFs
     unsigned int m_eq;  /// number of equality constraints
     unsigned int m_in;  /// number of inequality constraints
     Matrix m_Jc;        /// contact force Jacobian

src/formulations/inverse-dynamics-formulation-acc-force.cpp

@@ -44,13 +44,14 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
                                                                        bool verbose):
   InverseDynamicsFormulationBase(name, robot, verbose),
   m_data(robot.model()),
-  m_baseDynamics("base-dynamics", 6, robot.nv()),
+  m_baseDynamics("base-dynamics", robot.nv()-robot.na(), robot.nv()),
   m_solutionDecoded(false)
 {
   m_t = 0.0;
   m_v = robot.nv();
+  m_u = robot.nv() - robot.na();
   m_k = 0;
-  m_eq = 6;
+  m_eq = m_u;
   m_in = 0;
   m_hqpData.resize(2);
   m_Jc.setZero(m_k, m_v);
@@ -82,7 +83,7 @@ unsigned int InverseDynamicsFormulationAccForce::nIn() const
 void InverseDynamicsFormulationAccForce::resizeHqpData()
 {
   m_Jc.setZero(m_k, m_v);
-  m_baseDynamics.resize(6, m_v+m_k);
+  m_baseDynamics.resize(m_u, m_v+m_k);
   for(HQPData::iterator it=m_hqpData.begin(); it!=m_hqpData.end(); it++)
   {
     for(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++)
@@ -347,12 +348,12 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
     cl->forceRegTask->vector() = fr.vector();
   }
 
-  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
-  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-6);
-  const Matrix & J_a = m_Jc.rightCols(m_v-6);
-  const Matrix & M_u = m_robot.mass(m_data).topRows<6>();
-  const Vector & h_u = m_robot.nonLinearEffects(m_data).head<6>();
-  const Matrix & J_u = m_Jc.leftCols<6>();
+  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
+  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
+  const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
+  const Matrix & M_u = m_robot.mass(m_data).topRows(m_u);
+  const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u);
+  const Matrix & J_u = m_Jc.leftCols(m_u);
 
   m_baseDynamics.matrix().leftCols(m_v) = M_u;
   m_baseDynamics.matrix().rightCols(m_k) = -J_u.transpose();
@@ -425,9 +426,9 @@ bool InverseDynamicsFormulationAccForce::decodeSolution(const HQPOutput & sol)
 {
   if(m_solutionDecoded)
     return true;
-  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-6);
-  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-6);
-  const Matrix & J_a = m_Jc.rightCols(m_v-6);
+  const Matrix & M_a = m_robot.mass(m_data).bottomRows(m_v-m_u);
+  const Vector & h_a = m_robot.nonLinearEffects(m_data).tail(m_v-m_u);
+  const Matrix & J_a = m_Jc.rightCols(m_v-m_u);
   m_dv = sol.x.head(m_v);
   m_f = sol.x.tail(m_k);
   m_tau = h_a;