Implement LinearSpline in Math component

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 - 🤖 Add `ergoCubSN001` configuration files for the `balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776)
 - Implement `VectorsCollectionServer` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/776)
 - Implement `toString()` methods in `PlannedContact`, `ContactList`, `ContactPhase` and `ContactPhaseList` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/777)
+- Implement `LinearSpline` in `Math` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/782)
 
 ### Changed
 - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766)

bindings/python/Math/include/BipedalLocomotion/bindings/Math/Spline.h

@@ -18,6 +18,7 @@ namespace Math
 {
 
 void CreateSpline(pybind11::module& module);
+void CreateLinearSpline(pybind11::module& module);
 void CreateCubicSpline(pybind11::module& module);
 void CreateQuinticSpline(pybind11::module& module);
 

bindings/python/Math/src/Module.cpp

@@ -27,6 +27,7 @@ void CreateModule(pybind11::module& module)
     CreateSchmittTrigger(module);
     CreateWrench<double>(module, "d");
     CreateSpline(module);
+    CreateLinearSpline(module);
     CreateCubicSpline(module);
     CreateQuinticSpline(module);
 }

bindings/python/Math/src/Spline.cpp

@@ -15,6 +15,7 @@
 
 #include <BipedalLocomotion/Math/CubicSpline.h>
 #include <BipedalLocomotion/Math/QuinticSpline.h>
+#include <BipedalLocomotion/Math/LinearSpline.h>
 #include <BipedalLocomotion/Math/Spline.h>
 
 #include <BipedalLocomotion/bindings/Math/Spline.h>
@@ -79,6 +80,13 @@ template <typename T> void CreateSplineTmp(pybind11::module& module, const std::
              py::arg("acceleration"));
 }
 
+template <typename T> void CreateLinearSplineTmp(pybind11::module& module, const std::string& name)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::Math;
+    py::class_<LinearSpline<T>, Spline<T>>(module, name.c_str()).def(py::init());
+}
+
 template <typename T> void CreateCubicSplineTmp(pybind11::module& module, const std::string& name)
 {
     namespace py = ::pybind11;
@@ -102,6 +110,11 @@ void CreateSpline(pybind11::module& module)
     CreateSplineTmp<Eigen::VectorXd>(module, "Spline");
 }
 
+void CreateLinearSpline(pybind11::module& module)
+{
+    CreateLinearSplineTmp<Eigen::VectorXd>(module, "LinearSpline");
+}
+
 void CreateCubicSpline(pybind11::module& module)
 {
     CreateCubicSplineTmp<Eigen::VectorXd>(module, "CubicSpline");

src/Math/CMakeLists.txt

@@ -11,7 +11,7 @@ if(FRAMEWORK_COMPILE_Math)
     PUBLIC_HEADERS        ${H_PREFIX}/CARE.h ${H_PREFIX}/Constants.h
                           ${H_PREFIX}/LinearizedFrictionCone.h ${H_PREFIX}/ContactWrenchCone.h
                           ${H_PREFIX}/Wrench.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/QuadraticBezierCurve.h
-                          ${H_PREFIX}/Spline.h ${H_PREFIX}/CubicSpline.h ${H_PREFIX}/QuinticSpline.h
+                          ${H_PREFIX}/Spline.h ${H_PREFIX}/LinearSpline.h ${H_PREFIX}/CubicSpline.h ${H_PREFIX}/QuinticSpline.h
     SOURCES               src/CARE.cpp  src/LinearizedFrictionCone.cpp src/ContactWrenchCone.cpp
                           src/SchmittTrigger.cpp src/QuadraticBezierCurve.cpp
     PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler

src/Math/include/BipedalLocomotion/Math/CubicSpline.h

@@ -22,12 +22,12 @@ namespace BipedalLocomotion
 namespace Math
 {
 /**
- * Cubic spline implements a 3-rd order polynomial spline in \$f\mathbb{R}^n\$f.
+ * Cubic spline implements a 3-rd order polynomial spline in \f$\mathbb{R}^n\f$.
  * @note The spline is defined as a set of piecewise polynomial functions of degree 3 of the form
  * \f[
- * s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3
+ * s_i(t) = a_i + b_i t + c_i t^2 + d_i t^3
  * \f]
- * where \$t \in [0, T_i]\$ and \$T_i\$ is the duration of the i-th polynomial.
+ * where \f$t \in [0, T_i]\f$ and \f$T_i\f$ is the duration of the i-th polynomial.
  */
 template <typename T> class CubicSpline : public Spline<T>
 {

src/Math/include/BipedalLocomotion/Math/LinearSpline.h

@@ -0,0 +1,72 @@
+/**
+ * @file LinearSpline.h
+ * @authors Giulio Romualdi
+ * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_MATH_LINEAR_SPLINE_H
+#define BIPEDAL_LOCOMOTION_MATH_LINEAR_SPLINE_H
+
+#include <chrono>
+#include <memory>
+#include <vector>
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include <BipedalLocomotion/Math/Spline.h>
+
+namespace BipedalLocomotion
+{
+namespace Math
+{
+/**
+ * LinearSpline implements a 1-st order polynomial spline in \f$\mathbb{R}^n\f$.
+ * @note The spline is defined as a set of piecewise polynomial functions of degree 1 of the form
+ * \f[
+ * s_i(t) = a_i + b_i t
+ * \f]
+ * where \f$t \in [0, T_i]\f$ and \f$T_i\f$ is the duration of the i-th polynomial.
+ */
+template <typename T> class LinearSpline : public Spline<T>
+{
+
+private:
+    /**
+     * Update the polynomial coefficients.
+     * @param polynomial polynomial to be updated.
+     * @note this function is called by the Spline::computeCoefficients() function.
+     */
+    void updatePolynomialCoefficients(typename Spline<T>::Polynomial& polynomial) final;
+
+    /**
+     * Compute the intermediate quantities.
+     * @note this function is called by the Spline::computeCoefficients() function.
+     */
+    void computeIntermediateQuantities() final;
+};
+
+template <typename T>
+void LinearSpline<T>::updatePolynomialCoefficients(typename Spline<T>::Polynomial& poly)
+{
+    const double d = std::chrono::duration<double>(poly.duration).count();
+
+    const auto& x0 = poly.initialPoint->position;
+    const auto& xT = poly.finalPoint->position;
+
+    // resize the coefficients
+    poly.coefficients.resize(2);
+    poly.coefficients[0] = x0;
+    poly.coefficients[1] = (xT - x0) / d;
+}
+
+template <typename T> void LinearSpline<T>::computeIntermediateQuantities()
+{
+    return;
+}
+
+} // namespace Math
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_MATH_LINEAR_SPLINE_H

src/Math/include/BipedalLocomotion/Math/QuinticSpline.h

@@ -22,12 +22,12 @@ namespace BipedalLocomotion
 namespace Math
 {
 /**
- * Quintic spline implements a 5-th order polynomial spline in \$f\mathbb{R}^n\$f.
+ * Quintic spline implements a 5-th order polynomial spline in \f$\mathbb{R}^n\f$.
  * @note The spline is defined as a set piecewise polynomial functions of degree 5 of the form
  * \f[
  * s_i(t) = a_i + b_i t + c_i t^2 + d_i t^3 + e_i t^4 + f_i t^5
  * \f]
- * where \$t \in [0, T_i]\$ and \$T_i\$ is the duration of the i-th polynomial.
+ * where \f$t \in [0, T_i]\f$ and \f$T_i\f$ is the duration of the i-th polynomial.
  */
 template <typename T> class QuinticSpline : public Spline<T>
 {

src/Math/include/BipedalLocomotion/Math/Spline.h

@@ -71,7 +71,7 @@ template <typename T> class Spline : public System::Source<TrajectoryPoint<T>>
 
     /**
      * Set the knots of the spline.
-     * @param position position of the knots in \$f\mathbb{R}^n\$f.
+     * @param position position of the knots in \f$\mathbb{R}^n\f$.
      * @param time vector containing the time instant of the knots.
      * @return True in case of success, false otherwise.
      */

src/Math/tests/SplineTest.cpp

@@ -13,6 +13,7 @@
 #include <catch2/catch_test_macros.hpp>
 
 #include <BipedalLocomotion/Math/CubicSpline.h>
+#include <BipedalLocomotion/Math/LinearSpline.h>
 #include <BipedalLocomotion/Math/QuinticSpline.h>
 
 using namespace BipedalLocomotion::Math;
@@ -310,3 +311,112 @@ TEST_CASE("Cubic spline")
         }
     }
 }
+
+TEST_CASE("Linear spline")
+{
+    using namespace std::chrono_literals;
+
+    constexpr std::size_t numberOfPoints = 100;
+    constexpr std::chrono::nanoseconds initTime = 320ms;
+    constexpr std::chrono::nanoseconds finalTime = 2s + 640s;
+
+    constexpr std::chrono::nanoseconds dT = (finalTime - initTime) / (numberOfPoints - 1);
+    std::array<Eigen::Vector4d, 2> coefficients;
+    for (auto& coeff : coefficients)
+    {
+        coeff.setRandom();
+    }
+
+    std::vector<Eigen::Vector4d> knots;
+    std::vector<std::chrono::nanoseconds> time;
+    for (std::size_t i = 0; i < numberOfPoints; i++)
+    {
+        time.push_back(dT * i + initTime);
+        const double t = std::chrono::duration<double>(time.back()).count();
+        knots.push_back(coefficients[0] + coefficients[1] * t);
+    }
+
+    const double initT = std::chrono::duration<double>(initTime).count();
+    const double finalT = std::chrono::duration<double>(finalTime).count();
+
+    Eigen::Vector4d initVelocity = coefficients[1];
+    Eigen::Vector4d finalVelocity = coefficients[1];
+
+    Eigen::Vector4d initAcceleration = Eigen::Vector4d::Zero();
+    Eigen::Vector4d finalAcceleration = Eigen::Vector4d::Zero();
+
+    LinearSpline<Eigen::Vector4d> spline;
+    REQUIRE(spline.setKnots(knots, time));
+
+    REQUIRE(spline.setInitialConditions({initVelocity, initAcceleration}));
+    REQUIRE(spline.setFinalConditions({finalVelocity, finalAcceleration}));
+
+    constexpr std::size_t pointsToCheckNumber = 1e3;
+
+    constexpr std::chrono::nanoseconds dTCheckPoints = (finalTime - initTime) / pointsToCheckNumber;
+
+    Eigen::Vector4d expected, position, velocity, acceleration;
+
+    for (std::size_t i = 0; i < pointsToCheckNumber; i++)
+    {
+        std::chrono::nanoseconds t = dTCheckPoints * i + initTime;
+        double tSeconds = std::chrono::duration<double>(t).count();
+
+        REQUIRE(spline.evaluatePoint(t, position, velocity, acceleration));
+
+        // check position
+        expected = coefficients[0] + coefficients[1] * tSeconds;
+
+        REQUIRE(expected.isApprox(position, 1e-5));
+    }
+
+    SECTION("Advance capabilities")
+    {
+        REQUIRE_FALSE(spline.isOutputValid());
+        REQUIRE(spline.setAdvanceTimeStep(dTCheckPoints));
+
+        for (std::size_t i = 0; i < pointsToCheckNumber; i++)
+        {
+            double t = std::chrono::duration<double>(dTCheckPoints * i + initTime).count();
+
+            // advance the spline
+            REQUIRE(spline.advance());
+            REQUIRE(spline.isOutputValid());
+            const auto& traj = spline.getOutput();
+
+            // check position
+            expected = coefficients[0] + coefficients[1] * t;
+
+            REQUIRE(expected.isApprox(traj.position, 1e-5));
+        }
+    }
+
+    SECTION("Query from a vector of times")
+    {
+        std::vector<std::chrono::nanoseconds> timeVector;
+        for (std::size_t i = 0; i < pointsToCheckNumber; i++)
+        {
+            timeVector.push_back(dTCheckPoints * i + initTime);
+        }
+
+        std::vector<Eigen::Vector4d> positionVector, velocityVector, accelerationVector;
+        REQUIRE(spline.evaluateOrderedPoints(timeVector, //
+                                             positionVector,
+                                             velocityVector,
+                                             accelerationVector));
+
+        for (std::size_t i = 0; i < pointsToCheckNumber; i++)
+        {
+            double t = std::chrono::duration<double>(dTCheckPoints * i + initTime).count();
+
+            // check position
+            expected = coefficients[0] + coefficients[1] * t;
+
+            REQUIRE(expected.isApprox(positionVector[i], 1e-5));
+
+            // check velocity
+            expected = coefficients[1];
+            REQUIRE(expected.isApprox(velocityVector[i], 1e-5));
+        }
+    }
+}