Implement cubic spline class

CHANGELOG.md

@@ -3,6 +3,8 @@ All notable changes to this project are documented in this file.
 
 ## [Unreleased]
 ### Added
+- Implement `CubicSpline` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/344)
+
 ### Changed
 ### Fix
 

src/Planners/CMakeLists.txt

@@ -8,8 +8,8 @@ if (FRAMEWORK_COMPILE_Planners)
 
   add_bipedal_locomotion_library(
     NAME                   Planners
-    PUBLIC_HEADERS         ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h ${H_PREFIX}/QuinticSpline.h ${H_PREFIX}/SO3Planner.h ${H_PREFIX}/SO3Planner.tpp ${H_PREFIX}/SwingFootPlanner.h
-    SOURCES                src/ConvexHullHelper.cpp  src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp src/QuinticSpline.cpp src/SwingFootPlanner.cpp
+    PUBLIC_HEADERS         ${H_PREFIX}/ConvexHullHelper.h ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h ${H_PREFIX}/Spline.h ${H_PREFIX}/QuinticSpline.h ${H_PREFIX}/SO3Planner.h ${H_PREFIX}/SO3Planner.tpp ${H_PREFIX}/SwingFootPlanner.h ${H_PREFIX}/CubicSpline.h
+    SOURCES                src/ConvexHullHelper.cpp  src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp src/QuinticSpline.cpp src/SwingFootPlanner.cpp src/CubicSpline.cpp
     PUBLIC_LINK_LIBRARIES  Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System BipedalLocomotion::Contacts
     PRIVATE_LINK_LIBRARIES Qhull::qhullcpp Qhull::qhullstatic_r casadi BipedalLocomotion::Math BipedalLocomotion::TextLogging
     INSTALLATION_FOLDER    Planners)

src/Planners/include/BipedalLocomotion/Planners/CubicSpline.h

@@ -0,0 +1,130 @@
+/**
+ * @file CubicSpline.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_PLANNERS_CUBIC_SPLINE_H
+#define BIPEDAL_LOCOMOTION_PLANNERS_CUBIC_SPLINE_H
+
+#include <memory>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/Planners/Spline.h>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+/**
+ * Cubic spline implement a 3-rd order polynomial spline in \$f\mathbb{R}^n\$f.
+ */
+class CubicSpline : public Spline
+{
+    /**
+     * Private implementation of the class
+     */
+    struct Impl;
+    std::unique_ptr<Impl> m_pimpl; /**< Private implementation */
+
+public:
+    /**
+     * Constructor.
+     */
+    CubicSpline();
+
+    /**
+     * Destructor.
+     * @note It is required by the PIMPL idiom.
+     */
+    ~CubicSpline();
+
+    /**
+     * Set the time step of the advance interface.
+     * @warning if the the time step is not set the user cannot use the advance features.
+     * @param dt the time step of the advance block.
+     * @return True in case of success, false otherwise.
+     */
+    bool setAdvanceTimeStep(const double& dt) final;
+
+    /**
+     * Set the knots of the spline.
+     * @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.
+     */
+    bool setKnots(const std::vector<Eigen::VectorXd>& position, //
+                  const std::vector<double>& time) final;
+
+    /**
+     * Set the initial condition of the spline
+     * @param velocity initial velocity (i.e. first derivative).
+     * @param acceleration initial acceleration (i.e. second derivative).
+     * @note the acceleration is not considered in the spline evaluation
+     * @return True in case of success, false otherwise.
+     */
+    bool setInitialConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
+                              Eigen::Ref<const Eigen::VectorXd> acceleration) final;
+
+    /**
+     * Set the final condition of the spline
+     * @param velocity final velocity (i.e. first derivative).
+     * @param acceleration final acceleration (i.e. second derivative).
+     * @note the acceleration is not considered in the spline evaluation
+     * @return True in case of success, false otherwise.
+     */
+    bool setFinalConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
+                            Eigen::Ref<const Eigen::VectorXd> acceleration) final;
+
+    /**
+     * Evaluate the spline at a given point
+     * @param t instant time
+     * @param position position at time t
+     * @param velocity velocity at time t
+     * @param acceleration acceleration at time t
+     * @return True in case of success, false otherwise.
+     */
+    bool evaluatePoint(const double& t,
+                       Eigen::Ref<Eigen::VectorXd> position,
+                       Eigen::Ref<Eigen::VectorXd> velocity,
+                       Eigen::Ref<Eigen::VectorXd> acceleration) final;
+
+    /**
+     * Evaluate the spline at a given point
+     * @param t instant time
+     * @param state of the system
+     * @return True in case of success, false otherwise.
+     */
+    bool evaluatePoint(const double& t, SplineState& state) final;
+
+    /**
+     * Get the state of the system.
+     * @warning if the the time step of the advance is not set the user cannot use the advance
+     * features.
+     * @return a const reference of the requested object.
+     */
+    const SplineState& getOutput() const final;
+
+    /**
+     * Determines the validity of the object retrieved with get()
+     * @warning if the the time step of the advance is not set the user cannot use the advance
+     * features.
+     * @return True if the object is valid, false otherwise.
+     */
+    bool isOutputValid() const final;
+
+    /**
+     * Advance the internal state. This may change the value retrievable from get().
+     * @warning if the the time step of the advance is not set the user cannot use the advance
+     * features.
+     * @return True if the advance is successfull.
+     */
+    bool advance() final;
+};
+} // namespace Planners
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_CUBIC_SPLINE_H

