Better type for f_target: using matrix N, 3 instead of flattened vector for ID

Author: earlaud

include/simple-mpc/inverse-dynamics.hpp

@@ -26,6 +26,8 @@ namespace simple_mpc
   class KinodynamicsID
   {
   public:
+    typedef Eigen::Matrix<double, Eigen::Dynamic, 3> MatrixN3d;
+
     struct Settings
     {
 
@@ -59,7 +61,7 @@ namespace simple_mpc
       const Eigen::Ref<const Eigen::VectorXd> & v_target,
       const Eigen::Ref<const Eigen::VectorXd> & a_target,
       const std::vector<bool> & contact_state_target,
-      const Eigen::Ref<const Eigen::VectorXd> & f_target);
+      const Eigen::Ref<const MatrixN3d> & f_target);
 
     void solve(
       const double t,

src/inverse-dynamics.cpp

@@ -77,11 +77,11 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
   const Eigen::VectorXd q_ref = model_handler.getReferenceState().head(nq);
   const Eigen::VectorXd v_ref = model_handler.getReferenceState().tail(nv);
   std::vector<bool> c_ref(n_contacts);
-  Eigen::VectorXd f_ref = Eigen::VectorXd::Zero(3 * n_contacts);
+  MatrixN3d f_ref = MatrixN3d::Zero(n_contacts, 3);
   for (int i = 0; i < n_contacts; i++)
   {
     c_ref[i] = true;
-    f_ref[2 + 3 * i] = weight / n_contacts;
+    f_ref(i, 2) = weight / n_contacts;
   }
   setTarget(q_ref, v_ref, v_ref, c_ref, f_ref);
   const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(0, q_ref, v_ref);
@@ -93,7 +93,7 @@ void KinodynamicsID::setTarget(
   const Eigen::Ref<const Eigen::VectorXd> & v_target,
   const Eigen::Ref<const Eigen::VectorXd> & a_target,
   const std::vector<bool> & contact_state_target,
-  const Eigen::Ref<const Eigen::VectorXd> & f_target)
+  const Eigen::Ref<const MatrixN3d> & f_target)
 {
   data_handler_.updateInternalData(q_target, v_target, false);
 
@@ -119,7 +119,7 @@ void KinodynamicsID::setTarget(
       {
         formulation_.addRigidContact(tsid_contacts[i], settings_.w_contact_force, settings_.w_contact_motion, 1);
       }
-      tsid_contacts[i].setForceReference(f_target.segment(i * 3, 3));
+      tsid_contacts[i].setForceReference(f_target.row(i));
       active_tsid_contacts_[i] = true;
     }
     else

tests/inverse_dynamics.cpp

@@ -57,7 +57,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
 
   solver.setTarget(
     q_target, Eigen::VectorXd::Zero(model_handler.getModel().nv), Eigen::VectorXd::Zero(model_handler.getModel().nv),
-    {false, false, false, false}, Eigen::VectorXd::Zero(4 * 3));
+    {false, false, false, false}, Eigen::MatrixXd::Zero(4, 3));
 
   double t = 0;
   Eigen::VectorXd q = solo_q_start(model_handler);
@@ -110,11 +110,11 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_contact)
       .set_w_contact_force(1.0));
 
   const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
-  Eigen::VectorXd f_target = Eigen::VectorXd::Zero(4 * 3);
-  f_target[2] = model_handler.getMass() * 9.81 / 4;
-  f_target[5] = model_handler.getMass() * 9.81 / 4;
-  f_target[8] = model_handler.getMass() * 9.81 / 4;
-  f_target[11] = model_handler.getMass() * 9.81 / 4;
+  Eigen::MatrixXd f_target = Eigen::MatrixXd::Zero(4, 3);
+  f_target(0, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(1, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(2, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(3, 2) = model_handler.getMass() * 9.81 / 4;
 
   solver.setTarget(
     q_target, Eigen::VectorXd::Zero(model_handler.getModel().nv), Eigen::VectorXd::Zero(model_handler.getModel().nv),
@@ -230,11 +230,11 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
       .set_w_contact_motion(1.0));
 
   const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
-  Eigen::VectorXd f_target = Eigen::VectorXd::Zero(4 * 3);
-  f_target[2] = model_handler.getMass() * 9.81 / 4;
-  f_target[5] = model_handler.getMass() * 9.81 / 4;
-  f_target[8] = model_handler.getMass() * 9.81 / 4;
-  f_target[11] = model_handler.getMass() * 9.81 / 4;
+  Eigen::MatrixXd f_target = Eigen::MatrixXd::Zero(4, 3);
+  f_target(0, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(1, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(2, 2) = model_handler.getMass() * 9.81 / 4;
+  f_target(3, 2) = model_handler.getMass() * 9.81 / 4;
 
   solver.setTarget(
     q_target, Eigen::VectorXd::Zero(model_handler.getModel().nv), Eigen::VectorXd::Zero(model_handler.getModel().nv),