Friction compensation

Author: edantec

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,31 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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;
+
+    void exposeFrictionCompensation()
+    {
+      bp::class_<FrictionCompensation>(
+        "FrictionCompensation", bp::init<const Model &, const long>(bp::args("self", "model", "actuation_size")))
+        .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_);
+    }
+
+  } // 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])
@@ -140,6 +143,9 @@
 
 qp = IDSolver(id_conf, model_handler.getModel())
 
+""" Friction """
+fcompensation = FrictionCompensation(model_handler.getModel(), nu)
+
 """ Initialize simulation"""
 device = BulletRobot(
     model_handler.getModel().names,
@@ -270,9 +276,10 @@
             mpc.getDataHandler().getData().M,
         )
 
-        device.execute(qp.solved_torque)
+        fcompensation.computeFriction(x_interp[nq + 6:], qp.solved_torque)
+        device.execute(fcompensation.corrected_torque)
 
-        u_multibody.append(copy.deepcopy(qp.solved_torque))
+        u_multibody.append(copy.deepcopy(fcompensation.corrected_torque))
         x_multibody.append(x_measured)
 
 force_FL = np.array(force_FL)

include/simple-mpc/friction-compensation.hpp

@@ -0,0 +1,56 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 actuation_size Dimension of torque
+     */
+    FrictionCompensation(const Model & model, const long actuation_size);
+
+    /**
+     * @brief Compute the torque correction due to friction and store it internally.
+     *
+     * @param[in] velocity Joint velocity
+     * @param[in] torque Joint torque
+     */
+    void computeFriction(const Eigen::VectorXd & velocity, const Eigen::VectorXd & torque);
+
+    // Sign function for internal computation
+    static double signFunction(double x);
+
+    // Internal torque with friction compensation
+    Eigen::VectorXd corrected_torque_;
+
+    // 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,37 @@
+#include "simple-mpc/friction-compensation.hpp"
+
+namespace simple_mpc
+{
+
+  FrictionCompensation::FrictionCompensation(const Model & model, const long actuation_size)
+  {
+    corrected_torque_.resize(actuation_size);
+    dry_friction_ = model.lowerDryFrictionLimit;
+    viscuous_friction_ = model.damping;
+  }
+
+  void FrictionCompensation::computeFriction(const Eigen::VectorXd & velocity, const Eigen::VectorXd & torque)
+  {
+    if (velocity.size() != corrected_torque_.size())
+    {
+      throw std::runtime_error("Velocity has wrong size");
+    }
+    if (torque.size() != corrected_torque_.size())
+    {
+      throw std::runtime_error("Torque has wrong size");
+    }
+    corrected_torque_ = torque + viscuous_friction_.cwiseProduct(velocity)
+                        + dry_friction_.cwiseProduct(velocity.unaryExpr(std::function(signFunction)));
+  }
+
+  double FrictionCompensation::signFunction(double x)
+  {
+    if (x > 0)
+      return 1.0;
+    else if (x == 0)
+      return 0.0;
+    else
+      return -1.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 = 12;
+  model.lowerDryFrictionLimit = Eigen::VectorXd::Constant(nu, .5);
+  model.damping = Eigen::VectorXd::Constant(nu, .05);
+
+  FrictionCompensation friction = FrictionCompensation(model, nu);
+
+  BOOST_CHECK_EQUAL(model.lowerDryFrictionLimit, 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.lowerDryFrictionLimit[i] * sgn + model.damping[i] * velocity[i];
+  }
+
+  BOOST_CHECK(friction.corrected_torque_.isApprox(ctorque));
+}
+
+BOOST_AUTO_TEST_SUITE_END()