Merge pull request #40 from Simple-Robotics/topic/friction

Friction compensation

Author: EtienneAr

bindings/CMakeLists.txt

@@ -13,6 +13,7 @@ set(
   expose-centroidal.cpp
   expose-fulldynamics.cpp
   expose-kinodynamics.cpp
+  expose-friction-compensation.cpp
 )
 
 Python3_add_library(${PY_NAME}_pywrap MODULE WITH_SOABI ${${PY_NAME}_SOURCES})

bindings/expose-friction-compensation.cpp

@@ -0,0 +1,38 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 2-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+
+#include <eigenpy/eigenpy.hpp>
+#include <pinocchio/multibody/fwd.hpp>
+
+#include "simple-mpc/friction-compensation.hpp"
+
+namespace simple_mpc
+{
+  namespace python
+  {
+    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 bool>(bp::args("self", "model", "with_free_flyer")))
+        .def("computeFriction", &computeFrictionProxy)
+        .add_property("dry_friction", &FrictionCompensation::dry_friction_)
+        .add_property("viscuous_friction", &FrictionCompensation::viscuous_friction_);
+    }
+
+  } // namespace python
+} // namespace simple_mpc

bindings/module.cpp

@@ -17,6 +17,7 @@ namespace simple_mpc::python
   void exposeMPC();
   void exposeIDSolver();
   void exposeIKIDSolver();
+  void exposeFrictionCompensation();
 
   /* PYTHON MODULE */
   BOOST_PYTHON_MODULE(simple_mpc_pywrap)
@@ -35,6 +36,7 @@ namespace simple_mpc::python
     exposeMPC();
     exposeIDSolver();
     exposeIKIDSolver();
+    exposeFrictionCompensation();
   }
 
 } // namespace simple_mpc::python

examples/go2_fulldynamics.py

@@ -1,6 +1,6 @@
 import numpy as np
 from bullet_robot import BulletRobot
-from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver
+from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation
 import example_robot_data as erd
 import pinocchio as pin
 import time
@@ -21,6 +21,9 @@
 model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
 data_handler = RobotDataHandler(model_handler)
 
+nq = model_handler.getModel().nq
+nv = model_handler.getModel().nv
+nu = nv - 6
 force_size = 3
 nk = len(model_handler.getFeetNames())
 gravity = np.array([0, 0, -9.81])
@@ -110,11 +113,10 @@
     "RL_foot": False,
     "RR_foot": False,
 }
-contact_phases = [contact_phase_quadru] * int(T_ds / 2)
+contact_phases = [contact_phase_quadru] * T_ds
 contact_phases += [contact_phase_lift_FL] * T_ss
 contact_phases += [contact_phase_quadru] * T_ds
 contact_phases += [contact_phase_lift_FR] * T_ss
