Take PR comments into account

Author: edantec

bindings/expose-friction-compensation.cpp

@@ -17,12 +17,20 @@ namespace simple_mpc
   {
     namespace bp = boost::python;
 
+    /* Eigen::VectorXd computeFrictionProxy(
+      FrictionCompensation & self,
+      Eigen::Ref<const VectorXd> velocity,
+      Eigen::Ref<VectorXd> torque) {
+        self.computeFriction(velocity, torque);
+
+        return torque;
+    } */
+
     void exposeFrictionCompensation()
     {
       bp::class_<FrictionCompensation>(
-        "FrictionCompensation", bp::init<const Model &, const long>(bp::args("self", "model", "actuation_size")))
+        "FrictionCompensation", bp::init<const Model &, const bool>(bp::args("self", "model", "with_free_flyer")))
         .def("computeFriction", &FrictionCompensation::computeFriction)
-        .add_property("corrected_torque", &FrictionCompensation::corrected_torque_)
         .add_property("dry_friction", &FrictionCompensation::dry_friction_)
         .add_property("viscuous_friction", &FrictionCompensation::viscuous_friction_);
     }

examples/go2_fulldynamics.py

@@ -65,13 +65,13 @@
     kinematics_limits=True,
     force_cone=False,
 )
-T = 50
+T = 100
 
 dynproblem = FullDynamicsOCP(problem_conf, model_handler)
 dynproblem.createProblem(model_handler.getReferenceState(), T, force_size, gravity[2], False)
 
-T_ds = 10
-T_ss = 30
+T_ds = 50
+T_ss = 10
 N_simu = int(0.01 / 0.001)
 mpc_conf = dict(
     ddpIteration=1,
@@ -114,9 +114,9 @@
     "RR_foot": False,
 }
 contact_phases = [contact_phase_quadru] * int(T_ds / 2)
-contact_phases += [contact_phase_lift_FL] * T_ss
+contact_phases += [contact_phase_lift] * T_ss
 contact_phases += [contact_phase_quadru] * T_ds
