diff --git a/CHANGELOG.md b/CHANGELOG.md index dd03758593..bd9f6a43d2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,8 @@ All notable changes to this project are documented in this file. - ๐Ÿค– [ergoCubSN000] Clean the mas remapper files of the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/673) - ๐Ÿค– [ergoCubSN000] Enable the logging of the realsense camera `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/672) - Add the possibility to force the internal state of the `SchmittTrigger` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/684) +- Add the possibility to update the contact list in the swing foot planner when the contact is not active and the new orientation is different from the previous one (https://github.com/ami-iit/bipedal-locomotion-framework/pull/688) +- Add the possibility to set the boundary condition for the velocity and acceleration of the `SO3Planner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/688) ### Fixed - Fix RobotDynamicsEstimator compilation dependencies (https://github.com/ami-iit/bipedal-locomotion-framework/pull/665) diff --git a/docs/config.toml b/docs/config.toml index 25df53931d..be6837659b 100644 --- a/docs/config.toml +++ b/docs/config.toml @@ -1,6 +1,6 @@ [master] main_page = "docs/pages/main-page.dox" -additional_pages = ["docs/pages/python-additional-info.md"] +additional_pages = ["docs/pages/python-additional-info.md", "docs/pages/so3-minjerk.md"] src_folder = "src" ['v0.13.0'] diff --git a/docs/pages/main-page.dox b/docs/pages/main-page.dox index 73f890b48c..0a787fa5dc 100644 --- a/docs/pages/main-page.dox +++ b/docs/pages/main-page.dox @@ -6,6 +6,9 @@ \brief The **bipedal-locomotion-framework** project is a _suite_ of libraries for achieving bipedal locomotion on humanoid robots. +\section interesting-math-notes ๐Ÿ“š Interesting Math Notes +What follows is a list of interesting math notes that can be usefull to fully understand the usage of the library +- \ref so3-minjerk \section mandatory-dependencies ๐Ÿ“„ Mandatory dependencies The **bipedal-locomotion-framework** project is versatile and can be used to compile only some components. diff --git a/docs/pages/so3-minjerk.md b/docs/pages/so3-minjerk.md new file mode 100644 index 0000000000..caa2999e28 --- /dev/null +++ b/docs/pages/so3-minjerk.md @@ -0,0 +1,134 @@ +# ๐Ÿงฎ SO3 Minimum jerk trajectory {#so3-minjerk} + +## Introduction +Planning a minimum jerk trajectory in SO(3) can be a complex task. In this document, we aim to demonstrate the capabilities of the **bipedal-locomotion-framework** for generating minimum jerk trajectories in SO(3). The document is structured as follows: we first provide an overview of the mathematical foundations behind the planner, followed by a simple example illustrating its usage. Finally, we recommend some interesting readings for readers seeking more in-depth explanations and rigorous proofs. + +## ๐Ÿ“ Math +Given a fixed initial rotation \f$(t_0, R_0)\f$ and a final rotation \f$(t_f, R_f)\f$ and the associated left trivialized angular velocities, \f$(t_0, \omega_0)\f$ and \f$(t_f, \omega_f)\f$, we want to compute a trajectory \f$R : \mathbb{R}_+ \rightarrow SO(3)\f$ such that \f$R(t_0) = R_0\f$, \f$R(t_f) = R_f\f$, \f$\omega(t_0) = \omega_0\f$, \f$\omega(t_f) = \omega_f\f$, \f$\dot{\omega}(t_0) = \dot{\omega}_0\f$, \f$\dot{\omega}(t_f) = \dot{\omega}_f\f$ such that left trivialized angular jerk is minimized + +\f[ +\mathfrak{G} = \int_{t_0}^{t_f} \left({}^\mathcal{F} \ddot{\omega} _ {\mathcal{I}, F}^\top {}^\mathcal{F} \ddot{\omega} _ {\mathcal{I}, F} \right)\text{d} t. +\f] + +Following the work of [Zefran et al. "On the Generation of Smooth Three-Dimensional Rigid Body Motions"](https://doi.org/10.1109/70.704225) it is possible to prove that a trajectory \f$R(t)\f$ that satisfies the following equation is a minimum jerk trajectory in SO(3). + +\f[ +\begin{array}{ll} +\omega ^{(5)} & + 2 \omega \times \omega ^{(4)} \\ + & + \frac{4}{5} \omega \times (\omega \times \omega ^{(3)}) + \frac{5}{2} \dot{\omega} \times \omega^{(3)} \\ + & + \frac{1}{4} \omega \times (\omega \times (\omega \times \ddot{\omega})) \\ + & + \frac{3}{2} \omega \times (\dot{\omega} \times \ddot{\omega}) \\ + & - (\omega \times \ddot{\omega}) \times \dot{\omega} \\ + & - \frac{1}{4} (\omega \times \dot{\omega}) \times \ddot{\omega} \\ + & - \frac{3}{8} \omega \times ((\omega \times \dot{\omega}) \times \dot{\omega}) \\ + & - \frac{1}{8} (\omega \times (\omega \times \dot{\omega})) \times \dot{\omega} = 0. +\end{array} +\f] + +From now on we call this condition: **Minimum jerk trajectory condition**. + +It is worth noting that the above does not admit an analytic solution for arbitrary boundary conditions. +However, in the case of zero boundary velocity and acceleration or specific structure of the boundary condition, it is possible to show that +\f[ +R(t) = R_{0} \exp{\left(s(t-t_0) \log\left(R_0^\top R_f \right) \right)} \quad \quad s(\tau) = a_5 \tau^5 + a_4 \tau^4 + a_3 \tau^3 + a_2 \tau^2 + a_1 \tau + a_0 +\f] +satisfies condition the minimum jerk necessary condition. To prove the latest statement, we assume that \f$t_0 = 0\f$ and \f$t_1 = 1\f$, and then we compute the left trivialized angular velocity. The assumption can be easily removed with a simple change of variables. + +The angular velocity is given by + +\f[ +\begin{array}{ll} +\omega^\wedge &= R^\top \dot{R} \\ +&= \dot{s} \exp{\left(-s\log\left( R _ 0^\top R _ f \right) \right)}R_{0} ^\top R _ {0} \log\left( R _ 0^\top R _ f \right) \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \\ +&= \dot{s}\exp{\left(-s\log\left(R _ 0^\top R _ f \right) \right)} \log\left( R _ 0^\top R _ f \right) \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \\ +&= \dot{s}\exp{\left(-s\log\left(R _ 0^\top R _ f \right) \right)} \exp{\left(s\log\left( R _ 0^\top R _ f \right) \right)} \log\left( R _ 0^\top R _ f \right) \\ +&= \dot{s} \log\left( R _ 0^\top R _ f \right). +\end{array} +\f] + +In other words, the angular velocity will be always proportional to \f$\log\left( R_0^\top R_f \right)\f$. + +This means that we can ask for an initial and final angular velocity that is linearly dependent on \f$\log\left(R_0^\top R_f \right)\f$. +Let us introduce \f$\omega^\wedge(0)\f$ and \f$\omega^\wedge(1)\f$ as + +\f[ +\omega^\wedge(0) = \lambda ^\omega _ 0 \log\left( R_0^\top R_f \right) \quad \omega^\wedge(1) = \lambda ^\omega _ 1 \log\left( R_0^\top R_f \right) +\f] + +similarly for the angular acceleration + +\f[ +\dot{\omega}^\wedge(0) = \lambda ^\alpha _ 0 \log\left( R_0^\top R_f \right) \quad \dot{\omega}^\wedge(1) = \lambda ^\alpha _ 1 \log\left( R_0^\top R_f \right) +\f] + +So combining the initial and the final boundary conditions we can write the following linear system. +\f[ +\begin{array}{ll} +s(0) &= 0 \\ +s(1) &= 1 \\ +\dot{s}(0) &= \lambda ^\omega _ 0 \\ +\dot{s}(1) &= \lambda ^\omega _ 1 \\ +\ddot{s}(0) &= \lambda ^\alpha _ 0 \\ +\ddot{s}(1) &= \lambda ^\alpha _ 1 +\end{array} +\f] + +Solving the above system we obtain + +\f[ +\begin{array}{ll} +a_0 &= 0 \\ +a_1 &= \lambda ^\omega _ 0 \\ +a_2 &= \lambda ^\alpha _ 0/2 \\ +a_3 &= \lambda ^\alpha _ 1/2 - (3 \lambda ^\alpha _ 0)/2 - 6 \lambda ^\omega _ 0 - 4 \lambda ^\omega _ 1 + 10 \\ +a_4 &= (3 \lambda ^\alpha _ 0)/2 - \lambda ^\alpha _ 1 + 8 \lambda ^\omega _ 0 + 7 \lambda ^\omega _ 1 - 15 \\ +a_5 &= \lambda ^\alpha _ 1/2 - \lambda ^\alpha _ 0/2 - 3 \lambda ^\omega _ 0 - 3 \lambda ^\omega _ 1 + 6 +\end{array} +\f] + +It is now easy to show that \f$\omega\f$ satisfies the minimum jerk condition indeed + +\f[ +\omega ^{(5)} = 0 +\f] + +and all the cross products vanish since the angular velocity and its derivatives are linearly dependent. + +## ๐Ÿ’ป How to use the planner + +The SO3 minimum jerk planner is provided in **bipedal-locomotion-framework** through a template class that allows the user to use the left or the right trivialized angular velocity + +```cpp +using namespace std::chrono_literals; + +manif::SO3d initialTransform = manif::SO3d::Random(); +manif::SO3d finalTransform = manif::SO3d::Random(); + +constexpr std::chrono::nanoseconds T = 1s; + +BipedalLocomotion::Planner::SO3PlannerInertial planner; +manif::SO3d::Tangent initialVelocity = (finalTransform * initialTransform.inverse()).log(); + +initialVelocity.coeffs() = initialVelocity.coeffs() * 2; +planner.setInitialConditions(initialVelocity, manif::SO3d::Tangent::Zero()); +planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero()); +planner.setRotations(initialTransform, finalTransform, T); + +manif::SO3d rotation, predictedRotation; +manif::SO3d::Tangent velocity, predictedVelocity; +manif::SO3d::Tangent acceleration; +planner.evaluatePoint(0.42s, rotation, velocity, acceleration); +``` + +## ๐Ÿ“– Interesting readings +The interested reader +can refer to the extensive literature, among which it is worth mentioning: + +- ลฝefran, M., Kumar, V., and Croke, C. (1998). _On the generation of smooth threedimensional rigid body motions_. IEEE Transactions on Robotics and Automation, +14(4):576โ€“589. +- Dubrovin, B. A., Fomenko, A. T., and Novikov, S. P. (1984). _Modern Geometry โ€” +Methods and Applications, volume 93_. Springer New York, New York, NY. +- Needham, T. (2021). _Visual Differential Geometry and Forms_. Princeton University +Press. +- Pressley, A. (2010). _Elementary Differential Geometry_. Springer London, London. +- Giulio Romualdi (2022) _Online Control of Humanoid Robot Locomotion_ Ph.D. Thesis. diff --git a/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.h b/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.h index f2911d6530..2417f632c7 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.h @@ -73,6 +73,45 @@ class SO3Planner : public System::Source SO3PlannerState m_state; /**< Current state of the planner. It is used by the advance capabilities. */ + /** + * Description of a 5-th polynomial. It contains the coefficients of the sub-trajectory. + */ + struct Polynomial + { + double a0; + double a1; + double a2; + double a3; + double a4; + double a5; + }; + + /** + * Struct containing the boundary condition + */ + struct BoundaryConditions + { + manif::SO3d::Tangent velocity; + manif::SO3d::Tangent acceleration; + bool isSet{false}; + }; + + BoundaryConditions initialCondition; /**< Initial condition */ + BoundaryConditions finalCondition; /**< Final condition */ + bool areCoefficientsComputed{false}; /**< If true the coefficients are computed and updated */ + Polynomial polynomial; + + /** + * This function is the entry point to compute the coefficients of the spline + */ + bool computeCoefficients(); + + /** + * Compute the distance given two rotations. It depends on the trivialization. + */ + static manif::SO3d::Tangent + computeDistance(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation); + public: /** * Set the rotation @@ -91,8 +130,7 @@ class SO3Planner : public System::Source * @param state state of the planner. * @return True in case of success/false otherwise. */ - bool evaluatePoint(const std::chrono::nanoseconds& time, - SO3PlannerState& state) const; + bool evaluatePoint(const std::chrono::nanoseconds& time, SO3PlannerState& state); /** * Get the trajectory at a given time @@ -104,12 +142,11 @@ class SO3Planner : public System::Source * (accordingly to the trivialization used). * @return True in case of success/false otherwise. */ - template + template bool evaluatePoint(const std::chrono::nanoseconds& time, manif::SO3d& rotation, manif::SO3TangentBase& velocity, - manif::SO3TangentBase& acceleration) const; - + manif::SO3TangentBase& acceleration); /** * Set the time step of the advance interface. @@ -119,6 +156,24 @@ class SO3Planner : public System::Source */ bool setAdvanceTimeStep(const std::chrono::nanoseconds& dt); + /** + * Set the initial condition of the spline + * @param velocity initial angular velocity. + * @param acceleration initial angular acceleration. + * @return True in case of success, false otherwise. + */ + bool setInitialConditions(const manif::SO3d::Tangent& velocity, + const manif::SO3d::Tangent& acceleration); + + /** + * Set the final condition of the trajectory generator + * @param velocity final angular velocity. + * @param acceleration final angular acceleration. + * @return True in case of success, false otherwise. + */ + bool setFinalConditions(const manif::SO3d::Tangent& velocity, + const manif::SO3d::Tangent& acceleration); + /** * Get the state of the system. * @warning if the the time step of the advance is not set the user cannot use the advance @@ -142,6 +197,21 @@ class SO3Planner : public System::Source * @return True if the advance is successfull. */ bool advance() final; + + /** + * Project the tangent vector on the vector that represents the distance between the two + * rotations. + * @param initialRotation initial rotation \f${}^{\mathcal{I}}R_{\mathcal{B}_0}\f$. + * @param finalRotation final rotation \f${}^{\mathcal{I}}R_{\mathcal{B}_T}\f$. + * @param vector Tangent vector that needs to be projected. + * @return The projected tangent vector. + * @warning If the initial and final rotations coincides the projected tangent vector will be + * set equal to zero. + * @note this method can be used along with setInitialConditions and setFinalConditions. + */ + static manif::SO3d::Tangent projectTangentVector(const manif::SO3d& initialRotation, + const manif::SO3d& finalRotation, + const manif::SO3d::Tangent& vector); }; /** diff --git a/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.tpp b/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.tpp index 915b64f2b8..f9bd68999a 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.tpp +++ b/src/Planners/include/BipedalLocomotion/Planners/SO3Planner.tpp @@ -8,8 +8,8 @@ #ifndef BIPEDAL_LOCOMOTION_PLANNERS_SO3_PLANNER_TPP #define BIPEDAL_LOCOMOTION_PLANNERS_SO3_PLANNER_TPP -#include #include +#include #include @@ -27,12 +27,109 @@ constexpr double durationToSeconds(const std::chrono::duration& d) return std::chrono::duration(d).count(); } +template +bool SO3Planner::computeCoefficients() +{ + constexpr double tolerance = 1e-3; + constexpr auto logPrefix = "[SO3Planner::computeCoefficients]"; + + // no need to compute the coefficients again + if (areCoefficientsComputed) + { + return true; + } + + if (!this->initialCondition.isSet) + { + log()->error("{} The initial boundary condition is not set.", logPrefix); + return false; + } + + if (!this->finalCondition.isSet) + { + log()->error("{} The final boundary condition is not set.", logPrefix); + return false; + } + + // in the following function we check that vector anc rotationError are linear dependent and we + // compute the coefficient such that "vector = coefficient * rotationError". + auto findVectorCoefficient = [tolerance, logPrefix](const manif::SO3d::Tangent& rotationError, + const manif::SO3d::Tangent& vector, + double& coefficient) -> bool { + if (rotationError.coeffs().isZero()) + { + coefficient = 0; + return true; + } + + if (vector.coeffs().isZero()) + { + coefficient = 0; + return true; + } + + const double dotProduct = rotationError.coeffs().dot(vector.coeffs()); + const double rotationErrorNorm = rotationError.coeffs().norm(); + const double vectorNorm = vector.coeffs().norm(); + + const double cosOfAngleBetweenTwoVectors = dotProduct / (rotationErrorNorm * vectorNorm); + constexpr double cosOfTwoParallelVectors = 1; + // the cosine of the angle of two parallel vector can be 1 or -1 for this reason we compute + // the absolute value + if (std::abs(std::abs(cosOfAngleBetweenTwoVectors) - cosOfTwoParallelVectors) > tolerance) + { + return false; + } + + for (int i = 0; i < rotationError.coeffs().size(); i++) + { + const double tmp = vector.coeffs()[i] / rotationError.coeffs()[i]; + if (!std::isinf(tmp) && !std::isnan(tmp)) + { + coefficient = tmp; + return true; + } + } + + return false; + }; + + // check if the boundary conditions are linear independent with the error between the two + // rotation + double kv0, kv1, ka0, ka1; + if (!findVectorCoefficient(m_distance, initialCondition.velocity, kv0) + || !findVectorCoefficient(m_distance, initialCondition.acceleration, ka0) + || !findVectorCoefficient(m_distance, finalCondition.velocity, kv1) + || !findVectorCoefficient(m_distance, finalCondition.acceleration, ka1)) + { + log()->error("{} The velocity and the boundary acceleration must be a linear dependent to " + "the vector representing the axis between the two rotations.", + logPrefix); + return false; + } + + const double T = durationToSeconds(m_T); + + polynomial.a0 = 0; + polynomial.a1 = kv0; + polynomial.a2 = ka0 / 2.0; + polynomial.a3 = -(12 * T * kv0 + 8 * T * kv1 + 3 * T * T * ka0 - T * T * ka1 - 20) // + / (2 * T * T * T); + polynomial.a4 = (16 * T * kv0 + 14 * T * kv1 + 3 * T * T * ka0 - 2 * T * T * ka1 - 30) + / (2 * T * T * T * T); + polynomial.a5 = -(6 * T * kv0 + 6 * T * kv1 + T * T * ka0 - T * T * ka1 - 12) // + / (2 * T * T * T * T * T); + + areCoefficientsComputed = true; + + return true; +} + template bool SO3Planner::setRotations(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation, const std::chrono::nanoseconds& duration) { - constexpr auto logPrefix = "[SO3Planner::setRotation]"; if (duration <= std::chrono::nanoseconds::zero()) @@ -43,15 +140,7 @@ bool SO3Planner::setRotations(const manif::SO3d& initialRotation m_initialRotation = initialRotation; m_T = duration; - - if constexpr (trivialization == LieGroupTrivialization::Right) - { - m_distance = (finalRotation * initialRotation.inverse()).log(); - } else - // Please read it as trivialization == LieGroupTrivialization::Left - { - m_distance = (initialRotation.inverse() * finalRotation).log(); - } + m_distance = this->computeDistance(initialRotation, finalRotation); // reset the advance current time m_advanceCurrentTime = std::chrono::nanoseconds::zero(); @@ -60,18 +149,42 @@ bool SO3Planner::setRotations(const manif::SO3d& initialRotation m_state.rotation = m_initialRotation; m_state.velocity.setZero(); m_state.acceleration.setZero(); + this->areCoefficientsComputed = false; return true; } +template +bool SO3Planner::setInitialConditions(const manif::SO3d::Tangent& velocity, + const manif::SO3d::Tangent& acceleration) +{ + this->initialCondition.velocity = velocity; + this->initialCondition.acceleration = acceleration; + this->initialCondition.isSet = true; + this->areCoefficientsComputed = false; + return true; +} + +template +bool SO3Planner::setFinalConditions(const manif::SO3d::Tangent& velocity, + const manif::SO3d::Tangent& acceleration) +{ + this->finalCondition.velocity = velocity; + this->finalCondition.acceleration = acceleration; + this->finalCondition.isSet = true; + this->areCoefficientsComputed = false; + return true; +} + template template bool SO3Planner::evaluatePoint(const std::chrono::nanoseconds& time, manif::SO3d& rotation, manif::SO3TangentBase& velocity, - manif::SO3TangentBase& acceleration) const + manif::SO3TangentBase& acceleration) { constexpr auto logPrefix = "[SO3Planner::evaluatePoint]"; + if (time < std::chrono::nanoseconds::zero() || time > m_T) { log()->error("{} The time has to be a real positive number between 0 and {}.", @@ -80,8 +193,13 @@ bool SO3Planner::evaluatePoint(const std::chrono::nanoseconds& t return false; } + if (!this->computeCoefficients()) + { + log()->error("{} Unable to compute the coefficients.", logPrefix); + return false; + } + const double t = durationToSeconds(time); - const double T = durationToSeconds(m_T); // Compute the displacement in the tangent space // - displacement_tangent = log(finalRotation * initialRotation.inverse()) * s(t) (in case of // Right @@ -91,10 +209,16 @@ bool SO3Planner::evaluatePoint(const std::chrono::nanoseconds& t // Trivialization) // You can find the computation of s in "Modern Robotics: Mechanics, Planning, and Control" // (Chapter 9.2) - // TODO (Giulio) avoid to use t / T you may use std::chrono::ratio - const double s = 10 * std::pow(t / T, 3) - - 15 * std::pow(t / T, 4) - + 6 * std::pow(t / T, 5); + const double t2 = t * t; + const double t3 = t * t2; + const double t4 = t * t3; + const double t5 = t * t4; + const double s = polynomial.a0 + // + polynomial.a1 * t + // + polynomial.a2 * t2 + // + polynomial.a3 * t3 + // + polynomial.a4 * t4 + // + polynomial.a5 * t5; const manif::SO3d::Tangent displacementTangent = m_distance * s; if constexpr (trivialization == LieGroupTrivialization::Right) @@ -110,28 +234,31 @@ bool SO3Planner::evaluatePoint(const std::chrono::nanoseconds& t // compute velocity (it is expressed in body / inertial frame accordingly to the chosen // trivialization) - // You can find the computation of sDot in "Modern Robotics: Mechanics, Planning, and Control" + // You can find the computation of dds in "Modern Robotics: Mechanics, Planning, and Control" // (Chapter 9.2) - const double sDot = 10 * 3 * std::pow(t, 2) / std::pow(T, 3) - - 15 * 4 * std::pow(t, 3) / std::pow(T, 4) - + 6 * 5 * std::pow(t, 4) / std::pow(T, 5); - velocity = m_distance * sDot; + const double ds = polynomial.a1 + // + 2 * polynomial.a2 * t + // + 3 * polynomial.a3 * t2 + // + 4 * polynomial.a4 * t3 + // + 5 * polynomial.a5 * t4; + velocity = m_distance * ds; // compute acceleration (it is expressed in body / inertial frame accordingly to the chosen // trivialization) - // You can find the computation of sDdot in "Modern Robotics: Mechanics, + // You can find the computation of dds in "Modern Robotics: Mechanics, // Planning, and Control" (Chapter 9.2) - const double sDdot = 10 * 3 * 2 * t / std::pow(T, 3) - - 15 * 4 * 3 * std::pow(t, 2) / std::pow(T, 4) - + 6 * 5 * 4 * std::pow(t, 3) / std::pow(T, 5); - acceleration = m_distance * sDdot; + const double dds = 2 * polynomial.a2 + // + 3 * 2 * polynomial.a3 * t + // + 4 * 3 * polynomial.a4 * t2 + // + 5 * 4 * polynomial.a5 * t3; + acceleration = m_distance * dds; return true; } template bool SO3Planner::evaluatePoint(const std::chrono::nanoseconds& time, - SO3PlannerState& state) const + SO3PlannerState& state) { return this->evaluatePoint(time, state.rotation, state.velocity, state.acceleration); } @@ -183,6 +310,39 @@ const SO3PlannerState& SO3Planner::getOutput() const return m_state; } +template +manif::SO3d::Tangent SO3Planner::computeDistance(const manif::SO3d& initialRotation, + const manif::SO3d& finalRotation) +{ + if constexpr (trivialization == LieGroupTrivialization::Right) + { + return (finalRotation * initialRotation.inverse()).log(); + } else + // Please read it as trivialization == LieGroupTrivialization::Left + { + return (initialRotation.inverse() * finalRotation).log(); + } +} + +template +manif::SO3d::Tangent +SO3Planner::projectTangentVector(const manif::SO3d& initialRotation, + const manif::SO3d& finalRotation, + const manif::SO3d::Tangent& vector) +{ + const manif::SO3d::Tangent distanceVector = computeDistance(initialRotation, finalRotation); + const double distance = distanceVector.coeffs().norm(); + if (distance < 1e-4) + { + return manif::SO3d::Tangent::Zero(); + } + + // here we project vector on the vector named distance + manif::SO3d::Tangent ret; + ret.coeffs() = distanceVector.coeffs().dot(vector.coeffs()) / distance * distanceVector.coeffs(); + return ret; +} + } // namespace Planners } // namespace BipedalLocomotion diff --git a/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h b/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h index 989a10d70a..9a8fee5c1f 100644 --- a/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h +++ b/src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h @@ -13,10 +13,10 @@ #include -#include #include -#include +#include #include +#include #include namespace BipedalLocomotion @@ -50,9 +50,12 @@ class SwingFootPlanner : public System::Source /** Current time of the planner in seconds */ std::chrono::nanoseconds m_currentTrajectoryTime{std::chrono::nanoseconds::zero()}; + /** Starting time of the active SO3 trajectory seconds */ + std::chrono::nanoseconds m_staringTimeOfCurrentSO3Traj{std::chrono::nanoseconds::zero()}; + Contacts::ContactList m_contactList; /**< List of the contacts */ - Contacts::ContactList::const_iterator m_currentContactPtr; /**< Pointer to the current contact. (internal - use) */ + Contacts::ContactList::const_iterator m_currentContactPtr; /**< Pointer to the current contact. + (internal use) */ SO3PlannerInertial m_SO3Planner; /**< Trajectory planner in SO(3) */ std::unique_ptr m_planarPlanner; /**< Trajectory planner for the x y coordinates of the @@ -86,23 +89,25 @@ class SwingFootPlanner : public System::Source bool createSE3Traj(Eigen::Ref initialPlanarVelocity, Eigen::Ref initialPlanarAcceleration, Eigen::Ref> initialVerticalVelocity, - Eigen::Ref> initialVerticalAcceleration); + Eigen::Ref> initialVerticalAcceleration, + const manif::SO3d::Tangent& initialAngularVelocity, + const manif::SO3d::Tangent& initialAngularAcceleration); public: /** * Initialize the planner. * @param handler pointer to the parameter handler. * @note the following parameters are required by the class - * | Parameter Name | Type | Description | Mandatory | - * |:----------------------------:|:--------:|:--------------------------------------------------------------------------------------------------------------------------------------------------------------:|:---------:| - * | `sampling_time` | `double` | Sampling time of the planner in seconds | Yes | - * | `step_height` | `double` | Height of the swing foot. It is not the maximum height of the foot. If apex time is 0.5 `step_height` is the maximum | Yes | - * | `foot_apex_time` | `double` | Number between 0 and 1 representing the foot apex instant. If 0 the apex happens at take off if 1 at touch down | Yes | - * | `foot_landing_velocity` | `double` | Landing vertical velocity (default value 0.0) | No | - * | `foot_landing_acceleration` | `double` | Landing vertical acceleration (default value 0.0) | No | - * | `foot_take_off_velocity` | `double` | Take-off vertical velocity (default value 0.0) | No | - * | `foot_take_off_acceleration` | `double` | Take-off vertical acceleration (default value 0.0) | No | - * | `interpolation_method` | `string` | Define the interpolation method for the trajectory of the position. Accepted parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No | + * | Parameter Name | Type | Description | Mandatory | + * |:----------------------------:|:--------:|:----------------------------------------------------------------------------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time of the planner in seconds | Yes | + * | `step_height` | `double` | Height of the swing foot. It is not the maximum height of the foot. If apex time is 0.5 `step_height` is the maximum | Yes | + * | `foot_apex_time` | `double` | Number between 0 and 1 representing the foot apex instant. If 0 the apex happens at take off if 1 at touch down | Yes | + * | `foot_landing_velocity` | `double` | Landing vertical velocity (default value 0.0) | No | + * | `foot_landing_acceleration` | `double` | Landing vertical acceleration (default value 0.0) | No | + * | `foot_take_off_velocity` | `double` | Take-off vertical velocity (default value 0.0) | No | + * | `foot_take_off_acceleration` | `double` | Take-off vertical acceleration (default value 0.0) | No | + * | `interpolation_method` | `string` | Define the interpolation method for the trajectory of the position. Accepted parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No | * @return True in case of success/false otherwise. */ bool initialize(std::weak_ptr handler) final; @@ -112,7 +117,7 @@ class SwingFootPlanner : public System::Source * @param contactList contains the list for a given contact. * @return true in case of success, false otherwise. * @note The contact list can be updated at run-time, i.e., when the planner is running. However - * the new contact list must satify a set of hypothesis. + * the new contact list must satisfy a set of hypothesis. * If the contact list stored in the class is empty, then it is the first time the * contact list is added to the planner. In this case we accept all kinds of ContactList * If the contact list is not empty, we check if it is possible to update the list. Given some @@ -122,8 +127,10 @@ class SwingFootPlanner : public System::Source * active contact at the same pose. * - If the contact is not active (swing phase) the next contact must satisfy the following two * hypothesis - * 1. the orientation of the next contact must be the same as the orientation of the next - * contact in the contact list stored in the class. + * 1. the final orientation may change still the error (in the tangent space) between the + * new orientation and the current one should be parallel to the current velocity and + * acceleration vectors. This is required to keep the SO3Planner problem still treatable + * online. This check is not done here since the SO3Planner will complain in case of issues. * 2. the impact time of the next contact must be the same as the one of the next contact in * the contact list stored in the class. */ @@ -139,19 +146,19 @@ class SwingFootPlanner : public System::Source * @brief Get the object. * @return a const reference of the requested object. */ - const SwingFootPlannerState& getOutput() const final; + const SwingFootPlannerState& getOutput() const final; /** * @brief Determines the validity of the object retrieved with get() * @return True if the object is valid, false otherwise. */ - bool isOutputValid() const final; + bool isOutputValid() const final; /** * @brief Advance the internal state. This may change the value retrievable from get(). * @return True if the advance is successfull. */ - bool advance() final; + bool advance() final; }; } // namespace Planners diff --git a/src/Planners/src/SwingFootPlanner.cpp b/src/Planners/src/SwingFootPlanner.cpp index 493426dae4..76ffad9339 100644 --- a/src/Planners/src/SwingFootPlanner.cpp +++ b/src/Planners/src/SwingFootPlanner.cpp @@ -157,7 +157,11 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList) // - Given the current time instant the both the original and the new contact list have an // active contact at the same position. // - If the contact is not active we have to check that: - // 1. the final orientation didn't change + // 1. the final orientation may change still the error (in the tangent space) between the + // new orientation and the current one should be parallel to the current velocity and + // acceleration vectors. This is required to keep the SO3Planner problem still treatable + // online. This check is not done here since the SO3Planner will complain in case of + // issues. // 2. the swing foot trajectory duration did not change // Here the case where the contact is active @@ -183,7 +187,12 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList) && newContactAtCurrentTime != contactList.lastContact()) { // we take the next contact of the new contact list and we check: - // 1. the final orientation didn't change + // 1. the final orientation may change still the error (in the tangent space) between the + // new orientation and the current one should be parallel to the current velocity and + // acceleration vectors. This is required to keep the SO3Planner problem still treatable + // online. This check is not done here since the SO3Planner will complain in case of + // issues. + // // 2. the swing foot trajectory duration didn't change auto newNextContactAtCurrentTime = std::next(newContactAtCurrentTime, 1); @@ -197,21 +206,19 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList) const auto newSwingFootTrajectoryDuration = newNextContactAtCurrentTime->activationTime - newContactAtCurrentTime->deactivationTime; - if (!newNextContactAtCurrentTime->pose.quat().isApprox( - nextContactAtCurrentTime->pose.quat()) - || newSwingFootTrajectoryDuration != originalSwingFootTrajectoryDuration) + if (newSwingFootTrajectoryDuration != originalSwingFootTrajectoryDuration) { log()->error("{} Failing to update the contact list. At the give time instant t = {} " - "the foot is swinging. In order to update the conatct list we ask: 1) the " - "final orientation didn't change. 2) the duration of the swing foot phase " - "do not change. In this case we have. New Orientation {}, original " - "orientation {}. New duration {}, original duration {}.", - logPrefix, - m_currentTrajectoryTime, - newNextContactAtCurrentTime->pose.quat().coeffs().transpose(), - nextContactAtCurrentTime->pose.quat().coeffs().transpose(), - newSwingFootTrajectoryDuration, - originalSwingFootTrajectoryDuration); + "the foot is swinging. In order to update the conatct list we ask. The " + "duration of the swing foot phase do not change. In this case we have. " + "New Orientation {}, original orientation {}. New duration {}, original " + "duration {}.", + logPrefix, + m_currentTrajectoryTime, + newNextContactAtCurrentTime->pose.quat().coeffs().transpose(), + nextContactAtCurrentTime->pose.quat().coeffs().transpose(), + newSwingFootTrajectoryDuration, + originalSwingFootTrajectoryDuration); return false; } @@ -221,7 +228,9 @@ bool SwingFootPlanner::setContactList(const ContactList& contactList) if (!this->createSE3Traj(m_state.mixedVelocity.lin().head<2>(), m_state.mixedAcceleration.lin().head<2>(), m_state.mixedVelocity.lin().tail<1>(), - m_state.mixedAcceleration.lin().tail<1>())) + m_state.mixedAcceleration.lin().tail<1>(), + m_state.mixedVelocity.ang(), + m_state.mixedAcceleration.ang())) { log()->error("{} Unable to create the new SE(3) trajectory.", logPrefix); return false; @@ -258,7 +267,9 @@ void SwingFootPlanner::setTime(const std::chrono::nanoseconds& time) bool SwingFootPlanner::createSE3Traj(Eigen::Ref initialPlanarVelocity, Eigen::Ref initialPlanarAcceleration, Eigen::Ref initialVerticalVelocity, - Eigen::Ref initialVerticalAcceleration) + Eigen::Ref initialVerticalAcceleration, + const manif::SO3d::Tangent& initialAngularVelocity, + const manif::SO3d::Tangent& initialAngularAcceleration) { constexpr auto logPrefix = "[SwingFootPlanner::createSE3Traj]"; @@ -273,17 +284,17 @@ bool SwingFootPlanner::createSE3Traj(Eigen::Ref initialPl // The rotation cannot change when the contact list is updated. For this reason we can build the // trajectory considering the initial and the final conditions - const auto T = nextContactPtr->activationTime - m_currentContactPtr->deactivationTime; - if (!m_SO3Planner.setRotations(m_currentContactPtr->pose.asSO3(), - nextContactPtr->pose.asSO3(), - T)) + m_staringTimeOfCurrentSO3Traj = m_currentTrajectoryTime; + const auto T = nextContactPtr->activationTime - m_staringTimeOfCurrentSO3Traj; + m_SO3Planner.setInitialConditions(initialAngularVelocity, initialAngularAcceleration); + m_SO3Planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero()); + if (!m_SO3Planner.setRotations(m_state.transform.quat(), nextContactPtr->pose.asSO3(), T)) { log()->error("{} Unable to set the initial and final rotations for the SO(3) planner.", logPrefix); return false; } - m_planarPlanner->setInitialConditions(initialPlanarVelocity, initialPlanarAcceleration); m_planarPlanner->setFinalConditions(Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero()); @@ -308,7 +319,6 @@ bool SwingFootPlanner::createSE3Traj(Eigen::Ref initialPl = std::chrono::duration_cast( m_footApexTime * T + m_currentContactPtr->deactivationTime); - m_heightPlanner->setInitialConditions(initialVerticalVelocity, initialVerticalAcceleration); m_heightPlanner->setFinalConditions(Vector1d::Constant(m_footLandingVelocity), Vector1d::Constant(m_footLandingAcceleration)); @@ -346,7 +356,7 @@ bool SwingFootPlanner::updateSE3Traj() constexpr auto logPrefix = "[SwingFootPlanner::updateSE3Traj]"; // compute the trajectory at the current time - const auto shiftedTime = m_currentTrajectoryTime - m_currentContactPtr->deactivationTime; + const auto shiftedTime = m_currentTrajectoryTime - m_staringTimeOfCurrentSO3Traj; manif::SO3d rotation; @@ -408,7 +418,8 @@ bool SwingFootPlanner::advance() if (m_contactList.size() == 0) { - log()->error("{} Empty contact list. Please call SwingFootPlanner::setContactList()", logPrefix); + log()->error("{} Empty contact list. Please call SwingFootPlanner::setContactList()", + logPrefix); return false; } @@ -461,7 +472,9 @@ bool SwingFootPlanner::advance() if (!this->createSE3Traj(Eigen::Vector2d::Zero(), Eigen::Vector2d::Zero(), Vector1d::Constant(m_footTakeOffVelocity), - Vector1d::Constant(m_footTakeOffAcceleration))) + Vector1d::Constant(m_footTakeOffAcceleration), + manif::SO3d::Tangent::Zero(), + manif::SO3d::Tangent::Zero())) { log()->error("{} Unable to create the new SE(3) trajectory.", logPrefix); return false; diff --git a/src/Planners/tests/SO3PlannerTest.cpp b/src/Planners/tests/SO3PlannerTest.cpp index 9b971a47c2..705ef8d36e 100644 --- a/src/Planners/tests/SO3PlannerTest.cpp +++ b/src/Planners/tests/SO3PlannerTest.cpp @@ -5,8 +5,8 @@ * distributed under the terms of the BSD-3-Clause license. */ -#include #include // Required because of https://github.com/artivis/manif/issues/162 +#include // Catch2 #include @@ -33,6 +33,10 @@ TEST_CASE("SO3 planner") SECTION("Left - Trivialized [Body]") { SO3PlannerBody planner; + REQUIRE(planner.setInitialConditions(manif::SO3d::Tangent::Zero(), + manif::SO3d::Tangent::Zero())); + REQUIRE( + planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero())); REQUIRE(planner.setRotations(initialTranform, finalTranform, T)); manif::SO3d rotation, predictedRotation; @@ -50,7 +54,8 @@ TEST_CASE("SO3 planner") { // propagate the system predictedRotation = rotation + (velocity * std::chrono::duration(dT).count()); - predictedVelocity = velocity + (acceleration * std::chrono::duration(dT).count()); + predictedVelocity + = velocity + (acceleration * std::chrono::duration(dT).count()); planner.evaluatePoint(i * dT, rotation, velocity, acceleration); @@ -67,6 +72,10 @@ TEST_CASE("SO3 planner") SECTION("Right - Trivialized [Inertial]") { SO3PlannerInertial planner; + REQUIRE(planner.setInitialConditions(manif::SO3d::Tangent::Zero(), + manif::SO3d::Tangent::Zero())); + REQUIRE( + planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero())); REQUIRE(planner.setRotations(initialTranform, finalTranform, T)); manif::SO3d rotation, predictedRotation; @@ -84,7 +93,8 @@ TEST_CASE("SO3 planner") { // propagate the system predictedRotation = (velocity * std::chrono::duration(dT).count()) + rotation; - predictedVelocity = velocity + (acceleration * std::chrono::duration(dT).count()); + predictedVelocity + = velocity + (acceleration * std::chrono::duration(dT).count()); planner.evaluatePoint(i * dT, rotation, velocity, acceleration); @@ -97,4 +107,67 @@ TEST_CASE("SO3 planner") REQUIRE(velocity.isApprox(manif::SO3d::Tangent::Zero(), tolerance)); REQUIRE(acceleration.isApprox(manif::SO3d::Tangent::Zero(), tolerance)); } + + SECTION("Right - Trivialized [Inertial] Initial velocity different from zero") + { + SO3PlannerInertial planner; + manif::SO3d::Tangent initialVelocity = (finalTranform * initialTranform.inverse()).log(); + initialVelocity.coeffs() = initialVelocity.coeffs() * 2; + REQUIRE(planner.setInitialConditions(initialVelocity, manif::SO3d::Tangent::Zero())); + REQUIRE( + planner.setFinalConditions(manif::SO3d::Tangent::Zero(), manif::SO3d::Tangent::Zero())); + REQUIRE(planner.setRotations(initialTranform, finalTranform, T)); + + manif::SO3d rotation, predictedRotation; + manif::SO3d::Tangent velocity, predictedVelocity; + manif::SO3d::Tangent acceleration; + REQUIRE(planner.evaluatePoint(0s, rotation, velocity, acceleration)); + predictedRotation = rotation; + predictedVelocity = velocity; + + REQUIRE(rotation.isApprox(initialTranform, tolerance)); + REQUIRE(velocity.isApprox(initialVelocity, tolerance)); + REQUIRE(acceleration.isApprox(manif::SO3d::Tangent::Zero(), tolerance)); + + for (std::size_t i = 1; i < samples; i++) + { + // propagate the system + predictedRotation = (velocity * std::chrono::duration(dT).count()) + rotation; + predictedVelocity + = velocity + (acceleration * std::chrono::duration(dT).count()); + + planner.evaluatePoint(i * dT, rotation, velocity, acceleration); + + REQUIRE(predictedRotation.isApprox(rotation, tolerance)); + REQUIRE(predictedVelocity.isApprox(velocity, tolerance)); + } + + planner.evaluatePoint(T, rotation, velocity, acceleration); + REQUIRE(rotation.isApprox(finalTranform, tolerance)); + REQUIRE(velocity.isApprox(manif::SO3d::Tangent::Zero(), tolerance)); + REQUIRE(acceleration.isApprox(manif::SO3d::Tangent::Zero(), tolerance)); + } + + SECTION("Right - Trivialized [Inertial] Projected Initial velocity") + { + SO3PlannerInertial planner; + manif::SO3d rotation, predictedRotation; + manif::SO3d::Tangent velocity, predictedVelocity; + manif::SO3d::Tangent acceleration; + manif::SO3d::Tangent initialVelocity = manif::SO3d::Tangent::Random(); + REQUIRE(planner.setInitialConditions(initialVelocity, manif::SO3d::Tangent::Zero())); + REQUIRE(planner.setFinalConditions(manif::SO3d::Tangent::Zero(), // + manif::SO3d::Tangent::Zero())); + REQUIRE(planner.setRotations(initialTranform, finalTranform, T)); + + REQUIRE_FALSE(planner.evaluatePoint(0s, rotation, velocity, acceleration)); + + // update the initial velocity by projecting the previous one + initialVelocity = SO3PlannerInertial::projectTangentVector(initialTranform, + finalTranform, + initialVelocity); + REQUIRE(planner.setInitialConditions(initialVelocity, manif::SO3d::Tangent::Zero())); + + REQUIRE(planner.evaluatePoint(0s, rotation, velocity, acceleration)); + } } diff --git a/src/Planners/tests/SwingFootPlannerTest.cpp b/src/Planners/tests/SwingFootPlannerTest.cpp index fbcee46494..f692b810b7 100644 --- a/src/Planners/tests/SwingFootPlannerTest.cpp +++ b/src/Planners/tests/SwingFootPlannerTest.cpp @@ -84,7 +84,7 @@ ContactList createModifiedContactList() // third transform = manif::SE3d({0.5104, 0.415, 0.400}, - Eigen::AngleAxisd(2.0208, Eigen::Vector3d::UnitZ())); + Eigen::AngleAxisd(2.4, Eigen::Vector3d::UnitZ())); REQUIRE(contactList.addContact(transform, 1s + 800ms, 2s + 700ms)); // forth