Add contacts and base costs

Author: earlaud

include/simple-mpc/inverse-dynamics.hpp

@@ -38,15 +38,15 @@ namespace simple_mpc
         0.01) // Min force for one foot contact (express as a multiple of the robot weight)
 
       // Tasks gains
-      DEFINE_FIELD(double, kp_posture, 1.0)
-      DEFINE_FIELD(double, kp_base, 0.0)
-      DEFINE_FIELD(double, kp_contact, 0.0)
+      DEFINE_FIELD(double, kp_posture, 10.0)
+      DEFINE_FIELD(double, kp_base, 10.0)
+      DEFINE_FIELD(double, kp_contact, 10.0)
 
       // Tasks weights
-      DEFINE_FIELD(double, w_posture, 1e2)
-      DEFINE_FIELD(double, w_base, 0.)
-      DEFINE_FIELD(double, w_contact_force, 0.)
-      DEFINE_FIELD(double, w_contact_motion, 0.)
+      DEFINE_FIELD(double, w_posture, 10)
+      DEFINE_FIELD(double, w_base, 10)
+      DEFINE_FIELD(double, w_contact_force, 10)
+      DEFINE_FIELD(double, w_contact_motion, 10)
 
       static Settings Default()
       {

tests/inverse_dynamics.cpp

@@ -22,7 +22,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),
-    {true, true, true, true}, Eigen::VectorXd::Zero(4 * 3));
+    {false, false, false, false}, Eigen::VectorXd::Zero(4 * 3));
 
   double t = 0;
   double dt = 1e-3;
@@ -56,4 +56,50 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
   }
 }
 
+BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
+{
+  RobotModelHandler model_handler = getSoloHandler();
+  RobotDataHandler data_handler(model_handler);
+
+  KinodynamicsID solver(model_handler, KinodynamicsID::Settings::Default());
+
+  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;
+
+  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;
+  double dt = 1e-3;
+  Eigen::VectorXd q = pinocchio::randomConfiguration(model_handler.getModel());
+  q.head(7) = q_target.head(7);
+  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);
+
+  Eigen::VectorXd error = 1e12 * Eigen::VectorXd::Ones(model_handler.getModel().nv);
+
+  for (int i = 0; i < 1000; i++)
+  {
+    // Solve and get solution
+    solver.solve(t, q, v, tau);
+    a = solver.getAccelerations();
+
+    // Integrate
+    t += dt;
+    q = pinocchio::integrate(model_handler.getModel(), q, (v + a / 2. * dt) * dt);
+    v += a * dt;
+
+    // Check error is decreasing
+    Eigen::VectorXd new_error = pinocchio::difference(model_handler.getModel(), q, q_target);
+    BOOST_CHECK_LE(new_error.norm(), error.norm());
+    error = new_error;
+  }
+}
+
 BOOST_AUTO_TEST_SUITE_END()