-contact_phases += [contact_phase_quadru] * int(T_ds / 2)
 
 """ contact_phases = [contact_phase_quadru] * int(T_ds / 2)
 contact_phases += [contact_phase_lift] * T_ss
@@ -140,6 +142,9 @@
 
 qp = IDSolver(id_conf, model_handler.getModel())
 
+""" Friction """
+fcompensation = FrictionCompensation(model_handler.getModel(), True)
+
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -187,7 +192,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))
@@ -270,9 +275,11 @@
             mpc.getDataHandler().getData().M,
         )
 
-        device.execute(qp.solved_torque)
+        qp_torque = qp.solved_torque.copy()
+        friction_torque = fcompensation.computeFriction(x_interp[nq + 6:], qp_torque)
+        device.execute(friction_torque)
 
-        u_multibody.append(copy.deepcopy(qp.solved_torque))
+        u_multibody.append(copy.deepcopy(friction_torque))
         x_multibody.append(x_measured)
 
 force_FL = np.array(force_FL)

include/simple-mpc/friction-compensation.hpp

@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// BSD 2-Clause License
+//
+// Copyright (C) 2025, INRIA
+// Copyright note valid unless otherwise stated in individual files.
+// All rights reserved.
+///////////////////////////////////////////////////////////////////////////////
+#pragma once
+
+#include <pinocchio/fwd.hpp>
+// Include pinocchio first
+#include <Eigen/Dense>
+
+#include "simple-mpc/fwd.hpp"
+#include <pinocchio/multibody/model.hpp>
+
+namespace simple_mpc
+{
+  using namespace pinocchio;
+
+  /**
+   * @brief Class managing the friction compensation for torque
+   *
+   * It applies a compensation term to a torque depending on dry and
+   * viscuous frictions.
+   */
+  class FrictionCompensation
+  {
+  public:
+    /**
+     * @brief Construct a new Friction Compensation object
+     *
+     * @param model Pinocchio model containing friction coefficients
+       @param with_free_flyer Bool indicating if model has free flyer joint
+     */
+    FrictionCompensation(const Model & model, const bool with_free_flyer = true);
+
+    /**
+     * @brief Add friction correction to joint torque
+     *
+     * @param[in] velocity Joint velocity
+     * @param[in] torque Joint torque
+     */
+    void computeFriction(Eigen::Ref<const VectorXd> velocity, Eigen::Ref<VectorXd> torque);
+
+    // Sign function for internal computation
+    static double signFunction(double x);
+
+    // Actuation size
+    int nu_;
+
+    // Friction coefficients
+    Eigen::VectorXd dry_friction_;
+    Eigen::VectorXd viscuous_friction_;
+  };
+
+} // namespace simple_mpc

include/simple-mpc/fwd.hpp

@@ -38,6 +38,7 @@ namespace simple_mpc
   class OCPHandler;
   class IDSolver;
   class IKIDSolver;
+  class FrictionCompensation;
 
   /// EIGEN TYPEDEFS
 

src/friction-compensation.cpp

@@ -0,0 +1,38 @@
+#include "simple-mpc/friction-compensation.hpp"
+#include <iostream>
+namespace simple_mpc
+{
+
+  FrictionCompensation::FrictionCompensation(const Model & model, const bool with_free_flyer)
+  {
+    if (with_free_flyer)
+    {
+      // Ignore free flyer joint
+      nu_ = model.nv - 6;
+    }
+    else
+    {
+      // If no free flyer
+      nu_ = model.nv;
+    }
+    dry_friction_ = model.friction.tail(nu_);
+    viscuous_friction_ = model.damping.tail(nu_);
+  }
+
+  void FrictionCompensation::computeFriction(Eigen::Ref<const VectorXd> velocity, Eigen::Ref<VectorXd> torque)
+  {
+    if (velocity.size() != nu_)
+      throw std::runtime_error("Velocity has wrong size");
+    if (torque.size() != nu_)
+      throw std::runtime_error("Torque has wrong size");
+
+    torque += viscuous_friction_.cwiseProduct(velocity)
+              + dry_friction_.cwiseProduct(velocity.unaryExpr(std::function(signFunction)));
+  }
+
+  double FrictionCompensation::signFunction(double x)
+  {
+    return (x > 0) - (x < 0);
+  }
+
+} // namespace simple_mpc

tests/CMakeLists.txt

@@ -49,7 +49,14 @@ function(add_aligator_test name)
   _add_test_prototype(${name} "" ${PROJECT_NAME})
 endfunction()
 
-set(TEST_NAMES robot_handler problem mpc lowlevel)
+set(
+  TEST_NAMES
+  robot_handler
+  problem
+  mpc
+  lowlevel
+  friction
+)
 
 foreach(test_name ${TEST_NAMES})
   add_aligator_test(${test_name})

tests/friction.cpp

@@ -0,0 +1,42 @@
+
+#include <boost/test/unit_test.hpp>
+
+#include "simple-mpc/friction-compensation.hpp"
+#include "test_utils.cpp"
+
+BOOST_AUTO_TEST_SUITE(friction)
+
+using namespace simple_mpc;
+
+BOOST_AUTO_TEST_CASE(dry_viscuous_friction)
+{
+  Model model;
+
+  const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf";
+
+  pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model);
+  long nu = model.nv - 6;
+  model.friction.tail(nu) = Eigen::VectorXd::Constant(nu, .5);
+  model.damping.tail(nu) = Eigen::VectorXd::Constant(nu, .05);
+
+  FrictionCompensation friction = FrictionCompensation(model);
+
+  BOOST_CHECK_EQUAL(model.friction.tail(nu), friction.dry_friction_);
+  BOOST_CHECK_EQUAL(model.damping.tail(nu), friction.viscuous_friction_);
+
+  Eigen::VectorXd velocity = Eigen::VectorXd::Random(nu);
+  Eigen::VectorXd torque = Eigen::VectorXd::Random(nu);
+
+  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 + 6] * sgn + model.damping[i + 6] * velocity[i];
+  }
+
+  friction.computeFriction(velocity, torque);
+
+  BOOST_CHECK(torque.isApprox(ctorque));
+}
+
+BOOST_AUTO_TEST_SUITE_END()