src/Planners/include/BipedalLocomotion/Planners/QuinticSpline.h

@@ -8,29 +8,21 @@
 #ifndef BIPEDAL_LOCOMOTION_PLANNERS_QUINTIC_SPLINE_H
 #define BIPEDAL_LOCOMOTION_PLANNERS_QUINTIC_SPLINE_H
 
-#include <vector>
 #include <memory>
+#include <vector>
 
 #include <Eigen/Dense>
 
-#include <BipedalLocomotion/System/Source.h>
+#include <BipedalLocomotion/Planners/Spline.h>
 
 namespace BipedalLocomotion
 {
 namespace Planners
 {
-
-struct QuinticSplineState
-{
-    Eigen::VectorXd position;
-    Eigen::VectorXd velocity;
-    Eigen::VectorXd acceleration;
-};
-
 /**
  * Quintic spline implement a 5-th order polynomial spline in \$f\mathbb{R}^n\$f.
  */
-class QuinticSpline : public System::Source<QuinticSplineState>
+class QuinticSpline : public Spline
 {
     /**
      * Private implementation of the class
@@ -56,15 +48,16 @@ class QuinticSpline : public System::Source<QuinticSplineState>
      * @param dt the time step of the advance block.
      * @return True in case of success, false otherwise.
      */
-    bool setAdvanceTimeStep(const double& dt);
+    bool setAdvanceTimeStep(const double& dt) final;
 
     /**
      * Set the knots of the spline.
      * @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.
      */
-    bool setKnots(const std::vector<Eigen::VectorXd>& position, const std::vector<double>& time);
+    bool setKnots(const std::vector<Eigen::VectorXd>& position, //
+                  const std::vector<double>& time) final;
 
     /**
      * Set the initial condition of the spline
@@ -73,7 +66,7 @@ class QuinticSpline : public System::Source<QuinticSplineState>
      * @return True in case of success, false otherwise.
      */
     bool setInitialConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
-                              Eigen::Ref<const Eigen::VectorXd> acceleration);
+                              Eigen::Ref<const Eigen::VectorXd> acceleration) final;
 
     /**
      * Set the final condition of the spline
@@ -82,7 +75,7 @@ class QuinticSpline : public System::Source<QuinticSplineState>
      * @return True in case of success, false otherwise.
      */
     bool setFinalConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
-                            Eigen::Ref<const Eigen::VectorXd> acceleration);
+                            Eigen::Ref<const Eigen::VectorXd> acceleration) final;
 
     /**
      * Evaluate the spline at a given point
@@ -95,24 +88,23 @@ class QuinticSpline : public System::Source<QuinticSplineState>
     bool evaluatePoint(const double& t,
                        Eigen::Ref<Eigen::VectorXd> position,
                        Eigen::Ref<Eigen::VectorXd> velocity,
-                       Eigen::Ref<Eigen::VectorXd> acceleration);
+                       Eigen::Ref<Eigen::VectorXd> acceleration) final;
 
     /**
      * Evaluate the spline at a given point
      * @param t instant time
      * @param state of the system
      * @return True in case of success, false otherwise.
      */
