Add QuadFoot test on talos

Author: earlaud

tests/inverse_dynamics.cpp

@@ -214,7 +214,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_baseTask)
 
 BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
 {
-  RobotModelHandler model_handler = getSoloHandler();
+  RobotModelHandler model_handler = getTalosModelHandler();
   RobotDataHandler data_handler(model_handler);
   const double dt = 1e-3;
 
@@ -223,25 +223,25 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
     KinodynamicsID::Settings()
       .set_kp_base(7.)
       .set_kp_posture(10.)
-      .set_kp_contact(10.0)
-      .set_w_base(100.0)
-      .set_w_posture(1.0)
-      .set_w_contact_force(1.0)
+      .set_kp_contact(1.0)
+      .set_w_base(10.0)
+      .set_w_posture(10.0)
+      .set_w_contact_force(.1)
       .set_w_contact_motion(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 < 2; i++)
+  {
+    f_target.push_back(KinodynamicsID::TargetContactForce::Zero(6));
+    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);
 
   double t = 0;
-  Eigen::VectorXd q = solo_q_start(model_handler);
+  Eigen::VectorXd q = model_handler.getReferenceState().head(model_handler.getModel().nq);
   Eigen::VectorXd v = Eigen::VectorXd::Random(model_handler.getModel().nv);
   Eigen::VectorXd a = Eigen::VectorXd::Random(model_handler.getModel().nv);
   Eigen::VectorXd tau = Eigen::VectorXd::Zero(model_handler.getModel().nv - 6);