From 845dff17964edc4b490a12e79d680bbbbcd63349 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 13 Feb 2025 12:24:31 +0100 Subject: [PATCH] Add FloatingBaseSystemAccelerationKinematics class and update CMakeLists.txt --- src/ContinuousDynamicalSystem/CMakeLists.txt | 4 +- ...FloatingBaseSystemAccelerationKinematics.h | 137 ++++++++++++++++++ ...oatingBaseSystemAccelerationKinematics.cpp | 89 ++++++++++++ 3 files changed, 228 insertions(+), 2 deletions(-) create mode 100644 src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.h create mode 100644 src/ContinuousDynamicalSystem/src/FloatingBaseSystemAccelerationKinematics.cpp diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt index 7ef6762ba2..816da54b20 100644 --- a/src/ContinuousDynamicalSystem/CMakeLists.txt +++ b/src/ContinuousDynamicalSystem/CMakeLists.txt @@ -11,7 +11,7 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) add_bipedal_locomotion_library( NAME ContinuousDynamicalSystem PUBLIC_HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h - ${H_PREFIX}/FloatingBaseSystemVelocityKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h ${H_PREFIX}/FirstOrderSmoother.h + ${H_PREFIX}/FloatingBaseSystemVelocityKinematics.h ${H_PREFIX}/FloatingBaseSystemAccelerationKinematics.h ${H_PREFIX}/FloatingBaseDynamicsWithCompliantContacts.h ${H_PREFIX}/FixedBaseDynamics.h ${H_PREFIX}/FirstOrderSmoother.h ${H_PREFIX}/CentroidalDynamics.h ${H_PREFIX}/LieGroupDynamics.h ${H_PREFIX}/SO3Dynamics.h ${H_PREFIX}/Integrator.h ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/RK4.h @@ -19,7 +19,7 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) ${H_PREFIX}/MultiStateWeightProvider.h ${H_PREFIX}/ButterworthLowPassFilter.h PRIVATE_HEADERS ${H_PREFIX}/impl/traits.h - SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemVelocityKinematics.cpp src/CompliantContactWrench.cpp + SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemVelocityKinematics.cpp src/FloatingBaseSystemAccelerationKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp src/FirstOrderSmoother.cpp src/MultiStateWeightProvider.cpp src/ButterworthLowPassFilter.cpp diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.h new file mode 100644 index 0000000000..15cbe05b13 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemAccelerationKinematics.h @@ -0,0 +1,137 @@ +/** + * @file FloatingBaseSystemAccelerationKinematics.h + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H + +#include +#include + +#include +#include +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +// Forward declare for type traits specialization +class FloatingBaseSystemAccelerationKinematics; +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +namespace BipedalLocomotion::ContinuousDynamicalSystem::internal +{ +template <> struct traits +{ + using Twist = Eigen::Matrix; + using State = GenericContainer::named_tuple; + using StateDerivative + = GenericContainer::named_tuple; + using Input = GenericContainer::named_tuple; + using DynamicalSystem = FloatingBaseSystemAccelerationKinematics; +}; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * FloatingBaseSystemAccelerationKinematics describes a floating base system kinematics. + * The FloatingBaseSystemAccelerationKinematics inherits from a generic DynamicalSystem where the State + * is described by a BipedalLocomotion::GenericContainer::named_tuple | Name | Type | + * Description | + * |:----:|:-----------------:|:--------------------------------------------------------------------------------------------------------------------------------------------:| + * | `p` | `Eigen::Vector3d` | Position of the base + * w.r.t. the inertial frame | | `R` | `manif::SO3d` + * | Rotation matrix \f${} ^ I R _ {b}\f$. Matrix that transform a vector whose coordinates are + * expressed in the base frame in the inertial frame | | `s` | `Eigen::VectorXd` | Joint positions + * [in rad] | + * + * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:-------:|:----------------------:|:---------------------------------------------------------------------------------------------------------------------------:| + * | `dp` | `Eigen::Vector3d` | Linear Acceleration of the origin of the base link whose + * coordinates are expressed in the Inertial frame (MIXED RERPESENTATION) | | `omega` | + * `manif::SO3d::Tangent` | base angular Acceleration whose coordinates are expressed in + * the inertial frame (Left trivialized) | | `ds` | `Eigen::VectorXd` | Joint + * velocities [in rad/s] | + * + * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:-------:|:-----------------------------:|:--------------------------------------------:| + * | `twist` | `Eigen::Matrix` | Base twist expressed in mixed representation | + * | `ds` | `Eigen::VectorXd` | Joint velocities [in rad/s] | + */ +class FloatingBaseSystemAccelerationKinematics + : public DynamicalSystem +{ + State m_state; + Input m_controlInput; + +public: + /** + * Initialize the Dynamical system. + * @param handler pointer to the parameter handler. + * @return true in case of success/false otherwise. + * @note This function does nothing but it is required for CRTP. + */ + 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 std::chrono::nanoseconds& time, StateDerivative& stateDerivative); +}; + +} // namespace ContinuousDynamicalSystem +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FLOATING_BASE_SYSTEM_ACCELERATION_KINEMATICS_H diff --git a/src/ContinuousDynamicalSystem/src/FloatingBaseSystemAccelerationKinematics.cpp b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemAccelerationKinematics.cpp new file mode 100644 index 0000000000..a3eac5e20b --- /dev/null +++ b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemAccelerationKinematics.cpp @@ -0,0 +1,89 @@ +/** + * @file FloatingBaseSystemAccelerationKinematics.cpp + * @authors Giulio Romualdi + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion::ParametersHandler; + +bool FloatingBaseSystemAccelerationKinematics::initialize( + std::weak_ptr handler) +{ + return true; +} + +bool FloatingBaseSystemAccelerationKinematics::dynamics(const std::chrono::nanoseconds& time, + StateDerivative& stateDerivative) +{ + using namespace BipedalLocomotion::GenericContainer::literals; + + // get the state + const Eigen::Vector3d& basePositionState = m_state.get_from_hash<"p"_h>(); + const manif::SO3d& baseRotationState = m_state.get_from_hash<"R"_h>(); + const Eigen::VectorXd& jointPositionsState = m_state.get_from_hash<"s"_h>(); + const Eigen::Vector3d& baseLinearVelocityState = m_state.get_from_hash<"dp"_h>(); + const manif::SO3d::Tangent& baseAngularVelocityState = m_state.get_from_hash<"omega"_h>(); + const Eigen::VectorXd& jointVelocitiesState = m_state.get_from_hash<"ds"_h>(); + + // get the state derivative + Eigen::Vector3d& baseLinearVelocityStateDerivative = stateDerivative.get_from_hash<"dp"_h>(); + manif::SO3d::Tangent& baseAngularVelocityStateDerivative + = stateDerivative.get_from_hash<"omega"_h>(); + Eigen::VectorXd& jointVelocitiesStateDerivative = stateDerivative.get_from_hash<"ds"_h>(); + Eigen::Vector3d& baseLinearAccelerationStateDerivative + = stateDerivative.get_from_hash<"ddp"_h>(); + manif::SO3d::Tangent& baseAngularAccelerationStateDerivative + = stateDerivative.get_from_hash<"domega"_h>(); + Eigen::VectorXd& jointAccelerationsStateDerivative = stateDerivative.get_from_hash<"dds"_h>(); + + // get the control input + const Eigen::Matrix& baseAcceleration + = m_controlInput.get_from_hash<"dtwist"_h>(); + const Eigen::VectorXd& jointAcceleration = m_controlInput.get_from_hash<"dds"_h>(); + + // check the size of the joint vectors + if (jointAcceleration.size() != jointPositionsState.size() + || jointVelocitiesState.size() != jointPositionsState.size() + || jointVelocitiesStateDerivative.size() != jointPositionsState.size() + || jointAccelerationsStateDerivative.size() != jointPositionsState.size()) + { + log()->error("[FloatingBaseSystemAccelerationKinematics::dynamics] Wrong size of the " + "vectors."); + return false; + } + + baseLinearVelocityStateDerivative = baseLinearVelocityState; + baseAngularVelocityStateDerivative = baseAngularVelocityState; + jointVelocitiesStateDerivative = jointVelocitiesState; + + // we assume the base acceleration is expressed in the mixed representation + baseLinearAccelerationStateDerivative = baseAcceleration.head<3>(); + baseAngularAccelerationStateDerivative = baseAcceleration.tail<3>(); + jointAccelerationsStateDerivative = jointAcceleration; + + return true; +} + +bool FloatingBaseSystemAccelerationKinematics::setState( + const FloatingBaseSystemAccelerationKinematics::State& state) +{ + m_state = state; + return true; +} + +const FloatingBaseSystemAccelerationKinematics::State& +FloatingBaseSystemAccelerationKinematics::getState() const +{ + return m_state; +} + +bool FloatingBaseSystemAccelerationKinematics::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +}