Remove default values for ID gain and weights

Author: earlaud

include/simple-mpc/inverse-dynamics.hpp

@@ -10,6 +10,15 @@
 #include <tsid/tasks/task-se3-equality.hpp>
 #include <tsid/trajectories/trajectory-euclidian.hpp>
 
+// Allow to define a field, it's default value and its convenient chainable setter in one keyword.
+#define DEFINE_FIELD(type, name, value)                                                                                \
+  type name = value;                                                                                                   \
+  Settings & set_##name(type v)                                                                                        \
+  {                                                                                                                    \
+    name = v;                                                                                                          \
+    return *this;                                                                                                      \
+  }
+
 namespace simple_mpc
 {
 
@@ -18,13 +27,6 @@ namespace simple_mpc
   public:
     struct Settings
     {
-#define DEFINE_FIELD(type, name, value)                                                                                \
-  type name = value;                                                                                                   \
-  Settings & set_##name(type v)                                                                                        \
-  {                                                                                                                    \
-    name = v;                                                                                                          \
-    return *this;                                                                                                      \
-  }
 
       // Physical quantities
       DEFINE_FIELD(double, friction_coefficient, 0.3)
@@ -38,23 +40,18 @@ 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_base, 10)
-      DEFINE_FIELD(double, kp_posture, 10)
-      DEFINE_FIELD(double, kp_contact, 10)
+      DEFINE_FIELD(double, kp_base, 0.)
+      DEFINE_FIELD(double, kp_posture, 0.)
+      DEFINE_FIELD(double, kp_contact, 0.)
 
       // Tasks weights
-      DEFINE_FIELD(double, w_base, 10.)
-      DEFINE_FIELD(double, w_posture, 1.0)
-      DEFINE_FIELD(double, w_contact_motion, 1.0)
-      DEFINE_FIELD(double, w_contact_force, 1.0)
-
-      static Settings Default()
-      {
-        return {};
-      } // Work-around c++ bug to have a default constructor of nested class
+      DEFINE_FIELD(double, w_base, -1.)           // Disabled by default
+      DEFINE_FIELD(double, w_posture, -1.)        // Disabled by default
+      DEFINE_FIELD(double, w_contact_motion, -1.) // Disabled by default
+      DEFINE_FIELD(double, w_contact_force, -1.)  // Disabled by default
     };
 
-    KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, const Settings settings = Settings::Default())
+    KinodynamicsID(const simple_mpc::RobotModelHandler & model_handler, const Settings settings)
     : settings_(settings)
     , model_handler_(model_handler)
     , data_handler_(model_handler_)

tests/inverse_dynamics.cpp

@@ -31,7 +31,7 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_postureTask)
   RobotDataHandler data_handler(model_handler);
 
   KinodynamicsID solver(
-    model_handler, KinodynamicsID::Settings::Default().set_w_base(0.).set_w_contact_force(0.).set_w_contact_motion(0.));
+    model_handler, KinodynamicsID::Settings().set_kp_posture(10.).set_w_posture(1.)); // only a posture task
 
   const Eigen::VectorXd q_target = model_handler.getReferenceState().head(model_handler.getModel().nq);
 
@@ -75,7 +75,15 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_contact)
   RobotModelHandler model_handler = getSoloHandler();
   RobotDataHandler data_handler(model_handler);
 
-  KinodynamicsID solver(model_handler);
+  KinodynamicsID solver(
+    model_handler, KinodynamicsID::Settings()
+                     .set_kp_base(10.)
+                     .set_kp_posture(10.0)
+                     .set_kp_contact(10.0)
+                     .set_w_base(10.)
+                     .set_w_posture(1.0)
+                     .set_w_contact_motion(1.0)
+                     .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);
@@ -132,7 +140,15 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
   RobotModelHandler model_handler = getSoloHandler();
   RobotDataHandler data_handler(model_handler);
 
-  KinodynamicsID solver(model_handler);
+  KinodynamicsID solver(
+    model_handler, 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_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);
@@ -154,7 +170,8 @@ BOOST_AUTO_TEST_CASE(KinodynamicsID_allTasks)
 
   Eigen::VectorXd error = 1e12 * Eigen::VectorXd::Ones(model_handler.getModel().nv);
 
-  for (int i = 0; i < 10000; i++)
+  const int N_STEP = 10000;
+  for (int i = 0; i < N_STEP; i++)
   {
     // Solve and get solution
     solver.solve(t, q, v, tau);