-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #344 from dic-iit/feature/cubic_spline
Implement cubic spline class
- Loading branch information
Showing
11 changed files
with
1,111 additions
and
63 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
130 changes: 130 additions & 0 deletions
130
src/Planners/include/BipedalLocomotion/Planners/CubicSpline.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
104 changes: 104 additions & 0 deletions
104
src/Planners/include/BipedalLocomotion/Planners/Spline.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
Oops, something went wrong.