Implement LinearizedFrictionCone class

CHANGELOG.md

@@ -15,6 +15,7 @@ All notable changes to this project are documented in this file.
 - Implement `RealSense` driver class as a part of `PerceptionCapture` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/165)
 - Implement `realsense-test` utility application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/165)
 - Implement the inverse kinematics component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/229)
+- Implement LinearizedFrictionCone class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/244)
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

src/Math/CMakeLists.txt

@@ -6,7 +6,7 @@ set(H_PREFIX include/BipedalLocomotion/Math)
 
 add_bipedal_locomotion_library(
   NAME                  Math
-  PUBLIC_HEADERS        ${H_PREFIX}/CARE.h ${H_PREFIX}/Constants.h
-  SOURCES               src/CARE.cpp
-  PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler
+  PUBLIC_HEADERS        ${H_PREFIX}/CARE.h ${H_PREFIX}/Constants.h ${H_PREFIX}/LinearizedFrictionCone.h
+  SOURCES               src/CARE.cpp  src/LinearizedFrictionCone.cpp
+  PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging
   SUBDIRECTORIES        tests)

src/Math/include/BipedalLocomotion/Math/LinearizedFrictionCone.h

@@ -0,0 +1,70 @@
+/**
+ * @file LinearizedFrictionCone.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_MATH_LINEARIZED_FRICTION_CONE_H
+#define BIPEDAL_LOCOMOTION_MATH_LINEARIZED_FRICTION_CONE_H
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+
+namespace BipedalLocomotion
+{
+namespace Math
+{
+
+/**
+ * LinearizedFrictionCone computes the polyhedral approximation of the friction cone. A point contact remains
+ * fixed with respect the environment if the contact force lies in a cone described by
+ * \f[
+ * f ^c \cdot n > 0 \quad | f ^t | \le \mu f^c \cdot n
+ * \f]
+ * where \f$ f^c \f$ is the contact force, \f$ n \f$ is the vector normal to the contact surface. \f$ f^t \f$ is
+ * the tangential force to the contact surface and \f$ \mu \f$ is the friction parameter.
+ * The LinearizedFrictionCone aims to compute the polyhedral approximation of \f$ | f ^t | \le \mu f^c \cdot n \f$
+ * by spitting the base of the cone into slices.
+ */
+class LinearizedFrictionCone
+{
+    Eigen::MatrixXd m_A;
+    Eigen::VectorXd m_b;
+
+    bool m_isIntialized{false}; /**< True if the class has been correctly initialize */
+
+public:
+
+    /**
+     * Initialize the continuous algebraic riccati equation solver.
+     * @param handler pointer to the parameter handler.
+     * @note The following parameters are required:
+     * |           Parameter Name        |    Type   |               Description               |
+     * |:-------------------------------:|:---------:|:---------------------------------------:|
+     * |        `number_of_slices`       |   `int`   |  Number of slices used to split 90 deg. |
+     * | `static_friction_coefficient`   | `double`  |        Static friction coefficient.     |
+     * @return true in case of success/false otherwise.
+     */
+    bool initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler);
+
+    /**
+     * Get the matrix A.
+     * @return the matrix A.
+     */
+    Eigen::Ref<const Eigen::MatrixXd> getA() const;
+
+    /**
+     * Get the vector B.
+     * @return the matrix B..
+     */
+    Eigen::Ref<const Eigen::VectorXd> getB() const;
+};
+
+} // namespace Math
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_MATH_LINEARIZED_FRICTION_CONE_H

src/Math/src/LinearizedFrictionCone.cpp