-contact_phases += [contact_phase_lift_FR] * T_ss
+contact_phases += [contact_phase_lift] * T_ss
 contact_phases += [contact_phase_quadru] * int(T_ds / 2)
 
 """ contact_phases = [contact_phase_quadru] * int(T_ds / 2)
@@ -193,7 +193,7 @@
 L_measured = []
 
 v = np.zeros(6)
-v[0] = 0.2
+v[0] = 0.
 mpc.velocity_base = v
 for t in range(500):
     print("Time " + str(t))
@@ -276,10 +276,11 @@
             mpc.getDataHandler().getData().M,
         )
 
-        fcompensation.computeFriction(x_interp[nq + 6:], qp.solved_torque)
-        device.execute(fcompensation.corrected_torque)
+        qp_torque = qp.solved_torque.copy()
+        corrected_torque = fcompensation.computeFriction(x_interp[nq + 6:], qp_torque)
+        device.execute(corrected_torque)
 
-        u_multibody.append(copy.deepcopy(fcompensation.corrected_torque))
+        u_multibody.append(copy.deepcopy(corrected_torque))
         x_multibody.append(x_measured)
 
 force_FL = np.array(force_FL)

include/simple-mpc/friction-compensation.hpp

@@ -30,23 +30,24 @@ namespace simple_mpc
     /**
      * @brief Construct a new Friction Compensation object
      *
-     * @param actuation_size Dimension of torque
+     * @param model Pinocchio model containing friction coefficients
+       @param with_free_flyer Bool indicating if model has free flyer joint
      */
-    FrictionCompensation(const Model & model, const long actuation_size);
+    FrictionCompensation(const Model & model, const bool with_free_flyer = true);
 
     /**
-     * @brief Compute the torque correction due to friction and store it internally.
+     * @brief Add friction correction to joint torque
      *
      * @param[in] velocity Joint velocity
      * @param[in] torque Joint torque
      */
-    void computeFriction(const Eigen::VectorXd & velocity, const Eigen::VectorXd & torque);
+    void computeFriction(Eigen::Ref<const VectorXd> velocity, Eigen::Ref<VectorXd> torque);
 
     // Sign function for internal computation
     static double signFunction(double x);
 
-    // Internal torque with friction compensation
-    Eigen::VectorXd corrected_torque_;
+    // Actuation size
+    int nu_;
 
     // Friction coefficients
     Eigen::VectorXd dry_friction_;

src/friction-compensation.cpp

@@ -3,25 +3,28 @@
 namespace simple_mpc
 {
 
-  FrictionCompensation::FrictionCompensation(const Model & model, const long actuation_size)
+  FrictionCompensation::FrictionCompensation(const Model & model, const bool with_free_flyer)
   {
-    corrected_torque_.resize(actuation_size);
-    dry_friction_ = model.friction;
-    viscuous_friction_ = model.damping;
-  }
-
-  void FrictionCompensation::computeFriction(const Eigen::VectorXd & velocity, const Eigen::VectorXd & torque)
-  {
-    if (velocity.size() != corrected_torque_.size())
+    if (with_free_flyer)
     {
-      throw std::runtime_error("Velocity has wrong size");
+      // Ignore universe and root joints
+      nu_ = model.njoints - 2;
     }
-    if (torque.size() != corrected_torque_.size())
+    else
     {
-      throw std::runtime_error("Torque has wrong size");
+      // Ignore root joint
+      nu_ = model.njoints - 1;
     }
-    corrected_torque_ = torque + viscuous_friction_.cwiseProduct(velocity)
-                        + dry_friction_.cwiseProduct(velocity.unaryExpr(std::function(signFunction)));
+    dry_friction_ = model.friction.head(nu_);
+    viscuous_friction_ = model.damping.head(nu_);
+  }
+
+  void FrictionCompensation::computeFriction(Eigen::Ref<const VectorXd> velocity, Eigen::Ref<VectorXd> torque)
+  {
+    assert(("Velocity size must be equal to actuation size", velocity.size() == nu_));
+    assert(("Torque size must be equal to actuation size", torque.size() == nu_));
+    torque += viscuous_friction_.cwiseProduct(velocity)
+              + dry_friction_.cwiseProduct(velocity.unaryExpr(std::function(signFunction)));
   }
 
   double FrictionCompensation::signFunction(double x)

tests/friction.cpp

@@ -19,24 +19,24 @@ BOOST_AUTO_TEST_CASE(dry_viscuous_friction)
   model.friction = Eigen::VectorXd::Constant(nu, .5);
   model.damping = Eigen::VectorXd::Constant(nu, .05);
 
-  FrictionCompensation friction = FrictionCompensation(model, nu);
+  FrictionCompensation friction = FrictionCompensation(model);
 
   BOOST_CHECK_EQUAL(model.friction, friction.dry_friction_);
   BOOST_CHECK_EQUAL(model.damping, friction.viscuous_friction_);
 
   Eigen::VectorXd velocity = Eigen::VectorXd::Random(nu);
   Eigen::VectorXd torque = Eigen::VectorXd::Random(nu);
 
-  friction.computeFriction(velocity, torque);
-
   Eigen::VectorXd ctorque(nu);
   for (long i = 0; i < nu; i++)
   {
     double sgn = (velocity[i] > 0) - (velocity[i] < 0);
     ctorque[i] = torque[i] + model.friction[i] * sgn + model.damping[i] * velocity[i];
   }
 
-  BOOST_CHECK(friction.corrected_torque_.isApprox(ctorque));
+  friction.computeFriction(velocity, torque);
+
+  BOOST_CHECK(torque.isApprox(ctorque));
 }
 
 BOOST_AUTO_TEST_SUITE_END()