-    bool evaluatePoint(const double& t,
-                       QuinticSplineState& state);
+    bool evaluatePoint(const double& t, SplineState& state) final;
 
     /**
      * Get the state of the system.
      * @warning if the the time step of the advance is not set the user cannot use the advance
      * features.
      * @return a const reference of the requested object.
      */
-    const QuinticSplineState& getOutput() const final;
+    const SplineState& getOutput() const final;
 
     /**
      * Determines the validity of the object retrieved with get()
@@ -129,7 +121,6 @@ class QuinticSpline : public System::Source<QuinticSplineState>
      * @return True if the advance is successfull.
      */
     bool advance() final;
-
 };
 } // namespace Planners
 } // namespace BipedalLocomotion

src/Planners/include/BipedalLocomotion/Planners/Spline.h

@@ -0,0 +1,104 @@
+/**
+ * @file Spline.h
+ * @authors Giulio Romualdi
+ * @copyright 2020. 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_PLANNERS_SPLINE_H
+#define BIPEDAL_LOCOMOTION_PLANNERS_SPLINE_H
+
+#include <memory>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include <BipedalLocomotion/System/Source.h>
+
+namespace BipedalLocomotion
+{
+namespace Planners
+{
+
+struct SplineState
+{
+    Eigen::VectorXd position;
+    Eigen::VectorXd velocity;
+    Eigen::VectorXd acceleration;
+};
+
+/**
+ * spline implement an interface for a Spline in \f$\mathbb{R}^n\f$
+ */
+class Spline : public System::Source<SplineState>
+{
+public:
+    /**
+     * Destructor.
+     */
+    virtual ~Spline() = default;
+
+    /**
+     * Set the time step of the advance interface.
+     * @warning if the the time step is not set the user cannot use the advance features.
+     * @param dt the time step of the advance block.
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool setAdvanceTimeStep(const double& dt) = 0;
+
+    /**
+     * Set the knots of the spline.
+     * @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.
+     */
+    virtual bool setKnots(const std::vector<Eigen::VectorXd>& position, //
+                          const std::vector<double>& time)
+        = 0;
+
+    /**
+     * Set the initial condition of the spline
+     * @param velocity initial velocity (i.e. first derivative).
+     * @param acceleration initial acceleration (i.e. second derivative).
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool setInitialConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
+                                      Eigen::Ref<const Eigen::VectorXd> acceleration)
+        = 0;
+
+    /**
+     * Set the final condition of the spline
+     * @param velocity final velocity (i.e. first derivative).
+     * @param acceleration final acceleration (i.e. second derivative).
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool setFinalConditions(Eigen::Ref<const Eigen::VectorXd> velocity,
+                                    Eigen::Ref<const Eigen::VectorXd> acceleration)
+        = 0;
+
+    /**
+     * Evaluate the spline at a given point
+     * @param t instant time
+     * @param position position at time t
+     * @param velocity velocity at time t
+     * @param acceleration acceleration at time t
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool evaluatePoint(const double& t,
+                               Eigen::Ref<Eigen::VectorXd> position,
+                               Eigen::Ref<Eigen::VectorXd> velocity,
+                               Eigen::Ref<Eigen::VectorXd> acceleration)
+        = 0;
+
+    /**
+     * Evaluate the spline at a given point
+     * @param t instant time
+     * @param state of the system
+     * @return True in case of success, false otherwise.
+     */
+    virtual bool evaluatePoint(const double& t, SplineState& state) = 0;
+};
+} // namespace Planners
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_PLANNERS_SPLINE_H