@@ -0,0 +1,110 @@
+/**
+ * @file LinearizedFrictionCone.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <cmath>
+
+#include <BipedalLocomotion/Math/LinearizedFrictionCone.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+using namespace BipedalLocomotion::Math;
+
+bool LinearizedFrictionCone::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler)
+{
+    constexpr auto errorPrefix = "[LinearizedFrictionCone::initialize]";
+
+    auto ptr = handler.lock();
+    if (ptr == nullptr)
+    {
+        log()->error("{} Invalid parameter handler.", errorPrefix);
+        return false;
+    }
+
+    bool ok = true;
+    int numberOfSlices = -1;
+    ok = ok && ptr->getParameter("number_of_slices", numberOfSlices);
+    ok = ok && numberOfSlices > 0;
+
+    double staticFrictionCoefficient = -1;
+    ok = ok && ptr->getParameter("static_friction_coefficient", staticFrictionCoefficient);
+    ok = ok && staticFrictionCoefficient > 0;
+
+    if (!ok)
+    {
+        log()->error("{} Unable to retrieve all the parameters.", errorPrefix);
+        return false;
+    }
+
+    // split the friction cone into slices
+    const double segmentAngle = M_PI / (2 * numberOfSlices);
+    const int numberOfEquations = 4 * numberOfSlices;
+
+    m_A.resize(numberOfEquations, 3);
+    m_b = Eigen::VectorXd::Zero(numberOfEquations);
+
+    // evaluate friction cone constraint
+    std::vector<double> angles;
+    std::vector<double> pointsX;
+    std::vector<double> pointsY;
+
+    for (int i = 0; i < numberOfEquations; i++)
+    {
+        angles.push_back(i * segmentAngle);
+        pointsX.push_back(std::cos(angles.back()));
+        pointsY.push_back(std::sin(angles.back()));
+    }
+
+    double firstPointX, firstPointY, secondPointX, secondPointY;
+    for (int i = 0; i < numberOfEquations; i++)
+    {
+        firstPointX = pointsX[i];
+        firstPointY = pointsY[i];
+
+        secondPointX = pointsX[(i + 1) % numberOfEquations];
+        secondPointY = pointsY[(i + 1) % numberOfEquations];
+
+        const double angularCoefficients
+            = (secondPointY - firstPointY) / (secondPointX - firstPointX);
+        const double offset = firstPointY - angularCoefficients * firstPointX;
+
+        int inequalityFactor = 1;
+        if (angles[i] > M_PI || angles[(i + 1) % numberOfEquations] > M_PI)
+            inequalityFactor = -1;
+
+        //  A(i,:) = inequalityFactor.* [-angularCoefficients, 1, (-offsets*staticFrictionCoefficient)];
+        m_A.row(i) << -inequalityFactor * angularCoefficients, inequalityFactor,
+            -inequalityFactor * offset * staticFrictionCoefficient;
+    }
+
+    m_isIntialized = true;
+
+    return true;
+}
+
+Eigen::Ref<const Eigen::MatrixXd> LinearizedFrictionCone::getA() const
+{
+    constexpr auto error = "[LinearizedFrictionCone::getA] Please initialize the class before.";
+
+    if (!m_isIntialized)
+    {
+        log()->warn(error);
+        assert(m_isIntialized && error);
+    }
+
+    return m_A;
+}
+
+Eigen::Ref<const Eigen::VectorXd> LinearizedFrictionCone::getB() const
+{
+    constexpr auto error = "[LinearizedFrictionCone::getB] Please initialize the class before.";
+    if (!m_isIntialized)
+    {
+        log()->warn(error);
+        assert(m_isIntialized && error);
+    }
+
+    return m_b;
+}

src/Math/tests/CMakeLists.txt

@@ -7,3 +7,8 @@ add_bipedal_test(
   NAME CARE
   SOURCES CARETest.cpp
   LINKS BipedalLocomotion::Math)
+
+add_bipedal_test(
+  NAME LinearizedFrictionConeTest
+  SOURCES LinearizedFrictionConeTest.cpp
+  LINKS BipedalLocomotion::Math)

src/Math/tests/LinearizedFrictionConeTest.cpp

@@ -0,0 +1,40 @@
+/**
+ * @file LinearizedFrictionConeTest.cpp
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+// Catch2
+#include <catch2/catch.hpp>
+
+#include <BipedalLocomotion/Math/LinearizedFrictionCone.h>
+#include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
+
+using namespace BipedalLocomotion::Math;
+using namespace BipedalLocomotion::ParametersHandler;
+
+TEST_CASE("Linearized Friction Cone")
+{
+    auto params = std::make_shared<StdImplementation>();
+    params->setParameter("number_of_slices", 2);
+    params->setParameter("static_friction_coefficient", 0.3);
+
+    LinearizedFrictionCone cone;
+    REQUIRE(cone.initialize(params));
+
+    // test the solution
+    REQUIRE(cone.getB().isApprox(Eigen::VectorXd::Zero(8)));
+    Eigen::MatrixXd matlabSolution(8, 3);
+    matlabSolution << 2.4142,  1.0000, -0.7243,
+                      0.4142,  1.0000, -0.3000,
+                     -0.4142,  1.0000, -0.3000,
+                     -2.4142,  1.0000, -0.7243,
+                     -2.4142, -1.0000, -0.7243,
+                     -0.4142, -1.0000, -0.3000,
+                      0.4142, -1.0000, -0.3000,
+                      2.4142, -1.0000, -0.7243;
+
+    constexpr double tolerance = 1e-4;
+    REQUIRE(matlabSolution.isApprox(cone.getA(), tolerance));
+}