From eb6a258ad558783fcf72f968daff9baa20d544d5 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Dec 2023 17:01:15 +0100 Subject: [PATCH 1/3] Implement LinearSpline in Math component --- .../BipedalLocomotion/bindings/Math/Spline.h | 1 + bindings/python/Math/src/Module.cpp | 1 + bindings/python/Math/src/Spline.cpp | 13 +++ src/Math/CMakeLists.txt | 2 +- .../BipedalLocomotion/Math/LinearSpline.h | 72 ++++++++++++ src/Math/tests/SplineTest.cpp | 110 ++++++++++++++++++ 6 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 src/Math/include/BipedalLocomotion/Math/LinearSpline.h diff --git a/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Spline.h b/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Spline.h index b87039ec99..5099027f89 100644 --- a/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Spline.h +++ b/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); diff --git a/bindings/python/Math/src/Module.cpp b/bindings/python/Math/src/Module.cpp index 0dab0d017f..4421afc8d4 100644 --- a/bindings/python/Math/src/Module.cpp +++ b/bindings/python/Math/src/Module.cpp @@ -27,6 +27,7 @@ void CreateModule(pybind11::module& module) CreateSchmittTrigger(module); CreateWrench(module, "d"); CreateSpline(module); + CreateLinearSpline(module); CreateCubicSpline(module); CreateQuinticSpline(module); } diff --git a/bindings/python/Math/src/Spline.cpp b/bindings/python/Math/src/Spline.cpp index 30904a42c5..c86ab11e2e 100644 --- a/bindings/python/Math/src/Spline.cpp +++ b/bindings/python/Math/src/Spline.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -79,6 +80,13 @@ template void CreateSplineTmp(pybind11::module& module, const std:: py::arg("acceleration")); } +template void CreateLinearSplineTmp(pybind11::module& module, const std::string& name) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Math; + py::class_, Spline>(module, name.c_str()).def(py::init()); +} + template void CreateCubicSplineTmp(pybind11::module& module, const std::string& name) { namespace py = ::pybind11; @@ -102,6 +110,11 @@ void CreateSpline(pybind11::module& module) CreateSplineTmp(module, "Spline"); } +void CreateLinearSpline(pybind11::module& module) +{ + CreateLinearSplineTmp(module, "LinearSpline"); +} + void CreateCubicSpline(pybind11::module& module) { CreateCubicSplineTmp(module, "CubicSpline"); diff --git a/src/Math/CMakeLists.txt b/src/Math/CMakeLists.txt index e682c8d4e0..9e238d6ac6 100644 --- a/src/Math/CMakeLists.txt +++ b/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 diff --git a/src/Math/include/BipedalLocomotion/Math/LinearSpline.h b/src/Math/include/BipedalLocomotion/Math/LinearSpline.h new file mode 100644 index 0000000000..7b7c2e4071 --- /dev/null +++ b/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 +#include +#include + +#include +#include + +#include + +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(t) = a_0 + a_1 t + * \f] + * where \$t \in [0, T_i]\$ and \$T_i\$ is the duration of the i-th polynomial. + */ +template class LinearSpline : public Spline +{ + +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::Polynomial& polynomial) final; + + /** + * Compute the intermediate quantities. + * @note this function is called by the Spline::computeCoefficients() function. + */ + void computeIntermediateQuantities() final; +}; + +template +void LinearSpline::updatePolynomialCoefficients(typename Spline::Polynomial& poly) +{ + const double d = std::chrono::duration(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 void LinearSpline::computeIntermediateQuantities() +{ + return; +} + +} // namespace Math +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_MATH_LINEAR_SPLINE_H diff --git a/src/Math/tests/SplineTest.cpp b/src/Math/tests/SplineTest.cpp index 8123e6db77..6c695dc852 100644 --- a/src/Math/tests/SplineTest.cpp +++ b/src/Math/tests/SplineTest.cpp @@ -13,6 +13,7 @@ #include #include +#include #include 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 coefficients; + for (auto& coeff : coefficients) + { + coeff.setRandom(); + } + + std::vector knots; + std::vector time; + for (std::size_t i = 0; i < numberOfPoints; i++) + { + time.push_back(dT * i + initTime); + const double t = std::chrono::duration(time.back()).count(); + knots.push_back(coefficients[0] + coefficients[1] * t); + } + + const double initT = std::chrono::duration(initTime).count(); + const double finalT = std::chrono::duration(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 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(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(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 timeVector; + for (std::size_t i = 0; i < pointsToCheckNumber; i++) + { + timeVector.push_back(dTCheckPoints * i + initTime); + } + + std::vector positionVector, velocityVector, accelerationVector; + REQUIRE(spline.evaluateOrderedPoints(timeVector, // + positionVector, + velocityVector, + accelerationVector)); + + for (std::size_t i = 0; i < pointsToCheckNumber; i++) + { + double t = std::chrono::duration(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)); + } + } +} From f931b83fdc77f72d4a5d716170111090255435a1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Dec 2023 17:03:00 +0100 Subject: [PATCH 2/3] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1027c62ad9..4fd81ab127 100644 --- a/CHANGELOG.md +++ b/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) From 324136fa3ec022fc0beeb96c6f6d7c9e443b688e Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 14 Dec 2023 17:33:01 +0100 Subject: [PATCH 3/3] Fix typo in spline documentation --- src/Math/include/BipedalLocomotion/Math/CubicSpline.h | 6 +++--- src/Math/include/BipedalLocomotion/Math/LinearSpline.h | 6 +++--- src/Math/include/BipedalLocomotion/Math/QuinticSpline.h | 4 ++-- src/Math/include/BipedalLocomotion/Math/Spline.h | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Math/include/BipedalLocomotion/Math/CubicSpline.h b/src/Math/include/BipedalLocomotion/Math/CubicSpline.h index 3569b902b1..c9abc8caf4 100644 --- a/src/Math/include/BipedalLocomotion/Math/CubicSpline.h +++ b/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 class CubicSpline : public Spline { diff --git a/src/Math/include/BipedalLocomotion/Math/LinearSpline.h b/src/Math/include/BipedalLocomotion/Math/LinearSpline.h index 7b7c2e4071..6a2a9ffb49 100644 --- a/src/Math/include/BipedalLocomotion/Math/LinearSpline.h +++ b/src/Math/include/BipedalLocomotion/Math/LinearSpline.h @@ -22,12 +22,12 @@ namespace BipedalLocomotion namespace Math { /** - * Linearspline implements a 1-st order polynomial spline in \$f\mathbb{R}^n\$f. + * 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(t) = a_0 + a_1 t + * s_i(t) = a_i + b_i t * \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 class LinearSpline : public Spline { diff --git a/src/Math/include/BipedalLocomotion/Math/QuinticSpline.h b/src/Math/include/BipedalLocomotion/Math/QuinticSpline.h index 6a80b36959..c294cd377c 100644 --- a/src/Math/include/BipedalLocomotion/Math/QuinticSpline.h +++ b/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 class QuinticSpline : public Spline { diff --git a/src/Math/include/BipedalLocomotion/Math/Spline.h b/src/Math/include/BipedalLocomotion/Math/Spline.h index a3980cecea..587fab0753 100644 --- a/src/Math/include/BipedalLocomotion/Math/Spline.h +++ b/src/Math/include/BipedalLocomotion/Math/Spline.h @@ -71,7 +71,7 @@ template class Spline : public System::Source> /** * 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. */