diff --git a/CHANGELOG.md b/CHANGELOG.md index 0c8ecc2a29..364f68a82d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ All notable changes to this project are documented in this file. - Implement LinearizedFrictionCone class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/244) - Added a check on whether the installed public headers have the correct folder structure (https://github.com/dic-iit/bipedal-locomotion-framework/pull/247) - Implement python bindings for VariablesHandler class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/234) +- Implement FixedBaseDynamics class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/242) ### Changed - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204) @@ -28,6 +29,7 @@ All notable changes to this project are documented in this file. - Use std::optional instead of raw pointer in ISensorBridge. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/226) - Use `System::LinearTask` in TSID component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/240) - Restructure python bindings in submodules (https://github.com/dic-iit/bipedal-locomotion-framework/pull/238) +- Integrators and DynamicalSystems are now in the `ContinuousDynamicalSystem` component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/242) ### Fixed - Fix missing implementation of `YarpSensorBridge::getFailedSensorReads()`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/202) diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index 1dcc39e0f5..45659bbc52 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -110,6 +110,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_ContactModels framework_dependent_option(FRAMEWORK_COMPILE_System "Compile System library?" ON + "" OFF) + +framework_dependent_option(FRAMEWORK_COMPILE_ContinuousDynamicalSystem + "Compile System ContinuousDynamicalSystem?" ON "FRAMEWORK_COMPILE_ContactModels" OFF) framework_dependent_option(FRAMEWORK_COMPILE_AutoDiffCppAD diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bc5d2b5b4d..4848c096ae 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -2,6 +2,7 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. +add_subdirectory(ContinuousDynamicalSystem) add_subdirectory(TextLogging) add_subdirectory(GenericContainer) add_subdirectory(YarpUtilities) diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt new file mode 100644 index 0000000000..79fcc66085 --- /dev/null +++ b/src/ContinuousDynamicalSystem/CMakeLists.txt @@ -0,0 +1,26 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + + +if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) + + set(H_PREFIX include/BipedalLocomotion/ContinuousDynamicalSystem) + + # set target name + add_bipedal_locomotion_library( + NAME ContinuousDynamicalSystem + PUBLIC_HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h + ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h + ${H_PREFIX}/Integrator.h ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h + ${H_PREFIX}/CompliantContactWrench.h + PRIVATE_HEADERS ${H_PREFIX}/impl/traits.h + SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels + iDynTree::idyntree-high-level iDynTree::idyntree-model + Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math + PRIVATE_LINK_LIBRARIES BipedalLocomotion::CommonConversions + SUBDIRECTORIES tests + ) + +endif() diff --git a/src/System/include/BipedalLocomotion/System/ContactWrench.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CompliantContactWrench.h similarity index 66% rename from src/System/include/BipedalLocomotion/System/ContactWrench.h rename to src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CompliantContactWrench.h index 359763f1c2..5034c1ea19 100644 --- a/src/System/include/BipedalLocomotion/System/ContactWrench.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CompliantContactWrench.h @@ -1,40 +1,40 @@ /** - * @file ContactWrench.h + * @file CompliantContactWrench.h * @authors Giulio Romualdi - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * @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_SYSTEM_CONTACT_WRENCH_H -#define BIPEDAL_LOCOMOTION_SYSTEM_CONTACT_WRENCH_H +#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_COMPLIANT_CONTACT_WRENCH_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_COMPLIANT_CONTACT_WRENCH_H + +#include -#include -#include #include +#include namespace BipedalLocomotion { -namespace System +namespace ContinuousDynamicalSystem { /** * A wrench excerted on a link due to an external contact. */ -class ContactWrench +class CompliantContactWrench { iDynTree::FrameIndex m_frame; /**< Useful for identifying the variable in the Model */ std::shared_ptr m_contactModel; /**< Contact model */ public: - /** * Constructor * @param index index of the frame * @param wrench the contact wrench */ - ContactWrench(const iDynTree::FrameIndex& index, - std::shared_ptr model); + CompliantContactWrench(const iDynTree::FrameIndex& index, + std::shared_ptr model); /** * Get the index of the frame @@ -55,7 +55,7 @@ class ContactWrench const std::weak_ptr contactModel() const noexcept; }; -} // namespace System +} // namespace ContinuousDynamicalSystem } // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_SYSTEM_CONTACT_WRENCH_H +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_COMPLIANT_CONTACT_WRENCH_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h new file mode 100644 index 0000000000..c29734d3f5 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h @@ -0,0 +1,166 @@ +/** + * @file DynamicalSystem.h + * @authors Giulio Romualdi + * @copyright 2020 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_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H + +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * DynamicalSystem defines a continuous time dynamical system, i.e. \f$\dot{x}=f(x, u, t)\f$. Please + * inherit publicly from this class in order to define your custom dynamical system. + * Just be sure to call after your class definition + * #BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE() + * For instance + * \code{.cpp} + * namespace BipedalLocomotion + * { + * namespace ContinuousDynamicalSystem + * { + * // forward declaration + * class Foo; + * } + * } + * // Here we define the internal structure of the Foo. Notice that the number of types contained in + * // the FooStateType must be equal to the number of FooStateDerivetiveType (This is required by the integrator class) + * + * using FooStateType = Eigen::Vector2d; + * using FooStateDerivativeType = Eigen::Vector2d; + * using FooInputType = Eigen::Vector2d; + * BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(Foo, (FooStateType), + * (FooStateDerivativeType), + * (FooInputType)) + * namespace BipedalLocomotion + * { + * namespace ContinuousDynamicalSystem + * { + * // class definition + * class Foo : public DynamicalSystem + * { + * ... + * } + * } + * } + * \endcode + */ +template class DynamicalSystem +{ + constexpr _Derived& derived() + { + return *static_cast<_Derived*>(this); + } + constexpr const _Derived& derived() const + { + return *static_cast(this); + } + +public: + using State = typename internal::traits<_Derived>::State; /**< State space type */ + using StateDerivative = typename internal::traits<_Derived>::StateDerivative; /**< State space + derivative type + */ + using Input = typename internal::traits<_Derived>::Input; /**< Input type */ + + static_assert(is_specialization::value, + "Please define the internal structure of the dynamical system with " + "BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE() macro."); + + static_assert(is_specialization::value, + "Please define the internal structure of the dynamical system with " + "BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE() macro."); + + static_assert(is_specialization::value, + "Please define the internal structure of the dynamical system with " + "BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE() macro."); + + /** + * Initialize the Dynamical system. + * @param handler pointer to the parameter handler. + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Set the state of the dynamical system. + * @note In principle, there is no need to override this method. This value is stored in an + * internal buffer. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @note In principle, there is no need to override this method. This value is stored in an + * internal buffer. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); + + /** + * Computes the system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @warning Please implement the function in your custom dynamical system. + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); +}; + +template +bool DynamicalSystem<_Derived>::initialize( + std::weak_ptr handler) +{ + return this->derived().initialize(handler); +} + +template bool DynamicalSystem<_Derived>::setState(const State& state) +{ + return this->derived().setState(state); +} + +template +const typename DynamicalSystem<_Derived>::State& DynamicalSystem<_Derived>::getState() const +{ + return this->derived().getState(); +} + +template +bool DynamicalSystem<_Derived>::setControlInput(const typename DynamicalSystem<_Derived>::Input& controlInput) +{ + return this->derived().setControlInput(controlInput); +} + +template +bool DynamicalSystem<_Derived>::dynamics(const double& time, StateDerivative& stateDerivative) +{ + return this->derived().dynamics(time, stateDerivative); +} + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h new file mode 100644 index 0000000000..476cd6781b --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h @@ -0,0 +1,146 @@ +/** + * @file FixedBaseDynamics.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_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_BASE_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_BASE_DYNAMICS_H + +#include +#include +#include + + +#include +#include +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +class FixedBaseDynamics; +} +} // namespace BipedalLocomotion + + +// Please read it as +// BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( +// FixedBaseDynamics, +// (joint velocities, joint positions), +// (joint accelerations, joints velocities), +// (joint torques) +BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(FixedBaseDynamics, + (Eigen::VectorXd, Eigen::VectorXd), + (Eigen::VectorXd, Eigen::VectorXd), + (Eigen::VectorXd)) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +/** + * FixedBaseDynamics describes a fixed base dynamical system. + * The FixedBaseDynamics inherits from a generic DynamicalSystem where: + * - DynamicalSystem::State is described by an std::tuple containing: + * - Eigen::VectorXd: the joint velocities [in rad/s]; + * - Eigen::VectorXd: the joint positions [in rad]. + * - DynamicalSystem::StateDerivative is described by an std::tuple containing: + * - Eigen::VectorXd: the joint accelerations [in rad/s^2]; + * - Eigen::VectorXd: the joint velocities [in rad/s]. + * - DynamicalSystem::Input is described by an std::tuple containing: + * - Eigen::VectorXd: the joint torques [in Nm]; + */ +class FixedBaseDynamics : public DynamicalSystem +{ + std::shared_ptr m_kinDyn; /**< Pointer to an existing instance of + kinDynComputations object */ + std::size_t m_actuatedDoFs{0}; /**< Number of actuated degree of freedom */ + + Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector + */ + + Eigen::MatrixXd m_massMatrix; /**< Floating-base mass matrix */ + + // quantities useful to avoid dynamic allocation in the dynamic allocation in the + // FixedBaseDynamics::dynamics() method + Eigen::VectorXd m_knownCoefficent; + + bool m_useMassMatrixRegularizationTerm{false}; + Eigen::MatrixXd m_massMatrixReglarizationTerm; + + State m_state; + Input m_controlInput; + +public: + + /** + * Initialize the FixedBaseDynamics system. + * @param handler pointer to the parameter handler. + * @note The following parameters are used + * | Parameter Name | Type | Description | Mandatory | + * |:--------------:|:--------:|:--------------------------------------------------------------------------------------------:|:---------:| + * | `gravity` | `double` | Value of the Gravity. If not defined Math::StandardAccelerationOfGravitation is used | No | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Set a kinDynComputations object. + * @param kinDyn a pointer to the kinDynComputations object. + * @return true in case of success, false otherwise. + */ + bool setKinDyn(std::shared_ptr kinDyn); + + /** + * Set the mass matrix regularization term. i.e. $\f\bar{M} = M + M _ {reg}\f$. Where $\fM\f$ + * is the mass matrix and $\fM_{reg}\f$ is the matrix regularization term. + * @param matrix the regularization term for the mass matrix. + * @notice Calling this function is not mandatory. Call it only if you want to add a + * regularization term. + * @return true in case of success, false otherwise. + */ + bool setMassMatrixRegularization(const Eigen::Ref& matrix); + + /** + * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); + + /** + * Set the state of the dynamical system. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_BASE_DYNAMICS_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h new file mode 100644 index 0000000000..5e9ab12173 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h @@ -0,0 +1,115 @@ +/** + * @file FixedStepIntegrator.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_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_STEP_INTEGRATOR_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_STEP_INTEGRATOR_H + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * Fixed step integrator base class. Please inherit publicly from this class in order to specify a + * custom integration method. The custom integration method must define a method called + * `oneStepIntegration()` + */ +template class FixedStepIntegrator : public Integrator<_Derived> +{ + +protected: + double m_dT{-1}; /**< Fixed step size */ + +public: + /** + * Set the integration step time + * @param dT integration step time + */ + bool setIntegrationStep(const double& dT); + + /** + * Integrate the dynamical system from initialTime to finalTime. + * @note We assume a constant control input in the interval. + * @param initialTime initial time of the integration. + * @param finalTime final time of the integration. + * @return true in case of success, false otherwise. + */ + bool integrate(double initialTime, double finalTime); +}; + +template bool FixedStepIntegrator<_Derived>::setIntegrationStep(const double& dT) +{ + if (dT <= 0) + { + log()->error("[FixedStepIntegrator::setIntegrationStep] The integration must be a strict " + "positive number"); + return false; + } + + m_dT = dT; + + return true; +} + +template +bool FixedStepIntegrator<_Derived>::integrate(double initialTime, double finalTime) +{ + constexpr auto errorPrefix = "[FixedStepIntegrator::integrate]"; + + if (this->m_dynamicalSystem == nullptr) + { + log()->error("{} Please set the dynamical system before call this function.", errorPrefix); + return false; + } + + if (initialTime > finalTime) + { + log()->error("{} The final time has to be greater than the initial one.", errorPrefix); + return false; + } + + if (m_dT <= 0) + { + log()->error("{} Please set the integration step.", errorPrefix); + return false; + } + + int iterations = std::ceil((finalTime - initialTime) / m_dT); + + double currentTime = initialTime; + for (std::size_t i = 0; i < iterations - 1; i++) + { + // advance the current time + currentTime += m_dT; + + if (!static_cast<_Derived*>(this)->oneStepIntegration(currentTime, m_dT)) + { + log()->error("{} Error while integrating at time: {}.", errorPrefix, currentTime); + return false; + } + } + + // Consider last step separately to be sure that the last solution point is in finalTime + const double dT = finalTime - currentTime; + if (!static_cast<_Derived*>(this)->oneStepIntegration(currentTime, dT)) + { + log()->error("{} Error while integrating the last step.", errorPrefix); + return false; + } + + return true; +} + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_STEP_INTEGRATOR_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h new file mode 100644 index 0000000000..1a0517e9b2 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h @@ -0,0 +1,172 @@ +/** + * @file FloatingBaseDynamicsWithCompliantContacts.h + * @authors Giulio Romualdi + * @copyright 2020 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_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_WITH_COMPLIANT_CONTACTS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_WITH_COMPLIANT_CONTACTS_H + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +class FloatingBaseDynamicsWithCompliantContacts; +} +} + +// Please read it as +// BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( +// FloatingBaseDynamicsWithCompliantContacts +// (base velocity expressed in mixed, joint velocities, base position, base orientation, joint positions), +// (base acceleration expressed in mixed, joint acceleration, base linear velocity, rate of change of the base rotation matrix, joint velocities), +// (joint torques, list containing the compliant contact models) + +BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(FloatingBaseDynamicsWithCompliantContacts, + (Eigen::Matrix, + Eigen::VectorXd, + Eigen::Vector3d, + Eigen::Matrix3d, + Eigen::VectorXd), + (Eigen::Matrix, + Eigen::VectorXd, + Eigen::Vector3d, + Eigen::Matrix3d, + Eigen::VectorXd), + (Eigen::VectorXd, + std::vector)) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +/** + * FloatingBaseDynamicalSystem describes a floating base dynamical system. + * The FloatingBaseDynamicalSystem inherits from a generic DynamicalSystem where: + * - DynamicalSystem::StateType is described by an std::tuple containing: + * - Eigen::Vector6d: the base velocity expressed in mixed representation; + * - Eigen::VectorXd: the joint velocities [in rad/s]; + * - Eigen::Vector3d: position of the base w.r.t. the inertial frame + * - Eigen::Matrix3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector + * whose coordinates are expressed in the base frame in the inertial frame; + * - Eigen::VectorXd: the joint positions [in rad]. + * - DynamicalSystem::StateDerivativeType is described by an std::tuple containing: + * - Eigen::Vector6d: the base acceleration expressed in mixed representation; + * - Eigen::VectorXd: the joint accelerations [in rad/s^2]; + * - Eigen::Vector3d: base velocity w.r.t. the inertial frame; + * - Eigen::Matrix3d: rate of change of the rotation matrix \f${} ^ I \dot{R} _ {b}\f$. + * whose coordinates are expressed in the base frame in the inertial frame; + * - Eigen::VectorXd: the joint velocities [in rad/s]. + * - DynamicalSystem::InputType is described by an std::tuple containing: + * - Eigen::VectorXd: the joint torques [in Nm]; + * - std::vector: List of contact wrenches. + */ +class FloatingBaseDynamicsWithCompliantContacts + : public DynamicalSystem +{ + static constexpr size_t m_baseDoFs = 6; /**< Number of degree of freedom associated to the + floating base */ + + std::shared_ptr m_kinDyn; /**< Pointer to an existing instance of + kinDynComputations object */ + std::size_t m_actuatedDoFs{0}; /**< Number of actuated degree of freedom */ + + Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector + */ + + Eigen::MatrixXd m_massMatrix; /**< Floating-base mass matrix */ + + Eigen::MatrixXd m_jacobianMatrix; /**< Jacobian Matrix */ + + // quantities useful to avoid dynamic allocation in the dynamic allocation in the + // FloatingBaseDynamicsWithCompliantContacts::dynamics method + Eigen::VectorXd m_generalizedRobotAcceleration; + Eigen::VectorXd m_knownCoefficent; + + bool m_useMassMatrixRegularizationTerm{false}; + Eigen::MatrixXd m_massMatrixReglarizationTerm; + + double m_rho{0.01}; /**< Regularization term used for the Baumgarte stabilization over the SO(3) + group */ + + State m_state; + Input m_controlInput; + +public: + /** + * Initialize the FloatingBaseDynamicsWithCompliantContacts system. + * @param handler pointer to the parameter handler. + * @note The following parameters are used + * | Parameter Name | Type | Description | Mandatory | + * |:--------------:|:--------:|:--------------------------------------------------------------------------------------------:|:---------:| + * | `gravity` | `double` | Value of the Gravity. If not defined Math::StandardAccelerationOfGravitation is used | No | + * | `rho` | `double` | Baumgarte stabilization parameter over the SO(3) group. The default value is 0.01 | No | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Set a kinDynComputations object. + * @param kinDyn a pointer to the kinDynComputations object. + * @return true in case of success, false otherwise. + */ + bool setKinDyn(std::shared_ptr kinDyn); + + /** + * Set the mass matrix regularization term. i.e. $\f\bar{M} = M + M _ {reg}\f$. Where $\fM\f$ + * is the mass matrix and $\fM_{reg}\f$ is the matrix regularization term. + * @param matrix the regularization term for the mass matrix. + * @notice Calling this function is not mandatory. Call it only if you want to add a + * regularization term. + * @return true in case of success, false otherwise. + */ + bool setMassMatrixRegularization(const Eigen::Ref& matrix); + + /** + * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); + + /** + * Set the state of the dynamical system. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_WITH_COMPLIANT_CONTACTS_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h new file mode 100644 index 0000000000..3ea8689b68 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h @@ -0,0 +1,117 @@ +/** + * @file FloatingBaseSystemKinematics.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_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H + +#include +#include + +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +// Forward declare for type traits specialization +class FloatingBaseSystemKinematics; +} +} + +// Please read it as +// BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( +// FloatingBaseSystemKinematics, +// (base position, base orientation, joint positions), +// (base linear velocity, rate of change of base rotation matrix, joint velocities), +// (base twist expressed in mixed representation, joint velocities)) +BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( + FloatingBaseSystemKinematics, + (Eigen::Vector3d, Eigen::Matrix3d, Eigen::VectorXd), + (Eigen::Vector3d, Eigen::Matrix3d, Eigen::VectorXd), + (Eigen::Matrix, Eigen::VectorXd)) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * FloatingBaseSystemKinematics describes a floating base system kinematics. + * The FloatingBaseSystemKinematics inherits from a generic DynamicalSystem where: + * - DynamicalSystem::State is described by an std::tuple containing: + * - Eigen::Vector3d: position of the base w.r.t. the inertial frame + * - Eigen::Matrix3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector + * whose coordinates are expressed in the base frame in the inertial frame; + * - Eigen::VectorXd: the joint positions [in rad]. + * - DynamicalSystem::StateDerivative is described by an std::tuple containing: + * - Eigen::Vector3d: base linear velocity w.r.t. the inertial frame; + * - Eigen::Matrix3d: rate of change of the rotation matrix \f${} ^ I \dot{R} _ {b}\f$. + * whose coordinates are expressed in the base frame in the inertial frame; + * - Eigen::VectorXd: the joint velocities [in rad/s]. + * - DynamicalSystem::Input is described by an std::tuple containing: + * - Eigen::Vector6d: base twist w.r.t. the inertial frame; + * - Eigen::VectorXd: the joint velocities [in rad/s]. + */ +class FloatingBaseSystemKinematics : public DynamicalSystem +{ + double m_rho{0.01}; /**< Regularization term used for the Baumgarte stabilization over the SO(3) + group */ + + State m_state; + Input m_controlInput; + +public: + /** + * Initialize the FloatingBaseSystemKinematics system. + * @param handler pointer to the parameter handler. + * @note The following parameters are used + * | Parameter Name | Type | Description | Mandatory | + * |:--------------:|:--------:|:---------------------------------------------------------------------------------:|:---------:| + * | `rho` | `double` | Baumgarte stabilization parameter over the SO(3) group. The default value is 0.01 | No | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Set the state of the dynamical system. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); + + /** + * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h new file mode 100644 index 0000000000..652b0caf33 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h @@ -0,0 +1,110 @@ +/** + * @file ForwardEuler.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_CONTINUOUS_DYNAMICAL_SYSTEM_FORWARD_EULER_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FORWARD_EULER_H + +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +template class ForwardEuler; +} +} + +BLF_DEFINE_INTEGRATOR_STRUCTURE(ForwardEuler, _DynamicalSystemType) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * Forward Euler integration method. + * @tparam _DynamicalSystem a class derived from DynamicalSystem + * @warning We assume that the operator + is defined for the objects contained in the + * DynamicalSystem::State and DynamicalSystem::StateDerivative. + */ +template +class ForwardEuler : public FixedStepIntegrator> +{ + using State = typename internal::traits>::State; + using StateDerivative = typename internal::traits>::StateDerivative; + + /** Temporary buffer usefully to avoid continuous memory allocation */ + StateDerivative m_computationalBufferStateDerivative; + + /** Temporary buffer usefully to avoid continuous memory allocation */ + State m_computationalBufferState; + + template + inline typename std::enable_if::type + addArea(const std::tuple& dx, const double& dT, std::tuple& x) + { + static_assert(sizeof...(Tp) == sizeof...(Td)); + } + + template + inline typename std::enable_if < I::type + addArea(const std::tuple& dx, const double& dT, std::tuple& x) + { + static_assert(sizeof...(Tp) == sizeof...(Td)); + + std::get(x) += std::get(dx) * dT; + addArea(dx, dT, x); + } + +public: + /** + * Integrate one step. + * @param t0 initial time. + * @param dT sampling time. + * @return true in case of success, false otherwise. + */ + bool oneStepIntegration(double t0, double dT); +}; + +template +bool ForwardEuler<_DynamicalSystem>::oneStepIntegration(double t0, double dT) +{ + constexpr auto errorPrefix = "[ForwardEuler::oneStepIntegration]"; + if (this->m_dynamicalSystem == nullptr) + { + log()->error("{} Please specify the dynamical system.", errorPrefix); + return false; + } + + if (!this->m_dynamicalSystem->dynamics(t0, this->m_computationalBufferStateDerivative)) + { + log()->error("{} Unable to compute the system dynamics.", errorPrefix); + return false; + } + + // x = x0 + dT * dx + this->m_computationalBufferState = this->m_dynamicalSystem->getState(); + this->addArea(this->m_computationalBufferStateDerivative, dT, this->m_computationalBufferState); + + if (!this->m_dynamicalSystem->setState(this->m_computationalBufferState)) + { + log()->error("{} Unable to set the new state in the dynamical system.", errorPrefix); + return false; + } + + return true; +} + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FORWARD_EULER_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h new file mode 100644 index 0000000000..d6d372cbf7 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h @@ -0,0 +1,120 @@ +/** + * @file Integrator.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_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H + +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * Integrator base class. f$. Please inherit publicly from this class in order to define your custom + * integrator. Just be sure to call after your class definition #BLF_DEFINE_INTEGRATOR_STRUCTURE() + */ +template class Integrator +{ +public: + /** DynamicalSystem type */ + using DynamicalSystem = typename internal::traits<_Derived>::DynamicalSystem; + + /** State of the integrator type */ + using State = typename internal::traits<_Derived>::State; + + static_assert(std::is_base_of< + BipedalLocomotion::ContinuousDynamicalSystem::DynamicalSystem, + DynamicalSystem>::value, + "The integrator template type must be derived from DynamicalSystem."); + +protected: + /** Pointer to a dynamical system*/ + std::shared_ptr m_dynamicalSystem; + +public: + /** + * Set the DynamicalSystem to be considered. + * @note This methods changes the dynamical system only if it was not already set. + * @param dynamicalSystem Pointer to a dynamical system. + * @return true in case of success, false otherwise. + */ + bool setDynamicalSystem(std::shared_ptr dynamicalSystem); + + /** + * Get the dynamical system. + * @return a weak pointer to a dynamical system. + */ + std::weak_ptr dynamicalSystem() const; + + /** + * Retrieve the solution. + * @return a const reference to the solution. + */ + const State& getSolution() const; + + /** + * Integrate the dynamical system from initialTime to finalTime. + * @note We assume a constant control input in the interval. + * @param initialTime initial time of the integration. + * @param finalTime final time of the integration. + * @return true in case of success, false otherwise. + */ + bool integrate(double initialTime, double finalTime); +}; + +template +bool Integrator<_Derived>::setDynamicalSystem(std::shared_ptr::DynamicalSystem> dynamicalSystem) +{ + constexpr auto errorPrefix = "[Integrator::setDynamicalSystem]"; + + // The dynamical system can be set only once + if (m_dynamicalSystem != nullptr) + { + log()->error("{} The dynamical system has been already set.", errorPrefix); + return false; + } + + if (dynamicalSystem == nullptr) + { + log()->error("{} The dynamical system passed to the function is corrupted.", errorPrefix); + return false; + } + + m_dynamicalSystem = dynamicalSystem; + + return true; +} + +template +std::weak_ptr::DynamicalSystem> Integrator<_Derived>::dynamicalSystem() const +{ + return m_dynamicalSystem; +} + +template +const typename Integrator<_Derived>::State& Integrator<_Derived>::getSolution() const +{ + return m_dynamicalSystem->getState(); +} + +template bool Integrator<_Derived>::integrate(double initialTime, double finalTime) +{ + return static_cast<_Derived*>(this)->integrate(initialTime, finalTime); +} + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h new file mode 100644 index 0000000000..fe81aa9477 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h @@ -0,0 +1,116 @@ +/** + * @file LinearTimeInvariantSystem.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_CONTINUOUS_DYNAMICAL_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H + +#include +#include + +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +// Forward declare for type traits specialization + +class LinearTimeInvariantSystem; +} +} + +// Define the internal structure of the System +BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(LinearTimeInvariantSystem, + (Eigen::VectorXd), + (Eigen::VectorXd), + (Eigen::VectorXd)) + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * LinearTimeInvariantSystem describes a MIMO linear time invariant system of the form \f$\dot{x} = + * Ax + Bu\f$ where \a x is the state and \a u the control input. The state, its derivative and the + * control input are described by vectors + * The LinearTimeInvariantSystem inherits from a generic DynamicalSystem where: + * - DynamicalSystem::State is described by an std::tuple containing: + * - Eigen::VectorXd: a generic state. + * - DynamicalSystem::StateDerivative is described by an std::tuple containing: + * - Eigen::VectorXd: a generic state derivative. + * - DynamicalSystem::Input is described by an std::tuple containing: + * - Eigen::VectorXd: a generic control input. + */ +class LinearTimeInvariantSystem : public DynamicalSystem +{ + Eigen::MatrixXd m_A; + Eigen::MatrixXd m_B; + + bool m_isInitialized{false}; + + State m_state; + Input m_controlInput; + +public: + /** + * Set the system matrices. + * @param A the A matrix. + * @param B the B matrix. + * @return true in case of success, false otherwise. + */ + bool setSystemMatrices(const Eigen::Ref& A, + const Eigen::Ref& B); + + /** + * Initialize the Dynamical system. + * @param handler pointer to the parameter handler. + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Set the state of the dynamical system. + * @param state tuple containing a const reference to the state elements. + * @return true in case of success, false otherwise. + */ + bool setState(const State& state); + + /** + * Get the state to the dynamical system. + * @return the current state of the dynamical system + */ + const State& getState() const; + + /** + * Set the control input to the dynamical system. + * @note In principle, there is no need to override this method. This value is stored in an + * internal buffer. + * @param controlInput the value of the control input used to compute the system dynamics. + * @return true in case of success, false otherwise. + */ + bool setControlInput(const Input& controlInput); + + /** + * Computes the system dynamics. It return \f$Ax + Bu\f$ + * @note The control input and the state have to be set separately with the methods + * setControlInput and setState. + * @param time the time at witch the dynamics is computed. + * @param stateDynamics tuple containing a reference to the element of the state derivative + * @return true in case of success, false otherwise. + */ + bool dynamics(const double& time, StateDerivative& stateDerivative); +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h new file mode 100644 index 0000000000..919023eabf --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/impl/traits.h @@ -0,0 +1,99 @@ +/** + * @file traits.h + * @authors Giulio Romualdi + * @copyright 2020 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_CONTINUOUS_DYNAMICAL_SYSTEM_IMPL_TRAITS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_IMPL_TRAITS_H + +#include + +#define BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE(...) using State = std::tuple<__VA_ARGS__> + +#define BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE_DERIVATIVE(...) \ + using StateDerivative = std::tuple<__VA_ARGS__> + +#define BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE_INPUT(...) using Input = std::tuple<__VA_ARGS__> + +/** + * The user must call this macro before defining a custom ContinuousDynamicalSystem::DynamicalSystem + * @param DynamicalSystemType the type of the dynamical system + * @param StateType the list of the types used to define the state. The list must be defined using + * round parenthesis. E.g. `(Eigen::VectorXd, Eigen::VectorXd)`. + * @param StateDerivativeType the list of the types used to define the state derivative. The list + * must be defined using round parenthesis. E.g. `(Eigen::VectorXd, Eigen::VectorXd)`. + * @param InputType the list of the types used to define inputs. The list must be defined using + * round parenthesis. E.g. `(Eigen::VectorXd, Eigen::VectorXd)`. + */ +#define BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE(DynamicalSystemType, \ + StateType, \ + StateDerivativeType, \ + InputType) \ + namespace BipedalLocomotion \ + { \ + namespace ContinuousDynamicalSystem \ + { \ + namespace internal \ + { \ + template <> struct traits \ + { \ + BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE StateType; \ + BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE_DERIVATIVE StateDerivativeType; \ + BLF_CONTINUOUS_DYNAMICAL_SYSTEM_STATE_INPUT InputType; \ + }; \ + } \ + } \ + } + +/** + * The user must call this macro before defining a custom ContinuousDynamicalSystem::Integrator + * @param IntegratorType the type of the integrator. + * @param DynamicalSystemType the type of the dynamical system. + */ +#define BLF_DEFINE_INTEGRATOR_STRUCTURE(IntegratorType, DynamicalSystemType) \ + namespace BipedalLocomotion \ + { \ + namespace ContinuousDynamicalSystem \ + { \ + namespace internal \ + { \ + template struct traits> \ + { \ + /** State of the integrator */ \ + using State = typename traits::State; \ + /** State derivative of the integrator */ \ + using StateDerivative = typename traits::StateDerivative; \ + /** Type of the dynamical system */ \ + using DynamicalSystem = DynamicalSystemType; \ + }; \ + } \ + } \ + } + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ +namespace internal +{ + +template struct traits; + +/// @note the following is from the Eigen library +/// here we say once and for all that traits == traits +/// +/// When constness must affect traits, it has to be constness on +/// template parameters on which T itself depends. +/// For example, traits > != traits >, but +/// traits > == traits > +template struct traits : traits +{ +}; + +} // namespace internal +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_IMPL_TRAITS_H diff --git a/src/ContinuousDynamicalSystem/src/CompliantContactWrench.cpp b/src/ContinuousDynamicalSystem/src/CompliantContactWrench.cpp new file mode 100644 index 0000000000..e08f862b73 --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/CompliantContactWrench.cpp @@ -0,0 +1,36 @@ +/** + * @file CompliantContactWrench.cpp + * @authors Giulio Romualdi + * @copyright 2020 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. + */ + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion; + +CompliantContactWrench::CompliantContactWrench(const iDynTree::FrameIndex& index, + std::shared_ptr model) + +{ + m_contactModel = model; + m_frame = index; +} + +iDynTree::FrameIndex& CompliantContactWrench::index() noexcept +{ + return m_frame; +} + +const iDynTree::FrameIndex& CompliantContactWrench::index() const noexcept +{ + return m_frame; +} + +const std::weak_ptr +CompliantContactWrench::contactModel() const noexcept +{ + return m_contactModel; +} diff --git a/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp new file mode 100644 index 0000000000..7f79a70aef --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp @@ -0,0 +1,182 @@ +/** + * @file FixedBaseDynamics.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. + */ + +#include +#include +#include + +#include +#include +#include + +using namespace BipedalLocomotion; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion::ParametersHandler; + +bool FixedBaseDynamics::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[FixedBaseDynamics::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is expired. Please call the function passing a " + "pointer pointing an already allocated memory.", + logPrefix); + return false; + } + + if (!ptr->getParameter("gravity", m_gravity)) + { + log()->info("{} The gravity vector not found. The default one will be " + "used {}.", + logPrefix, + m_gravity.transpose()); + } + + return true; +} + +bool FixedBaseDynamics::setKinDyn(std::shared_ptr kinDyn) +{ + constexpr auto logPrefix = "[FixedBaseDynamics::setKinDyn]"; + constexpr std::size_t spatialVelocitySize = 6; + + if (kinDyn == nullptr) + { + log()->error("{} The kinDyn computation object is not valid.", logPrefix); + return false; + } + + m_kinDyn = kinDyn; + m_actuatedDoFs = m_kinDyn->model().getNrOfDOFs(); + + // resize matrices + m_massMatrix.resize(m_actuatedDoFs + spatialVelocitySize, m_actuatedDoFs + spatialVelocitySize); + m_knownCoefficent.resize(m_actuatedDoFs + spatialVelocitySize); + + return true; +} + +bool FixedBaseDynamics::setMassMatrixRegularization(const Eigen::Ref& matrix) +{ + constexpr auto logPrefix = "[FixedBaseDynamics::setMassMatrixRegularization]"; + + if (m_kinDyn == nullptr) + { + log()->error("{} Please call setKinDyn() before.", logPrefix); + return false; + } + + if ((m_actuatedDoFs != matrix.rows()) || (matrix.cols() != matrix.rows())) + { + log()->error("{} The size of the regularization matrix is not correct. The correct size " + "is: {} x {}. While the input of the function is a {} x {} matrix.", + logPrefix, + m_actuatedDoFs, + m_actuatedDoFs, + matrix.rows(), + matrix.cols()); + return false; + } + + m_massMatrixReglarizationTerm = matrix; + m_useMassMatrixRegularizationTerm = true; + + return true; +} + +bool FixedBaseDynamics::dynamics(const double& time, StateDerivative& stateDerivative) +{ + constexpr auto logPrefix = "[FixedBaseDynamics::dynamics]"; + + if (m_kinDyn == nullptr) + { + log()->error("{} Please call setKinDyn() before.", logPrefix); + return false; + } + + // get the state + const auto& [jointVelocity, jointPositions] = m_state; + + auto& [jointAcceleration, jointVelocityOutput] = stateDerivative; + + const auto& [jointTorques] = m_controlInput; + + // check the size of the vectors + if (jointVelocity.size() != m_actuatedDoFs || jointPositions.size() != m_actuatedDoFs + || jointTorques.size() != m_actuatedDoFs) + { + log()->error("{} Wrong size of the vectors.", logPrefix); + return false; + } + + + jointVelocityOutput = jointVelocity; + + // update kindyncomputations object + if (!m_kinDyn->setRobotState(jointPositions, jointVelocity, m_gravity)) + { + log()->error("{} Unable to update the kinDyn object.", logPrefix); + return false; + } + + // compute the mass matrix + if (!m_kinDyn->getFreeFloatingMassMatrix(m_massMatrix)) + { + log()->error("{} Unable to get the mass matrix.", logPrefix); + return false; + } + + // compute the generalized bias forces + // here we want to compute the robot acceleration as + // robotAcceleration = M^-1 (-h + J' F + tau) = M^-1 * m_knownCoefficent + + if (!m_kinDyn->generalizedBiasForces(m_knownCoefficent)) + { + log()->error("{} Unable to get the bias forces.", logPrefix); + return false; + } + + m_knownCoefficent *= -1; + m_knownCoefficent.tail(m_actuatedDoFs) += jointTorques; + + // compute the joint acceleration solving the linear system. Here we assume that the + // mass matrix is positive definite (check here for further informations: + // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html) + if (m_useMassMatrixRegularizationTerm) + { + jointAcceleration = (m_massMatrix.bottomRightCorner(m_actuatedDoFs, m_actuatedDoFs) + + m_massMatrixReglarizationTerm) + .llt() + .solve(m_knownCoefficent.tail(m_actuatedDoFs)); + } else + { + jointAcceleration = m_massMatrix.bottomRightCorner(m_actuatedDoFs, m_actuatedDoFs) + .llt() + .solve(m_knownCoefficent.tail(m_actuatedDoFs)); + } + + return true; +} + +bool FixedBaseDynamics::setState(const State& state) +{ + m_state = state; + return true; +} + +const FixedBaseDynamics::State& FixedBaseDynamics::getState() const +{ + return m_state; +} + +bool FixedBaseDynamics::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/System/src/FloatingBaseSystemDynamics.cpp b/src/ContinuousDynamicalSystem/src/FloatingBaseDynamicsWithCompliantContacts.cpp similarity index 50% rename from src/System/src/FloatingBaseSystemDynamics.cpp rename to src/ContinuousDynamicalSystem/src/FloatingBaseDynamicsWithCompliantContacts.cpp index 5abda7a1a9..e33bd6a02c 100644 --- a/src/System/src/FloatingBaseSystemDynamics.cpp +++ b/src/ContinuousDynamicalSystem/src/FloatingBaseDynamicsWithCompliantContacts.cpp @@ -1,65 +1,63 @@ /** - * @file FloatingBaseSystemDynamics.cpp + * @file FloatingBaseDynamicsWithCompliantContacts.cpp * @authors Giulio Romualdi - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * @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. */ -#include -#include #include +#include +#include +#include #include -#include #include +#include using namespace BipedalLocomotion; -using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::ParametersHandler; -bool FloatingBaseDynamicalSystem::initalize(std::weak_ptr handler) +bool FloatingBaseDynamicsWithCompliantContacts::initialize(std::weak_ptr handler) { + constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::initialize]"; + auto ptr = handler.lock(); if (ptr == nullptr) { - std::cerr << "[FloatingBaseDynamicalSystem::initalize] The parameter handler is expired. " - "Please call the function passing a pointer pointing an already allocated " - "memory." - << std::endl; + log()->error("{} The parameter handler is expired. Please call the function passing a " + "pointer pointing an already allocated memory.", + logPrefix); return false; } if (!ptr->getParameter("rho", m_rho)) { - std::cerr << "[FloatingBaseDynamicalSystem::initalize] Unable to load the Baumgarte " - "stabilization parameter." - << std::endl; - return false; + log()->info("{} The Baumgarte stabilization parameter not found. The default one will be " + "used {}.", + logPrefix, + m_rho); } - return true; -} - + if (!ptr->getParameter("gravity", m_gravity)) + { + log()->info("{} The gravity vector not found. The default one will be " + "used {}.", + logPrefix, + m_gravity.transpose()); + } -FloatingBaseDynamicalSystem::FloatingBaseDynamicalSystem() -{ - // set the gravity vector - m_gravity.setZero(); - m_gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + return true; } -void FloatingBaseDynamicalSystem::setGravityVector(const Eigen::Ref& gravity) +bool FloatingBaseDynamicsWithCompliantContacts::setKinDyn( + std::shared_ptr kinDyn) { - m_gravity = gravity; -} + constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::setKinDyn]"; -bool FloatingBaseDynamicalSystem::setKinDyn(std::shared_ptr kinDyn) -{ if (kinDyn == nullptr) { - std::cerr << "[FloatingBaseDynamicalSystem::setKinDynComputation] Corrupted KinDyn " - "computation object." - << std::endl; + log()->error("{} The kinDyn computation object is not valid.", logPrefix); return false; } @@ -75,40 +73,46 @@ bool FloatingBaseDynamicalSystem::setKinDyn(std::shared_ptr& matrix) +bool FloatingBaseDynamicsWithCompliantContacts::setMassMatrixRegularization( + const Eigen::Ref& matrix) { + constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::" + "setMassMatrixRegularization]"; if (m_kinDyn == nullptr) { - std::cerr << "[FloatingBaseDynamicalSystem::setMassMatrixRegularization] Please call " - "'setKinDyn()' before." - << std::endl; + log()->error("{} Please call setKinDyn() before.", logPrefix); return false; } if ((m_actuatedDoFs + m_baseDoFs != matrix.rows()) || (matrix.cols() != matrix.rows())) { const auto rightSize = m_actuatedDoFs + m_baseDoFs; - std::cerr << "[FloatingBaseDynamicalSystem::setMassMatrixRegularization] The size of the " - "regularization matrix is not correct. The correct size is: " - << rightSize << " x " << rightSize << ". While the input of the function is a " - << matrix.rows() << " x " << matrix.cols() << " matrix." << std::endl; + + log()->error("{} The size of the regularization matrix is not correct. The correct size " + "is: {} x {}. While the input of the function is a {} x {} matrix.", + logPrefix, + rightSize, + rightSize, + matrix.rows(), + matrix.cols()); return false; } m_massMatrixReglarizationTerm = matrix; m_useMassMatrixRegularizationTerm = true; + return true; } -bool FloatingBaseDynamicalSystem::dynamics(const double& time, - StateDerivativeType& stateDerivative) +bool FloatingBaseDynamicsWithCompliantContacts::dynamics(const double& time, + StateDerivative& stateDerivative) { + constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::dynamics]"; if (m_kinDyn == nullptr) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Please call 'setKinDyn()' before." - << std::endl; + log()->error("{} Please call setKinDyn() before.", logPrefix); return false; } @@ -116,48 +120,45 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, const auto& [baseVelocity, jointVelocity, basePosition, baseOrientation, jointPositions] = m_state; - auto - & [baseAcceleration, jointAcceleration, baseLinearVelocity, baseRotationRate, + auto& [baseAcceleration, + jointAcceleration, + baseLinearVelocity, + baseRotationRate, jointVelocityOutput] = stateDerivative; - const Eigen::VectorXd & jointTorques = std::get<0>(m_controlInput); - const std::vector& contactWrenches = std::get<1>(m_controlInput); + const auto& [jointTorques, contactWrenches] = m_controlInput; // check the size of the vectors if (jointVelocity.size() != m_actuatedDoFs || jointPositions.size() != m_actuatedDoFs || jointTorques.size() != m_actuatedDoFs) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Wrong size of the vectors." - << std::endl; + log()->error("{} Wrong size of the vectors.", logPrefix); return false; } // compute the base linear velocity baseLinearVelocity = baseVelocity.head<3>(); + + // here we assume that the velocity is expressed using the mixed representation baseRotationRate = -baseOrientation.colwise().cross(baseVelocity.tail<3>()) - + m_rho / 2.0 - * ((baseOrientation * baseOrientation.transpose()).inverse() - - Eigen::Matrix3d::Identity()) - * baseOrientation; + + m_rho / 2.0 * (baseOrientation.transpose().inverse() - baseOrientation); jointVelocityOutput = jointVelocity; // update kindyncomputations object const Eigen::Matrix4d baseTransform = Conversions::toEigenPose(baseOrientation, basePosition); - if (!m_kinDyn->setRobotState(baseTransform, jointPositions, - baseVelocity, jointVelocity, m_gravity)) + if (!m_kinDyn + ->setRobotState(baseTransform, jointPositions, baseVelocity, jointVelocity, m_gravity)) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Unable to update the kindyn object." - << std::endl; + log()->error("{} Unable to update the kinDyn object.", logPrefix); return false; } // compute the mass matrix if (!m_kinDyn->getFreeFloatingMassMatrix(m_massMatrix)) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Unable to get the mass matrix." - << std::endl; + log()->error("{} Unable to get the mass matrix.", logPrefix); return false; } @@ -167,8 +168,7 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, if (!m_kinDyn->generalizedBiasForces(m_knownCoefficent)) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Unable to get the bias forces." - << std::endl; + log()->error("{} Unable to get the bias forces.", logPrefix); return false; } @@ -180,9 +180,9 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, // compute the contact jacobian if (!m_kinDyn->getFrameFreeFloatingJacobian(contactWrench.index(), m_jacobianMatrix)) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] Unable to get the Jacobian for " - "the frame named: " - << m_kinDyn->model().getFrameLink(contactWrench.index()) << "." << std::endl; + log()->error("{} Unable to get the jacobian of the frame named: {}.", + logPrefix, + m_kinDyn->model().getFrameLink(contactWrench.index())); return false; } @@ -190,21 +190,20 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, auto contactPtr = contactWrench.contactModel().lock(); if (contactPtr == nullptr) { - std::cerr << "[FloatingBaseDynamicalSystem::dynamics] The contact model associated to " - "the frame named: " - << m_kinDyn->model().getFrameLink(contactWrench.index()) - << " has been expired." << std::endl; + log()->error("{} The contact model associated to the frame named {} is expired", + logPrefix, + m_kinDyn->model().getFrameLink(contactWrench.index())); return false; } contactPtr->setState(m_kinDyn->getFrameVel(contactWrench.index()), m_kinDyn->getWorldTransform(contactWrench.index())); - m_knownCoefficent += m_jacobianMatrix.transpose() - * iDynTree::toEigen(contactPtr->getContactWrench()); + m_knownCoefficent.noalias() + += m_jacobianMatrix.transpose() * iDynTree::toEigen(contactPtr->getContactWrench()); } - // add the joint torques to the known coefficent + // add the joint torques to the known coefficient m_knownCoefficent.tail(m_actuatedDoFs) += jointTorques; // resize the joint acceleration @@ -214,10 +213,13 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, // mass matrix is positive definite (check here for further informations: // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html) if (m_useMassMatrixRegularizationTerm) + { m_generalizedRobotAcceleration = (m_massMatrix + m_massMatrixReglarizationTerm).llt().solve(m_knownCoefficent); - else + } else + { m_generalizedRobotAcceleration = m_massMatrix.llt().solve(m_knownCoefficent); + } // split the acceleration in base and joint acceleration baseAcceleration = m_generalizedRobotAcceleration.head(); @@ -225,3 +227,21 @@ bool FloatingBaseDynamicalSystem::dynamics(const double& time, return true; } + +bool FloatingBaseDynamicsWithCompliantContacts::setState(const State& state) +{ + m_state = state; + return true; +} + +const FloatingBaseDynamicsWithCompliantContacts::State& +FloatingBaseDynamicsWithCompliantContacts::getState() const +{ + return m_state; +} + +bool FloatingBaseDynamicsWithCompliantContacts::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/System/src/FloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp similarity index 54% rename from src/System/src/FloatingBaseSystemKinematics.cpp rename to src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp index 2adf9a0776..429b01e38c 100644 --- a/src/System/src/FloatingBaseSystemKinematics.cpp +++ b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp @@ -1,40 +1,42 @@ /** * @file FloatingBaseSystemKinematics.cpp * @authors Giulio Romualdi - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * @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. */ -#include +#include +#include -using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::ParametersHandler; -bool FloatingBaseSystemKinematics::initalize(std::weak_ptr handler) +bool FloatingBaseSystemKinematics::initialize(std::weak_ptr handler) { + constexpr auto logPrefix = "[FloatingBaseSystemKinematics::initialize]"; + auto ptr = handler.lock(); if (ptr == nullptr) { - std::cerr << "[FloatingBaseSystemKinematics::initalize] The parameter handler is expired. " - "Please call the function passing a pointer pointing an already allocated " - "memory." - << std::endl; + log()->error("{} The parameter handler is expired. Please call the function passing a " + "pointer pointing an already allocated memory.", + logPrefix); return false; } if (!ptr->getParameter("rho", m_rho)) { - std::cerr << "[FloatingBaseSystemKinematics::initalize] Unable to load the Baumgarte " - "stabilization parameter." - << std::endl; - return false; + log()->info("{} The Baumgarte stabilization parameter not found. The default one will be " + "used {}.", + logPrefix, + m_rho); } return true; } bool FloatingBaseSystemKinematics::dynamics(const double& time, - StateDerivativeType& stateDerivative) + StateDerivative& stateDerivative) { // get the state const Eigen::Vector3d& basePosition = std::get<0>(m_state); @@ -62,12 +64,26 @@ bool FloatingBaseSystemKinematics::dynamics(const double& time, // here we assume that the velocity is expressed using the mixed representation baseRotationRate = -baseRotation.colwise().cross(baseTwist.tail<3>()) - + m_rho / 2.0 - * ((baseRotation * baseRotation.transpose()).inverse() - - Eigen::Matrix3d::Identity()) - * baseRotation; + + m_rho / 2.0 * (baseRotation.transpose().inverse() - baseRotation); jointVelocityOutput = jointVelocity; return true; } + +bool FloatingBaseSystemKinematics::setState(const FloatingBaseSystemKinematics::State& state) +{ + m_state = state; + return true; +} + +const FloatingBaseSystemKinematics::State& FloatingBaseSystemKinematics::getState() const +{ + return m_state; +} + +bool FloatingBaseSystemKinematics::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp b/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp new file mode 100644 index 0000000000..87302ef15e --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp @@ -0,0 +1,92 @@ +/** + * @file LinearTimeInvariantSystem.cpp + * @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. + */ + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +bool LinearTimeInvariantSystem::setSystemMatrices(const Eigen::Ref& A, + const Eigen::Ref& B) +{ + constexpr auto errorPrefix = "[LinearTimeInvariantSystem::setSystemMatrices]"; + if (A.rows() != B.rows()) + { + log()->error("{} A and B must have the same number of rows.", errorPrefix); + return false; + } + + if (A.rows() != A.cols()) + { + log()->error("{} A must be a square matrix.", errorPrefix); + return false; + } + + m_A = A; + m_B = B; + m_isInitialized = true; + + return true; +} + +bool LinearTimeInvariantSystem::dynamics(const double& time, StateDerivative& stateDerivative) +{ + constexpr auto errorPrefix = "[LinearTimeInvariantSystem::dynamics]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the matrices. Call setSystemMatrices()", errorPrefix); + return false; + } + + const auto& x = std::get<0>(m_state); + auto& dx = std::get<0>(stateDerivative); + const auto& u = std::get<0>(m_controlInput); + + if (x.size() != m_A.rows()) + { + log()->error("{} The size of the vector 'state' is not coherent with the system matrices.", + errorPrefix); + return false; + } + + if (u.size() != m_B.cols()) + { + log()->error("{} The size of the vector 'control input' is not coherent with the system " + "matrices.", + errorPrefix); + return false; + } + + dx.noalias() = m_A * x; + dx.noalias() += m_B * u; + + return true; +} + +bool LinearTimeInvariantSystem::initialize( + std::weak_ptr handler) +{ + return true; +} + +bool LinearTimeInvariantSystem::setState(const LinearTimeInvariantSystem::State& state) +{ + m_state = state; + return true; +} + +const LinearTimeInvariantSystem::State& LinearTimeInvariantSystem::getState() const +{ + return m_state; +} + +bool LinearTimeInvariantSystem::setControlInput(const LinearTimeInvariantSystem::Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/ContinuousDynamicalSystem/tests/CMakeLists.txt b/src/ContinuousDynamicalSystem/tests/CMakeLists.txt new file mode 100644 index 0000000000..093b04d210 --- /dev/null +++ b/src/ContinuousDynamicalSystem/tests/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +add_bipedal_test( + NAME IntegratorLinearSystemTest + SOURCES IntegratorLinearSystem.cpp + LINKS BipedalLocomotion::ContinuousDynamicalSystem Eigen3::Eigen) + + +add_bipedal_test( + NAME IntegratorFloatingBaseSystemKinematicsTest + SOURCES IntegratorFloatingBaseSystemKinematics.cpp + LINKS BipedalLocomotion::ContinuousDynamicalSystem Eigen3::Eigen) diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp new file mode 100644 index 0000000000..32e11869ca --- /dev/null +++ b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp @@ -0,0 +1,73 @@ +/** + * @file IntegratorFloatingBaseSystemKinematics.cpp + * @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. + */ + +#include + +// Catch2 +#include + +#include + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +TEST_CASE("Integrator - Linear system") +{ + constexpr double dT = 0.0001; + constexpr double tolerance = 1e-3; + constexpr double simulationTime = 0.5; + + auto system = std::make_shared(); + + Eigen::Matrix twist; + twist.setRandom(); + + Eigen::VectorXd jointVelocity(20); + jointVelocity.setRandom(); + + Eigen::Matrix3d rotation0; + rotation0.setIdentity(); + + Eigen::Vector3d position0; + position0.setZero(); + + Eigen::VectorXd jointPosition0(20); + jointPosition0.setZero(); + + auto closeFormSolution = [&](const double& t) { + Eigen::Vector3d pos = position0 + t * twist.head<3>(); + Eigen::Matrix3d rot + = Eigen::AngleAxisd(twist.tail<3>().norm() * t, twist.tail<3>().normalized()) + * rotation0; + + Eigen::VectorXd jointPos = jointPosition0 + t * jointVelocity; + return std::make_tuple(pos, rot, jointPos); + }; + + system->setControlInput({twist, jointVelocity}); + system->setState({position0, rotation0, jointPosition0}); + + ForwardEuler integrator; + integrator.setIntegrationStep(dT); + integrator.setDynamicalSystem(system); + + for (int i = 0; i < simulationTime / dT; i++) + { + const auto& [basePosition, baseRotation, jointPosition] = integrator.getSolution(); + + const auto& [basePositionExact, baseRotationExact, jointPositionExact] + = closeFormSolution(dT * i); + + REQUIRE(baseRotation.isApprox(baseRotationExact, tolerance)); + REQUIRE(basePosition.isApprox(basePositionExact, tolerance)); + REQUIRE(jointPosition.isApprox(jointPositionExact, tolerance)); + + REQUIRE(integrator.integrate(0, dT)); + } +} diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp new file mode 100644 index 0000000000..b22540968b --- /dev/null +++ b/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp @@ -0,0 +1,73 @@ +/** + * @file IntegratorLinearSystem.cpp + * @authors Giulio Romualdi + * @copyright 2020 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. + */ + +#include + +// Catch2 +#include + +#include + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; + +TEST_CASE("Integrator - Linear system") +{ + constexpr double dT = 0.0001; + constexpr double tolerance = 1e-3; + constexpr double simulationTime = 2; + + // Create the linear system + /** + * _ _ _ _ + * d | 0 1 | | 0 | + * -- x = | | x + | | u + * dt |_ - 2 - 2_| |_2 _| + * + */ + + auto system = std::make_shared(); + Eigen::Matrix2d A; + A << 0, 1, -2, -2; + + Eigen::Vector2d b; + b << 0, 2; + + REQUIRE(system->setSystemMatrices(A, b)); + + // analyze a step response + Eigen::VectorXd u(1); + u(0) = 1; + + Eigen::Vector2d x0; + x0.setZero(); + + // the presented dynamical has the following close form solution in case of step response + auto closeFormSolution = [](const double& t) { + Eigen::Vector2d sol; + sol(0) = 1 - std::exp(-t) * (std::cos(t) + std::sin(t)); + sol(1) = 2 * std::exp(-t) * std::sin(t); + return sol; + }; + + system->setControlInput({u}); + system->setState({x0}); + + ForwardEuler integrator; + REQUIRE(integrator.setIntegrationStep(dT)); + integrator.setDynamicalSystem(system); + + for (int i = 0; i < simulationTime / dT; i++) + { + const auto& [solution] = integrator.getSolution(); + + REQUIRE(solution.isApprox(closeFormSolution(dT * i), tolerance)); + REQUIRE(integrator.integrate(0, dT)); + } +} diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index 02ed199acf..08c7b2fb48 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -25,4 +25,4 @@ add_bipedal_test( add_bipedal_test( NAME QPInverseKinematics SOURCES QPInverseKinematicsTest.cpp - LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions) + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions BipedalLocomotion::ContinuousDynamicalSystem) diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index 03a279a700..156350a8fa 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -22,14 +22,15 @@ #include #include -#include -#include +#include +#include #include #include using namespace BipedalLocomotion::ParametersHandler; using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::IK; using namespace BipedalLocomotion::Conversions; @@ -198,7 +199,8 @@ System getSystem(std::shared_ptr kinDyn) basePose.topLeftCorner<3, 3>(), jointPositions}); - out.integrator = std::make_shared>(dT); + out.integrator = std::make_shared>(); + REQUIRE(out.integrator->setIntegrationStep(dT)); out.integrator->setDynamicalSystem(out.dynamics); return out; diff --git a/src/Planners/src/TimeVaryingDCMPlanner.cpp b/src/Planners/src/TimeVaryingDCMPlanner.cpp index d8939d99a3..b010a43d23 100644 --- a/src/Planners/src/TimeVaryingDCMPlanner.cpp +++ b/src/Planners/src/TimeVaryingDCMPlanner.cpp @@ -11,7 +11,6 @@ #include #include #include -#include #include using namespace BipedalLocomotion::Planners; diff --git a/src/System/CMakeLists.txt b/src/System/CMakeLists.txt index 9443a1680b..008fc76abe 100644 --- a/src/System/CMakeLists.txt +++ b/src/System/CMakeLists.txt @@ -10,10 +10,9 @@ if(FRAMEWORK_COMPILE_System) # set target name add_bipedal_locomotion_library( NAME System - PUBLIC_HEADERS ${H_PREFIX}/Advanceable.h ${H_PREFIX}/VariablesHandler.h ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/FloatingBaseSystemDynamics.h ${H_PREFIX}/DynamicalSystem.tpp ${H_PREFIX}/ContactWrench.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/Integrator.h ${H_PREFIX}/Integrator.tpp ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/FixedStepIntegrator.tpp ${H_PREFIX}/Integrator.tpp ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/ForwardEuler.tpp ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/LinearTask.h - SOURCES src/FloatingBaseSystemDynamics.cpp src/VariablesHandler.cpp src/ContactWrench.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/LinearTask.cpp - PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels iDynTree::idyntree-high-level iDynTree::idyntree-model Eigen3::Eigen - PRIVATE_LINK_LIBRARIES BipedalLocomotion::CommonConversions BipedalLocomotion::Math + PUBLIC_HEADERS ${H_PREFIX}/Advanceable.h ${H_PREFIX}/VariablesHandler.h ${H_PREFIX}/LinearTask.h + SOURCES src/VariablesHandler.cpp src/LinearTask.cpp + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler Eigen3::Eigen SUBDIRECTORIES tests ) diff --git a/src/System/include/BipedalLocomotion/System/DynamicalSystem.h b/src/System/include/BipedalLocomotion/System/DynamicalSystem.h deleted file mode 100644 index dd9cc38bad..0000000000 --- a/src/System/include/BipedalLocomotion/System/DynamicalSystem.h +++ /dev/null @@ -1,111 +0,0 @@ -/** - * @file DynamicalSystem.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_DYNAMICAL_SYSTEM_H -#define BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_H - -#include -#include - -#include -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * DynamicalSystem defines a continuous time dynamical system, i.e. \f$\dot{x}=f(x, u, t)\f$. Please - * inherit publicly from this class in order to define your custom dynamical system. - * @tparam State type used for describing the state (i.e. it has to be a std::tuple vectors/matrices - * or in general classes). - * @tparam StateDerivative type used for describing the state derivative (i.e. it has to be a - * std::tuple of vectors/matrices or in general classes). - * @tparam Input type used for describing the input (i.e. it has to be a std::tuple of - * vectors/matrices or in general classes). - */ -template -class DynamicalSystem -{ - static_assert(is_specialization::value, - "The State type must be a specialization of the std::tuple. E.g. " - "std::tuple"); - - static_assert(is_specialization::value, - "The StateDerivative type must be a specialization of the std::tuple. E.g. " - "std::tuple"); - - static_assert(is_specialization::value, - "The Input type must be a specialization of the std::tuple. E.g. " - "std::tuple"); - -public: - using StateType = State; /**< State space type */ - using StateDerivativeType = StateDerivative; /**< State space derivative type */ - using InputType = Input; /**< Input type */ - -protected: - InputType m_controlInput; /**< Value of the control input */ - StateType m_state; /**< Value of the current state of the system */ - -public: - - /** - * Initialize the Dynamical system. - * @param handler pointer to the parameter handler. - * @return true in case of success/false otherwise. - */ - virtual bool initalize(std::weak_ptr handler); - - /** - * Set the state of the dynamical system. - * @note In principle, there is no need to override this method. This value is stored in an - * internal buffer. - * @param state tuple containing a const reference to the state elements. - * @return true in case of success, false otherwise. - */ - virtual bool setState(const StateType& state); - - /** - * Get the state to the dynamical system. - * @return the current state of the dynamical system - */ - const StateType & getState() const; - - /** - * Set the control input to the dynamical system. - * @note In principle, there is no need to override this method. This value is stored in an - * internal buffer. - * @param controlInput the value of the control input used to compute the system dynamics. - * @return true in case of success, false otherwise. - */ - virtual bool setControlInput(const InputType& controlInput); - - /** - * Computes the system dynamics. It return \f$f(x, u, t)\f$. - * @note The control input and the state have to be set separately with the methods - * setControlInput and setState. - * @param time the time at witch the dynamics is computed. - * @param stateDynamics tuple containing a reference to the element of the state derivative - * @warning Please implement the function in your custom dynamical system. - * @return true in case of success, false otherwise. - */ - virtual bool dynamics(const double& time, StateDerivativeType& stateDerivative) = 0; - - /** - * Destructor - */ - ~DynamicalSystem() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#include - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_H diff --git a/src/System/include/BipedalLocomotion/System/DynamicalSystem.tpp b/src/System/include/BipedalLocomotion/System/DynamicalSystem.tpp deleted file mode 100644 index 55b0391fad..0000000000 --- a/src/System/include/BipedalLocomotion/System/DynamicalSystem.tpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * @file DynamicalSystem.tpp - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_DYNAMICAL_SYSTEM_TPP -#define BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_TPP - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -template -bool DynamicalSystem::setState(const StateType& state) -{ - m_state = state; - return true; -} - -template -bool DynamicalSystem::setControlInput( - const InputType& controlInput) -{ - m_controlInput = controlInput; - return true; -} - -template -const StateType& DynamicalSystem::getState() const -{ - return m_state; -} - -template -bool DynamicalSystem::initalize( - std::weak_ptr handler) -{ - return true; -} - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_TPP diff --git a/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.h b/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.h deleted file mode 100644 index af22148603..0000000000 --- a/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.h +++ /dev/null @@ -1,69 +0,0 @@ -/** - * @file FixedStepIntegrator.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FIXED_STEP_INTEGRATOR_H -#define BIPEDAL_LOCOMOTION_SYSTEM_FIXED_STEP_INTEGRATOR_H - -#include -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * Fixed step integrator base class. Please inherit publicly from this class in order to specify a - * custom integration method. - * @tparam DynamicalSystemDerived a class derived from DynamicalSystem - */ -template -class FixedStepIntegrator : public Integrator -{ - -protected: - double m_dT{0.0}; /**< Fixed step size */ - - /** - * Integrate one step. - * @param t0 initial time. - * @param dT sampling time. - * @return true in case of success, false otherwise. - */ - virtual bool oneStepIntegration(double t0, double dT) = 0; - -public: - - /** - * Constructor - * @param dT the sampling time - */ - FixedStepIntegrator(const double& dT) - : m_dT{dT} - { - } - - /** - * Integrate the dynamical system from initialTime to finalTime. - * @note We assume a constant control input in the interval. - * @param initialTime initial time of the integration. - * @param finalTime final time of the integration. - * @return true in case of success, false otherwise. - */ - bool integrate(double initialTime, double finalTime) final; - - ~FixedStepIntegrator() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#include - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FIXED_STEP_INTEGRATOR_H diff --git a/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.tpp b/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.tpp deleted file mode 100644 index 5a93bb35c2..0000000000 --- a/src/System/include/BipedalLocomotion/System/FixedStepIntegrator.tpp +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file FixedStepIntegrator.tpp - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FIXED_STEP_INTEGRATOR_TPP -#define BIPEDAL_LOCOMOTION_SYSTEM_FIXED_STEP_INTEGRATOR_TPP - -#include -#include -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ -template -bool FixedStepIntegrator::integrate(double initialTime, double finalTime) -{ - if (this->m_dynamicalSystem == nullptr) - { - std::cerr << "[FixedStepIntegrator::integrate] Please set the dynamical system before call " - "this function." - << std::endl; - return false; - } - - if (initialTime > finalTime) - { - std::cerr << "[FixedStepIntegrator::integrate] The final time has to be greater than the " - "initial one." - << std::endl; - return false; - } - - if (m_dT <= 0) - { - std::cerr << "[FixedStepIntegrator::integrate] The sampling time must be a strictly " - "positive number." - << std::endl; - return false; - } - - int iterations = std::ceil((finalTime - initialTime) / m_dT); - - double currentTime = initialTime; - for (std::size_t i = 0; i < iterations - 1; i++) - { - currentTime = initialTime + m_dT * i; - - if (!oneStepIntegration(currentTime, m_dT)) - { - std::cerr << "[FixedStepIntegrator::integrate] Error while integrating at time: " - << currentTime << "." << std::endl; - return false; - } - } - - // Consider last step separately to be sure that the last solution point is in finalTime - double dT = finalTime - currentTime; - if (!oneStepIntegration(currentTime, dT)) - { - std::cerr << "[FixedStepIntegrator::integrate] Error while integrating the last step." - << std::endl; - return false; - } - return true; -} - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FIXED_STEP_INTEGRATOR_TPP diff --git a/src/System/include/BipedalLocomotion/System/FloatingBaseSystemDynamics.h b/src/System/include/BipedalLocomotion/System/FloatingBaseSystemDynamics.h deleted file mode 100644 index 6c59f420a8..0000000000 --- a/src/System/include/BipedalLocomotion/System/FloatingBaseSystemDynamics.h +++ /dev/null @@ -1,147 +0,0 @@ -/** - * @file FloatingBaseSystemDynamics.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_H -#define BIPEDAL_LOCOMOTION_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_H - -#include -#include - -#include -#include - -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * FloatingBaseDynamicalSystem describes a floating base dynamical system. - * The FloatingBaseDynamicalSystem inherits from a generic DynamicalSystem where: - * - DynamicalSystem::StateType is described by an std::tuple containing: - * - Eigen::Vector6d: the base velocity expressed in mixed representation; - * - Eigen::VectorXd: the joint velocities [in rad/s]; - * - Eigen::Vector3d: position of the base w.r.t. the inertial frame - * - Eigen::Matrix3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector - * whose coordinates are expressed in the base frame in the inertial frame; - * - Eigen::VectorXd: the joint positions [in rad]. - * - DynamicalSystem::StateDerivativeType is described by an std::tuple containing: - * - Eigen::Vector6d: the base acceleration expressed in mixed representation; - * - Eigen::VectorXd: the joint accelerations [in rad/s^2]; - * - Eigen::Vector3d: base velocity w.r.t. the inertial frame; - * - Eigen::Matrix3d: rate of change of the rotation matrix \f${} ^ I \dot{R} _ {b}\f$. - * whose coordinates are expressed in the base frame in the inertial frame; - * - Eigen::VectorXd: the joint velocities [in rad/s]. - * - DynamicalSystem::InputType is described by an std::tuple containing: - * - Eigen::VectorXd: the joint torques [in Nm]; - * - std::vector: List of contact wrenches. - */ -class FloatingBaseDynamicalSystem - : public DynamicalSystem, - Eigen::VectorXd, - Eigen::Vector3d, - Eigen::Matrix3d, - Eigen::VectorXd>, - std::tuple, - Eigen::VectorXd, - Eigen::Vector3d, - Eigen::Matrix3d, - Eigen::VectorXd>, - std::tuple>> -{ - static constexpr size_t m_baseDoFs = 6; /**< Number of degree of freedom associated to the - floating base */ - - std::shared_ptr m_kinDyn; /**< Pointer to an existing instance of - kinDynComputations object */ - std::size_t m_actuatedDoFs{0}; /**< Number of actuated degree of freedom */ - - Eigen::Vector3d m_gravity; /**< Gravity vector */ - - Eigen::MatrixXd m_massMatrix; /**< Floating-base mass matrix */ - - Eigen::MatrixXd m_jacobianMatrix; /**< Jacobian Matrix */ - - // quantities useful to avoid dynamic allocation in the dynamic allocation in the - // FloatingBaseDynamicalSystem::dynamics method - Eigen::VectorXd m_generalizedRobotAcceleration; - Eigen::VectorXd m_knownCoefficent; - - bool m_useMassMatrixRegularizationTerm{false}; - Eigen::MatrixXd m_massMatrixReglarizationTerm; - - double m_rho{0.01}; /**< Regularization term used for the Baumgarte stabilization over the SO(3) - group */ - - -public: - /** - * Constructor. - * @note The constructor set the gravity acceleration vector to - * \f$\begin{bmatrix} 0 & 0 & -g_s \end{bmatrix}^\top\f$. Where \f$g_s\$ is equal to - * BipedalLocomotion::Math::StandardAccelerationOfGravitation. Please call setGravityVector() if - * you want define your custom gravity vector. - */ - FloatingBaseDynamicalSystem(); - - /** - * Initialize the Dynamical system. - * @note Please call this function only if you want to set an arbitrary value for the parameter - * used in the Baumgarte stabilization \f$\rho\f$ (The default value is 0.01 ). In this case the - * handler should contain a key called rho. - * @param handler pointer to the parameter handler. - * @return true in case of success/false otherwise. - */ - bool initalize(std::weak_ptr handler) override; - - /** - * Set the vector of gravity. - * @param gravity a 3D vector describing the gravity acceleration. - */ - void setGravityVector(const Eigen::Ref& gravity); - - /** - * Set a kinDynComputations object. - * @param kinDyn a pointer to the kinDynComputations object. - * @return true in case of success, false otherwise. - */ - bool setKinDyn(std::shared_ptr kinDyn); - - /** - * Set the mass matrix regularization term. i.e. $\f\bar{M} = M + M _ {reg}\f$. Where $\fM\f$ - * is the mass matrix and $\fM_{reg}\f$ is the matrix regularization term. - * @param matrix the regularization term for the mass matrix. - * @notice Calling this function is not mandatory. Call it only if you want to add a - * regularization term. - * @return true in case of success, false otherwise. - */ - bool setMassMatrixRegularization(const Eigen::Ref& matrix); - - /** - * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. - * @note The control input and the state have to be set separately with the methods - * setControlInput and setState. - * @param time the time at witch the dynamics is computed. - * @param stateDynamics tuple containing a reference to the element of the state derivative - * @return true in case of success, false otherwise. - */ - bool dynamics(const double& time, StateDerivativeType& stateDerivative) final; - - /** - * Destructor. - */ - ~FloatingBaseDynamicalSystem() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FLOATING_BASE_SYSTEM_DYNAMICS_H diff --git a/src/System/include/BipedalLocomotion/System/FloatingBaseSystemKinematics.h b/src/System/include/BipedalLocomotion/System/FloatingBaseSystemKinematics.h deleted file mode 100644 index 06e4dba836..0000000000 --- a/src/System/include/BipedalLocomotion/System/FloatingBaseSystemKinematics.h +++ /dev/null @@ -1,80 +0,0 @@ -/** - * @file FloatingBaseSystemKinematics.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H -#define BIPEDAL_LOCOMOTION_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H - -#include -#include - -#include -#include -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * FloatingBaseSystemKinematics describes a floating base system kinematics. - * The FloatingBaseSystemKinematics inherits from a generic DynamicalSystem where: - * - DynamicalSystem::StateType is described by an std::tuple containing: - * - Eigen::Vector6d: position of the base w.r.t. the inertial frame - * - Eigen::Matrix3d: rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector - * whose coordinates are expressed in the base frame in the inertial frame; - * - Eigen::VectorXd: the joint positions [in rad]. - * - DynamicalSystem::StateDerivativeType is described by an std::tuple containing: - * - Eigen::Vector6d: base velocity w.r.t. the inertial frame; - * - Eigen::Matrix3d: rate of change of the rotation matrix \f${} ^ I \dot{R} _ {b}\f$. - * whose coordinates are expressed in the base frame in the inertial frame; - * - Eigen::VectorXd: the joint velocities [in rad/s]. - * - DynamicalSystem::InputType is described by an std::tuple containing: - * - Eigen::Vector6d: base twist w.r.t. the inertial frame; - * - Eigen::VectorXd: the joint velocities [in rad/s]. - */ -class FloatingBaseSystemKinematics - : public DynamicalSystem, - std::tuple, - std::tuple, Eigen::VectorXd>> -{ - double m_rho{0.01}; /**< Regularization term used for the Baumgarte stabilization over the SO(3) - group */ - -public: - /** - * Initialize the Dynamical system. - * @note Please call this function only if you want to set an arbitrary value for the parameter - * used in the Baumgarte stabilization \f$\rho\f$ (The default value is 0.01 ). In this case the - * handler should contain a key called rho. - * @param handler pointer to the parameter handler. - * @return true in case of success/false otherwise. - */ - bool initalize(std::weak_ptr handler) override; - - /** - * Computes the floating based system dynamics. It return \f$f(x, u, t)\f$. - * @note The control input and the state have to be set separately with the methods - * setControlInput and setState. - * @param time the time at witch the dynamics is computed. - * @param stateDynamics tuple containing a reference to the element of the state derivative - * @return true in case of success, false otherwise. - */ - bool dynamics(const double& time, StateDerivativeType& stateDerivative) final; - - /** - * Destructor. - */ - ~FloatingBaseSystemKinematics() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FLOATING_BASE_SYSTEM_KINEMATICS_H diff --git a/src/System/include/BipedalLocomotion/System/ForwardEuler.h b/src/System/include/BipedalLocomotion/System/ForwardEuler.h deleted file mode 100644 index df5cc90563..0000000000 --- a/src/System/include/BipedalLocomotion/System/ForwardEuler.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file ForwardEuler.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FORWARD_EULER_H -#define BIPEDAL_LOCOMOTION_SYSTEM_FORWARD_EULER_H - -#include -#include - -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * Forward Euler integration method. - * @tparam DynamicalSystemDerived a class derived from DynamicalSystem - * @warning We assume that the operator + is defined for the objects contained in the - * DynamicalSystemDerived::StateType and DynamicalSystemDerived::StateDerivativeType. - */ -template -class ForwardEuler : public FixedStepIntegrator -{ - typename DynamicalSystemDerived::StateDerivativeType m_computationalBufferStateDerivative; - typename DynamicalSystemDerived::StateType m_computationalBufferState; - - template - inline typename std::enable_if::type - addArea(const std::tuple& dx, const double& dT, std::tuple& x) - { - static_assert(sizeof...(Tp) == sizeof...(Td)); - } - - template - inline typename std::enable_if < I::type - addArea(const std::tuple& dx, const double& dT, std::tuple& x) - { - static_assert(sizeof...(Tp) == sizeof...(Td)); - - std::get(x) += std::get(dx) * dT; - addArea(dx, dT, x); - } - - /** - * Integrate one step. - * @param t0 initial time. - * @param dT sampling time. - * @return true in case of success, false otherwise. - */ - bool oneStepIntegration(double t0, double dT) final; - -public: - /** - * Constructor - * @param dT the sampling time - */ - ForwardEuler(const double& dT) - : FixedStepIntegrator(dT) - { - } - ~ForwardEuler() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#include - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FORWARD_EULER_H diff --git a/src/System/include/BipedalLocomotion/System/ForwardEuler.tpp b/src/System/include/BipedalLocomotion/System/ForwardEuler.tpp deleted file mode 100644 index def51c88d6..0000000000 --- a/src/System/include/BipedalLocomotion/System/ForwardEuler.tpp +++ /dev/null @@ -1,54 +0,0 @@ -/** - * @file ForwardEuler.tpp - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_FORWARD_EULER_TPP -#define BIPEDAL_LOCOMOTION_SYSTEM_FORWARD_EULER_TPP - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -template -bool ForwardEuler::oneStepIntegration(double t0, double dT) -{ - - if (this->m_dynamicalSystem == nullptr) - { - std::cerr << "[ForwardEuler::oneStepIntegration] Please specify the dynamical system." - << std::endl; - return false; - } - - if (!this->m_dynamicalSystem->dynamics(t0, this->m_computationalBufferStateDerivative)) - { - std::cerr << "[ForwardEuler::oneStepIntegration] Unable to compute the system dynamics." - << std::endl; - return false; - } - - // x = x0 + dT * dx - this->m_computationalBufferState = this->m_dynamicalSystem->getState(); - this->addArea(this->m_computationalBufferStateDerivative, dT, this->m_computationalBufferState); - - if (!this->m_dynamicalSystem->setState(this->m_computationalBufferState)) - { - std::cerr << "[ForwardEuler::oneStepIntegration] Unable to set the new state in the " - "dynamical system." - << std::endl; - return false; - } - - return true; -} - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_FORWARD_EULER_TPP diff --git a/src/System/include/BipedalLocomotion/System/Integrator.h b/src/System/include/BipedalLocomotion/System/Integrator.h deleted file mode 100644 index 405ce5eb26..0000000000 --- a/src/System/include/BipedalLocomotion/System/Integrator.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file Integrator.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_INTEGRATOR_H -#define BIPEDAL_LOCOMOTION_SYSTEM_INTEGRATOR_H - -#include -#include - -#include -#include -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * Integrator base class. Please inherit publicly from this class in order to specify a custom - * integration method. - * @tparam DynamicalSystemDerived a class derived from DynamicalSystem - */ -template class Integrator -{ - static_assert( - std::is_base_of, - DynamicalSystemDerived>::value, - "The integrator template type has to be derived from DynamicalSystem."); - -protected: - - /** Pointer to a dynamical system*/ - std::shared_ptr m_dynamicalSystem; - -public: - - /** - * Set the DynamicalSystem to be considered. - * @note This methods changes the dynamical system only if it was not already set. - * @param dynamicalSystem Pointer to a dynamical system. - * @return true in case of success, false otherwise. - */ - bool setDynamicalSystem(std::shared_ptr dynamicalSystem); - - /** - * Get the dynamical system. - * @return a weak pointer to a dynamical system. - */ - const std::weak_ptr dynamicalSystem () const; - - /** - * Retrieve the solution. - * @return a const reference to the solution. - */ - const typename DynamicalSystemDerived::StateType& getSolution() const; - - /** - * Integrate the dynamical system from initialTime to finalTime. - * @note We assume a constant control input in the interval. - * @param initialTime initial time of the integration. - * @param finalTime final time of the integration. - * @return true in case of success, false otherwise. - */ - virtual bool integrate(double initialTime, double finalTime) = 0; - - ~Integrator() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - - -#include - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_H diff --git a/src/System/include/BipedalLocomotion/System/Integrator.tpp b/src/System/include/BipedalLocomotion/System/Integrator.tpp deleted file mode 100644 index 36ba0a424d..0000000000 --- a/src/System/include/BipedalLocomotion/System/Integrator.tpp +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file Integrator.tpp - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_INTEGRATOR_TPP -#define BIPEDAL_LOCOMOTION_SYSTEM_INTEGRATOR_TPP - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -template -bool Integrator::setDynamicalSystem(std::shared_ptr dynamicalSystem) -{ - // The dynamical system can be set only once - if (m_dynamicalSystem != nullptr) - { - std::cerr << "[Integrator::setDynamicalSystem] The dynamical system has been already set." - << std::endl; - return false; - } - - if (dynamicalSystem == nullptr) - { - std::cerr << "[Integrator::setDynamicalSystem] The dynamical system passed to the function " - "is corrupted." - << std::endl; - return false; - } - - m_dynamicalSystem = dynamicalSystem; - - return true; -} - -template -const std::weak_ptr -Integrator::dynamicalSystem() const -{ - return m_dynamicalSystem; -} - -template -const typename DynamicalSystemDerived::StateType& -Integrator::getSolution() const -{ - return m_dynamicalSystem->getState(); -} - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_DYNAMICAL_SYSTEM_H diff --git a/src/System/include/BipedalLocomotion/System/LinearTimeInvariantSystem.h b/src/System/include/BipedalLocomotion/System/LinearTimeInvariantSystem.h deleted file mode 100644 index b24d2007d1..0000000000 --- a/src/System/include/BipedalLocomotion/System/LinearTimeInvariantSystem.h +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file LinearTimeInvariantSystem.h - * @authors Giulio Romualdi - * @copyright 2020 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_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H -#define BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H - -#include -#include - -#include - -#include - -namespace BipedalLocomotion -{ -namespace System -{ - -/** - * LinearTimeInvariantSystem describes a MIMO linear time invariant system of the form \f$\dot{x} = - * Ax + Bu\f$ where \a x is the state and \a u the control input. The state, its derivative and the - * control input are described by vectors - * The LinearTimeInvariantSystem inherits from a generic DynamicalSystem where: - * - DynamicalSystem::StateType is described by an std::tuple containing: - * - Eigen::VectorXd: a generic state. - * - DynamicalSystem::StateDerivativeType is described by an std::tuple containing: - * - Eigen::VectorXd: a generic state derivative. - * - DynamicalSystem::InputType is described by an std::tuple containing: - * - Eigen::VectorXd: a generic control input. - */ -class LinearTimeInvariantSystem : public DynamicalSystem, - std::tuple, - std::tuple> -{ - Eigen::MatrixXd m_A; - Eigen::MatrixXd m_B; - - bool m_isInitialized{false}; - -public: - /** - * Set the system matrices. - * @param A the A matrix. - * @param B the B matrix. - * @return true in case of success, false otherwise. - */ - bool setSystemMatrices(const Eigen::Ref& A, - const Eigen::Ref& B); - - /** - * Computes the linear system dynamics. It returns \f$\dot{x} = Ax + Bu\f$ - * @note The control input and the state have to be set separately with the methods - * setControlInput and setState. - * @param time the time at witch the dynamics is computed. - * @param stateDynamics tuple containing a reference to the element of the state derivative - * @return true in case of success, false otherwise. - */ - bool dynamics(const double& time, - StateDerivativeType& stateDerivative) final; - - /** - * Destructor - */ - ~LinearTimeInvariantSystem() = default; -}; - -} // namespace System -} // namespace BipedalLocomotion - -#endif // BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TIME_INVARIANT_SYSTEM_H diff --git a/src/System/src/ContactWrench.cpp b/src/System/src/ContactWrench.cpp deleted file mode 100644 index 4643daac18..0000000000 --- a/src/System/src/ContactWrench.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/** - * @file ContactWrench.cpp - * @authors Giulio Romualdi - * @copyright 2020 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. - */ - -#include - -using namespace BipedalLocomotion::System; -using namespace BipedalLocomotion; - -ContactWrench::ContactWrench(const iDynTree::FrameIndex& index, - std::shared_ptr model) - -{ - m_contactModel = model; - m_frame = index; -} - -iDynTree::FrameIndex& ContactWrench::index() noexcept -{ - return m_frame; -} - -const iDynTree::FrameIndex& ContactWrench::index() const noexcept -{ - return m_frame; -} - -const std::weak_ptr ContactWrench::contactModel() const noexcept -{ - return m_contactModel; -} diff --git a/src/System/src/LinearTimeInvariantSystem.cpp b/src/System/src/LinearTimeInvariantSystem.cpp deleted file mode 100644 index 7c0e1b1775..0000000000 --- a/src/System/src/LinearTimeInvariantSystem.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/** - * @file LinearTimeInvariantSystem.cpp - * @authors Giulio Romualdi - * @copyright 2020 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. - */ - -#include -#include - -using namespace BipedalLocomotion::System; - -bool LinearTimeInvariantSystem::setSystemMatrices(const Eigen::Ref& A, - const Eigen::Ref& B) -{ - - if (A.rows() != B.rows()) - { - std::cerr << "[LinearTimeInvariantSystem::setSystemMatrices] A and B must have the same " - "number of rows." - << std::endl; - return false; - } - - if (A.rows() != A.cols()) - { - std::cerr << "[LinearTimeInvariantSystem::setSystemMatrices] The A matrix has to be a " - "square matrix." - << std::endl; - return false; - } - - m_A = A; - m_B = B; - m_isInitialized = true; - - return true; -} - -bool LinearTimeInvariantSystem::dynamics(const double& time, - StateDerivativeType& stateDerivative) -{ - - if (!m_isInitialized) - { - std::cerr << "[LinearTimeInvariantSystem::dynamics] Please initialize the matrices." - << std::endl; - return false; - } - - const auto& x = std::get<0>(m_state); - auto& dx = std::get<0>(stateDerivative); - const auto& u = std::get<0>(m_controlInput); - - if (x.size() != m_A.rows()) - { - std::cerr << "[LinearTimeInvariantSystem::dynamics] The size of the vector 'state' is not " - "coherent with the system matrices." - << std::endl; - return false; - } - - if (u.size() != m_B.cols()) - { - std::cerr << "[LinearTimeInvariantSystem::dynamics] The size of the vector 'control input' " - "is not coherent with the system matrices." - << std::endl; - return false; - } - - dx = m_A * x + m_B * u; - - return true; -} diff --git a/src/System/tests/CMakeLists.txt b/src/System/tests/CMakeLists.txt index d7a7234ede..12025cde2b 100644 --- a/src/System/tests/CMakeLists.txt +++ b/src/System/tests/CMakeLists.txt @@ -6,8 +6,3 @@ add_bipedal_test( NAME VariablesHandler SOURCES VariablesHandlerTest.cpp LINKS BipedalLocomotion::System) - -add_bipedal_test( - NAME IntegratorTest - SOURCES IntegratorTest.cpp - LINKS BipedalLocomotion::System Eigen3::Eigen) diff --git a/src/System/tests/IntegratorTest.cpp b/src/System/tests/IntegratorTest.cpp deleted file mode 100644 index 40682a9386..0000000000 --- a/src/System/tests/IntegratorTest.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/** - * @file RecursiveLeastSquareTest.cpp - * @authors Giulio Romualdi - * @copyright 2020 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. - */ - -#include - -// Catch2 -#include - -#include - -#include -#include -#include - -using namespace BipedalLocomotion::System; - -TEST_CASE("Integrator - Linear system") -{ - constexpr double dT = 0.0001; - constexpr double tolerance = 1e-3; - constexpr double simulationTime = 2; - - SECTION("Linear System") - { - // Create the linear system - /** - * _ _ _ _ - * d | 0 1 | | 0 | - * -- x = | | x + | | u - * dt |_ - 2 - 2_| |_2 _| - * - */ - - auto system = std::make_shared(); - Eigen::Matrix2d A; - A << 0, 1, -2, -2; - - Eigen::Vector2d b; - b << 0, 2; - - REQUIRE(system->setSystemMatrices(A, b)); - - // analyze a step response - Eigen::VectorXd u(1); - u(0) = 1; - - Eigen::Vector2d x0; - x0.setZero(); - - // the presented dynamical has the following close form solution in case of step response - auto closeFormSolution = [](const double& t) { - Eigen::Vector2d sol; - sol(0) = 1 - std::exp(-t) * (std::cos(t) + std::sin(t)); - sol(1) = 2 * std::exp(-t) * std::sin(t); - return sol; - }; - - system->setControlInput({u}); - system->setState({x0}); - - ForwardEuler integrator(dT); - integrator.setDynamicalSystem(system); - - for (int i = 0; i < simulationTime / dT; i++) - { - const auto& [solution] = integrator.getSolution(); - - REQUIRE(solution.isApprox(closeFormSolution(dT * i), tolerance)); - REQUIRE(integrator.integrate(0, dT)); - } - } - - SECTION("Floating base System Kinematics") - { - - auto system = std::make_shared(); - - Eigen::Matrix twist; - twist.setRandom(); - - Eigen::VectorXd jointVelocity(20); - jointVelocity.setRandom(); - - Eigen::Matrix3d rotation0; - rotation0.setIdentity(); - - Eigen::Vector3d position0; - position0.setZero(); - - Eigen::VectorXd jointPosition0(20); - jointPosition0.setZero(); - - auto closeFormSolution = [&](const double& t) { - Eigen::Vector3d pos = position0 + t * twist.head<3>(); - Eigen::Matrix3d rot - = Eigen::AngleAxisd(twist.tail<3>().norm() * t, twist.tail<3>().normalized()) - * rotation0; - - Eigen::VectorXd jointPos = jointPosition0 + t * jointVelocity; - return std::make_tuple(pos, rot, jointPos); - }; - - system->setControlInput({twist, jointVelocity}); - system->setState({position0, rotation0, jointPosition0}); - - ForwardEuler integrator(dT); - integrator.setDynamicalSystem(system); - - for (int i = 0; i < simulationTime / dT; i++) - { - const auto& [basePosition, baseRotation, jointPosition] = integrator.getSolution(); - - const auto& [basePositionExact, baseRotationExact, jointPositionExact] - = closeFormSolution(dT * i); - - REQUIRE(baseRotation.isApprox(baseRotationExact, tolerance)); - REQUIRE(basePosition.isApprox(basePositionExact, tolerance)); - REQUIRE(jointPosition.isApprox(jointPositionExact, tolerance)); - - REQUIRE(integrator.integrate(0, dT)); - } - } -} diff --git a/src/TextLogging/include/BipedalLocomotion/TextLogging/Logger.h b/src/TextLogging/include/BipedalLocomotion/TextLogging/Logger.h index 4efa217db2..64e88ceefc 100644 --- a/src/TextLogging/include/BipedalLocomotion/TextLogging/Logger.h +++ b/src/TextLogging/include/BipedalLocomotion/TextLogging/Logger.h @@ -8,6 +8,9 @@ #ifndef BIPEDAL_LOCOMOTION_TEXT_LOGGING_LOGGER_H #define BIPEDAL_LOCOMOTION_TEXT_LOGGING_LOGGER_H +// Required to print Eigen::Vectors (https://github.com/gabime/spdlog/issues/1638) +#include + #include namespace BipedalLocomotion