TargetContactForce now a list of vector instead of matrices

Author: earlaud

examples/talos_kinodynamics.py

@@ -235,6 +235,7 @@
         xs_interp = interpolator.interpolateLinear(delay, dt_mpc, xss)
         acc_interp = interpolator.interpolateLinear(delay, dt_mpc, ddqs)
         force_interp = interpolator.interpolateLinear(delay, dt_mpc, forces).reshape((2,6))
+        force_interp = [force_interp[0, :], force_interp[1, :]]
 
         q_interp = xs_interp[:mpc.getModelHandler().getModel().nq]
         v_interp = xs_interp[mpc.getModelHandler().getModel().nq:]

include/simple-mpc/inverse-dynamics.hpp

@@ -26,7 +26,8 @@ namespace simple_mpc
   class KinodynamicsID
   {
   public:
-    typedef Eigen::Matrix<double, Eigen::Dynamic, 3> MatrixN3d;
+    typedef Eigen::VectorXd TargetContactForce;
+    // typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 6, 1> TargetContactForce;
 
     struct Settings
     {
@@ -61,7 +62,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 MatrixN3d> & f_target);
+      const std::vector<TargetContactForce> & f_target);
 
     void solve(
       const double t,

src/inverse-dynamics.cpp

@@ -122,7 +122,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 MatrixN3d> & f_target)
+  const std::vector<TargetContactForce> & f_target)
 {
   data_handler_.updateInternalData(q_target, v_target, false);
 
@@ -151,11 +151,11 @@ void KinodynamicsID::setTarget(
       switch (model_handler_.getFootType(i))
       {
       case RobotModelHandler::FootType::POINT: {
-        std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[i])->setForceReference(f_target.row(i));
+        std::static_pointer_cast<tsid::contacts::ContactPoint>(tsid_contacts[i])->setForceReference(f_target.at(i));
         break;
       }
-      case RobotModelHandler::FootType::SIX_D: {
-        std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[i])->setForceReference(f_target.row(i));
+      case RobotModelHandler::FootType::QUAD: {
+        std::static_pointer_cast<tsid::contacts::Contact6d>(tsid_contacts[i])->setForceReference(f_target.at(i));
         break;
       }
       default: {

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::MatrixXd::Zero(4, 3));
+    {false, false, false, false}, {});
 
   double t = 0;
   Eigen::VectorXd q = solo_q_start(model_handler);
@@ -110,12 +110,12 @@ 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::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;
-
+  std::vector<KinodynamicsID::TargetContactForce> f_target;
+  for (int i = 0; i < 4; i++)
+  {
+    f_target.push_back(KinodynamicsID::TargetContactForce::Zero(3));
+    f_target[i][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),
     {true, true, true, true}, f_target);