diff --git a/CHANGELOG.md b/CHANGELOG.md index f293825361..e2d496fbc3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,8 @@ All notable changes to this project are documented in this file. - Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Implement `System::NamedTuple` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/642) +- Implement `ContinuousDynamicalSystem::CentroidalDynamics` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/642) ### Changed - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) @@ -23,6 +25,7 @@ All notable changes to this project are documented in this file. - Update the python bindings to consider the time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Robustify SubModelCreator and SubModelKinDynWrapper tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/631) - `SwingFootTrajectoryPlanner::advance()` must be called before getting the output (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Update the already existing classes in `ContinuousDynamicalSystem`to be compatible with the `System::NamedTuple` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/642) ### Fixed - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622) diff --git a/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt b/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt index 558d466f8b..f8db196a83 100644 --- a/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt +++ b/bindings/python/ContinuousDynamicalSystem/CMakeLists.txt @@ -1,15 +1,16 @@ # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT). All rights reserved. # This software may be modified and distributed under the terms of the # BSD-3-Clause license. +if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) + set(H_PREFIX include/BipedalLocomotion/bindings/ContinuousDynamicalSystem) -set(H_PREFIX include/BipedalLocomotion/bindings/ContinuousDynamicalSystem) - -add_bipedal_locomotion_python_module( - NAME ContinuousDynamicalSystemBindings - SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/Module.cpp - HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/Integrator.h - ${H_PREFIX}/MultiStateWeightProvider.h - ${H_PREFIX}/Module.h - LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem - TESTS tests/test_linear_system.py tests/test_floating_base_system_kinematics.py - ) + add_bipedal_locomotion_python_module( + NAME ContinuousDynamicalSystemBindings + SOURCES src/MultiStateWeightProvider.cpp src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/Module.cpp + HEADERS ${H_PREFIX}/DynamicalSystem.h ${H_PREFIX}/LinearTimeInvariantSystem.h ${H_PREFIX}/FloatingBaseSystemKinematics.h ${H_PREFIX}/Integrator.h + ${H_PREFIX}/MultiStateWeightProvider.h + ${H_PREFIX}/Module.h + LINK_LIBRARIES BipedalLocomotion::ContinuousDynamicalSystem + TESTS tests/test_linear_system.py tests/test_floating_base_system_kinematics.py + ) +endif() diff --git a/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h b/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h index a02cc094b6..45e9e29516 100644 --- a/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h +++ b/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/DynamicalSystem.h @@ -36,18 +36,43 @@ void CreateDynamicalSystem(pybind11::module& module, const std::string& name) return impl.initialize(handler); }, py::arg("param_handler")) - .def("set_state", &_DynamicalSystem::setState, py::arg("state")) - .def("get_state", &_DynamicalSystem::getState) + .def( + "set_state", + [](_DynamicalSystem& impl, + const typename _DynamicalSystem::State::underlying_tuple& state) -> bool { + typename _DynamicalSystem::State tempState; + tempState = state; + return impl.setState(tempState); + }, + py::arg("state")) + .def("get_state", + [](const _DynamicalSystem& impl) -> + typename _DynamicalSystem::State::underlying_tuple { + return impl.getState().to_tuple(); + }) .def_property( "state", - &_DynamicalSystem::getState, - [](_DynamicalSystem& impl, const typename _DynamicalSystem::State& state) { - if (!impl.setState(state)) + [](const _DynamicalSystem& impl) -> typename _DynamicalSystem::State::underlying_tuple { + return impl.getState().to_tuple(); + }, + [](_DynamicalSystem& impl, + const typename _DynamicalSystem::State::underlying_tuple& state) { + typename _DynamicalSystem::State tempState; + tempState = state; + if (!impl.setState(tempState)) { throw py::value_error("Invalid state."); }; }) - .def("set_control_input", &_DynamicalSystem::setControlInput, py::arg("control_input")); + .def( + "set_control_input", + [](_DynamicalSystem& impl, + const typename _DynamicalSystem::Input::underlying_tuple& input) -> bool { + typename _DynamicalSystem::Input tempInput; + tempInput = input; + return impl.setControlInput(tempInput); + }, + py::arg("control_input")); } } // namespace ContinuousDynamicalSystem diff --git a/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h b/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h index c6a9f9da4a..c54dab2054 100644 --- a/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h +++ b/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H #define BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H +#include #include #include @@ -15,6 +16,7 @@ #include #include +#include #include #include #include @@ -63,7 +65,11 @@ void CreateIntegrator(pybind11::module& module, const std::string& name) throw py::value_error("The Dynamical system is not valid."); } }) - .def("get_solution", &Integrator<_Derived>::getSolution) + .def("get_solution", + [](const Integrator<_Derived>& impl) -> + typename Integrator<_Derived>::State::underlying_tuple { + return impl.getSolution().to_tuple(); + }) .def("integrate", &Integrator<_Derived>::integrate, py::arg("initial_time"), @@ -88,7 +94,7 @@ void CreateFixedStepIntegrator(pybind11::module& module, const std::string& name .def("get_integration_step", &FixedStepIntegrator<_Derived>::getIntegrationStep) .def_property("integration_step", &FixedStepIntegrator<_Derived>::getIntegrationStep, - [](FixedStepIntegrator<_Derived>& impl, double dt) { + [](FixedStepIntegrator<_Derived>& impl, const std::chrono::nanoseconds& dt) { if (!impl.setIntegrationStep(dt)) { throw py::value_error("Invalid integration step."); diff --git a/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py b/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py index 17eea7d23f..d974a32c3c 100644 --- a/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py +++ b/bindings/python/ContinuousDynamicalSystem/tests/test_floating_base_system_kinematics.py @@ -4,12 +4,14 @@ import numpy as np import manifpy as manif +from datetime import timedelta + def close_form_solution(t, position0, rotation0, joint_position0, twist, joint_velocity): return position0 + t * twist[0:3], \ manif.SO3Tangent(twist[3:6] *t) + rotation0, \ joint_position0 + t * joint_velocity -def test_linear_system(): +def test_floating_base_system_kinematics(): tolerance = 1e-3 dt = 0.0001 @@ -31,7 +33,7 @@ def test_linear_system(): integrator = blf.continuous_dynamical_system.FloatingBaseSystemKinematicsForwardEulerIntegrator() assert integrator.set_dynamical_system(system) - integrator.integration_step = dt + integrator.integration_step = timedelta(seconds=dt) for i in range(0, int(simulation_time / dt)): @@ -43,4 +45,4 @@ def test_linear_system(): assert base_rotation.rotation() == pytest.approx(base_rotation_exact.rotation(), abs=tolerance) assert joint_position == pytest.approx(joint_position_exact, abs=tolerance) - assert integrator.integrate(0, dt) + assert integrator.integrate(timedelta(seconds=0), timedelta(seconds=dt)) diff --git a/bindings/python/ContinuousDynamicalSystem/tests/test_linear_system.py b/bindings/python/ContinuousDynamicalSystem/tests/test_linear_system.py index 1aca646390..58bbf65f39 100644 --- a/bindings/python/ContinuousDynamicalSystem/tests/test_linear_system.py +++ b/bindings/python/ContinuousDynamicalSystem/tests/test_linear_system.py @@ -3,6 +3,8 @@ import bipedal_locomotion_framework.bindings as blf import numpy as np +from datetime import timedelta + def close_form_solution(t): return np.array([1 - np.exp(-t) * (np.cos(t) + np.sin(t)), 2 * np.exp(-t) * np.sin(t)]) @@ -22,11 +24,11 @@ def test_linear_system(): integrator = blf.continuous_dynamical_system.LinearTimeInvariantSystemForwardEulerIntegrator() assert integrator.set_dynamical_system(system) - assert integrator.set_integration_step(dt) + assert integrator.set_integration_step(timedelta(seconds=dt)) tolerance = 1e-3 for i in range(0, int(simulation_time / dt)): solution, = integrator.get_solution() assert solution == pytest.approx(close_form_solution(i * dt), abs=tolerance) - assert integrator.integrate(0, dt) + assert integrator.integrate(timedelta(seconds=0), timedelta(seconds=dt)) diff --git a/bindings/python/IK/tests/test_QP_inverse_kinematics.py b/bindings/python/IK/tests/test_QP_inverse_kinematics.py index 438b7e65f2..8158d42b7a 100644 --- a/bindings/python/IK/tests/test_QP_inverse_kinematics.py +++ b/bindings/python/IK/tests/test_QP_inverse_kinematics.py @@ -8,6 +8,8 @@ import icub_models import idyntree.swig as idyn +from datetime import timedelta + def test_custom_task(): class CustomTask(blf.ik.IKLinearTask): def __init__(self): @@ -183,7 +185,7 @@ def test_joint_limits_task(): joint_limits_param_handler.set_parameter_bool(name="use_model_limits",value=False) joint_limits_param_handler.set_parameter_vector_float(name="upper_limits",value=np.array(joint_values) + 0.01) joint_limits_param_handler.set_parameter_vector_float(name="lower_limits",value=np.array(joint_values) - 0.01) - joint_limits_param_handler.set_parameter_float(name="sampling_time",value=0.01) + joint_limits_param_handler.set_parameter_datetime(name="sampling_time",value=timedelta(milliseconds=10)) # Initialize the task joint_limits_task = blf.ik.JointLimitsTask() diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index a00fa4a60d..965beea89b 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -154,7 +154,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_Unicycle framework_dependent_option(FRAMEWORK_COMPILE_ContinuousDynamicalSystem "Compile System ContinuousDynamicalSystem?" ON - "FRAMEWORK_COMPILE_ContactModels;FRAMEWORK_COMPILE_Math" OFF) + "FRAMEWORK_COMPILE_ContactModels;FRAMEWORK_COMPILE_Math;FRAMEWORK_COMPILE_Contact" OFF) framework_dependent_option(FRAMEWORK_COMPILE_AutoDiffCppAD "Compile CppAD-Eigen wrapper?" ON diff --git a/src/ContinuousDynamicalSystem/CMakeLists.txt b/src/ContinuousDynamicalSystem/CMakeLists.txt index f48047e02d..7e0a057ceb 100644 --- a/src/ContinuousDynamicalSystem/CMakeLists.txt +++ b/src/ContinuousDynamicalSystem/CMakeLists.txt @@ -12,15 +12,18 @@ if(FRAMEWORK_COMPILE_ContinuousDynamicalSystem) 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}/FirstOrderSmoother.h + ${H_PREFIX}/CentroidalDynamics.h ${H_PREFIX}/Integrator.h ${H_PREFIX}/FixedStepIntegrator.h ${H_PREFIX}/ForwardEuler.h ${H_PREFIX}/CompliantContactWrench.h ${H_PREFIX}/MultiStateWeightProvider.h PRIVATE_HEADERS ${H_PREFIX}/impl/traits.h - SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp + SOURCES src/LinearTimeInvariantSystem.cpp src/FloatingBaseSystemKinematics.cpp src/CompliantContactWrench.cpp + src/FloatingBaseDynamicsWithCompliantContacts.cpp src/FixedBaseDynamics.cpp src/CentroidalDynamics.cpp src/FirstOrderSmoother.cpp src/MultiStateWeightProvider.cpp PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ContactModels BipedalLocomotion::System iDynTree::idyntree-high-level iDynTree::idyntree-model - Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math + Eigen3::Eigen BipedalLocomotion::TextLogging BipedalLocomotion::Math BipedalLocomotion::Contacts + BipedalLocomotion::GenericContainer PRIVATE_LINK_LIBRARIES BipedalLocomotion::CommonConversions SUBDIRECTORIES tests ) diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h new file mode 100644 index 0000000000..17250a78a6 --- /dev/null +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h @@ -0,0 +1,156 @@ +/** + * @file CentroidalDynamics.h + * @authors Giulio Romualdi + * @copyright 2023 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_CENROIDAL_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace BipedalLocomotion::ContinuousDynamicalSystem +{ +class CentroidalDynamics; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem + +namespace BipedalLocomotion::ContinuousDynamicalSystem::internal +{ +template <> struct traits +{ + using Contacts = std::map; + using ExternalForce = std::optional; + + using State = GenericContainer::named_tuple; + using StateDerivative + = GenericContainer::named_tuple; + using Input = GenericContainer::named_tuple; + using DynamicalSystem = CentroidalDynamics; +}; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal + +namespace BipedalLocomotion +{ +namespace ContinuousDynamicalSystem +{ + +/** + * CentroidalDynamics describes the centroidal dynamics of a multi-body system. + * The centroidal momentum \f${}_{\bar{G}} h ^\top= \begin{bmatrix} {}_{\bar{G}} h^{p\top} & {}_{\bar{G}} h^{\omega\top} \end{bmatrix} \in \mathbb{R}^6\f$ + * is the aggregate linear and angular momentum of each link of the robot referred to the robot + * center of mass (CoM). The vectors \f${}_{\bar{G}} h^p \in \mathbb{R}^3\f$ and + * \f${}_{\bar{G}} h^\omega * \in \mathbb{R}^3\f$ are the linear and angular momentum, respectively. + * The coordinates of \f${}_{\bar{G}} h\f$ are expressed w.r.t. a frame centered in the robot CoM + * and oriented as the inertial frame \f$\mathcal{I}\f$ + * The CentroidalDynamics class describes the dynamics of the centroidal momentum and of the CoM. + * Indeed the time derivative of the centroidal momentum depends on the external contact wrenches + * acting on the system, thus leading to: + * \f[ + * {}_{\bar{G}} \dot{h} = \sum_{i = 1}^{n_c} + * \begin{bmatrix} + * I_3 & 0_3 \ + * (p_{\mathcal{C}_i} - p_{\text{CoM}})^\wedge & I_3 + * \end{bmatrix} \mathrm{f}_i + m\bar{g} + * \f] + * where \f$\bar{g}^\top = \begin{bmatrix} g^\top & 0_{1\times3} \end{bmatrix}\f$, \f$m\f$ + * is the mass of the robot and \f$n_c\f$ the number of active contacts. + * The relation between the linear momentum \f${}_{\bar{G}} h^{p}\f$ and the robot CoM velocity + * is linear and depends on the robot mass \f$m\f$, i.e. \f${}_{\bar{G}} h^{p} = m v_{\text{CoM}}\f$. + * + * The CentroidalDynamics inherits from a generic DynamicalSystem where the State is + * described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:-----------------:|:-----------------:|:-----------------------------------------------------------------------------------------:| + * | `com_pos` | `Eigen::Vector3d` | Position of the CoM written in the inertial frame | + * | `com_vel` | `Eigen::Vector3d` | Velocity of the CoM written in the inertial frame | + * |`angular_momentum` | `Eigen::Vector3d` | The centroidal angular momentum whose coordinates are written respect the inertial frame. | + * + * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----------------------:|:-----------------:|:----------------------------------------------------------------------------------:| + * | `com_vel` | `Eigen::Vector3d` | Velocity of the CoM written in the inertial frame | + * | `com_acc` | `Eigen::Vector3d` | Acceleration of the CoM written in the inertial frame | + * |`angular_momentum_rate` | `Eigen::Vector3d` | The centroidal angular momentum rate of change written respect the inertial frame. | + * + * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----------------:|:---------------------------------------------------------------------------------------:|:---------------------------------------------------------------------------------------------------------------------------:| + * | `contacts` | `std::unordered_map` | List of contact where each force applied in the discere geometry contact is expressed with respect to the inertial frame | + * | `external_force` | `std::optional` | Optional force applied to the robot center of mass. The coordinates are expressed in the inertial frame | + */ +class CentroidalDynamics : public DynamicalSystem + +{ + /** Gravity vector expressed in the inertial frame*/ + Eigen::Vector3d m_gravity{0, 0, -Math::StandardAccelerationOfGravitation}; + double m_mass{1}; /**< Entire mass of the robot in kg */ + + State m_state; /**< State of the dynamical system */ + Input m_controlInput; /**< Input of the dynamical system */ + +public: + + /** + * Initialize the CentroidalDynamics 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 | + * | `mass` | `double` | Value of the mass in kg. If not defined 1 is used. | No | + * @return true in case of success/false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Computes the centroidal momentum 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); + + /** + * 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_CENROIDAL_DYNAMICS_H diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h index d7994f8e77..a431dafb49 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h @@ -8,10 +8,12 @@ #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H +#include #include #include #include +#include #include #include @@ -75,24 +77,28 @@ template class DynamicalSystem */ 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 + || is_specialization::value), + "State must specialize std::tuple or ::BipedalLocomotion::GenericContainer::named_tuple"); - 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 + || is_specialization::value), + "StateDerivative must specialize std::tuple or " + "::BipedalLocomotion::GenericContainer::named_tuple"); - 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 + || is_specialization::value), + "Input must specialize std::tuple or ::BipedalLocomotion::GenericContainer::named_tuple"); /** * 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); + bool initialize(std::weak_ptr handler); /** * Set the state of the dynamical system. @@ -127,12 +133,12 @@ template class DynamicalSystem * @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); + bool dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative); }; template bool DynamicalSystem<_Derived>::initialize( - std::weak_ptr handler) + std::weak_ptr handler) { return this->derived().initialize(handler); } @@ -155,7 +161,7 @@ bool DynamicalSystem<_Derived>::setControlInput(const typename DynamicalSystem<_ } template -bool DynamicalSystem<_Derived>::dynamics(const double& time, StateDerivative& stateDerivative) +bool DynamicalSystem<_Derived>::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) { return this->derived().dynamics(time, stateDerivative); } diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h index 574cbdbf81..7344131972 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h @@ -8,11 +8,11 @@ #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 @@ -30,33 +30,44 @@ class FixedBaseDynamics; } } // namespace BipedalLocomotion +namespace BipedalLocomotion::ContinuousDynamicalSystem::internal +{ +template <> struct traits +{ + using State = GenericContainer::named_tuple; + using StateDerivative = GenericContainer::named_tuple; + using Input = GenericContainer::named_tuple; + using DynamicalSystem = FixedBaseDynamics; +}; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal -// 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]; + * FixedBaseDynamics describes a the dynamics of a fixed base system + * The FixedBaseDynamics inherits from a generic DynamicalSystem where the State is + * described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:---------------------------:| + * | `s` | `Eigen::VectorXd` | Joint positions [in rad] | + * | `ds` | `Eigen::VectorXd` | Joint velocities [in rad/s] | + * + * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:--------------------------------:| + * | `ds` | `Eigen::VectorXd` | Joint velocities [in rad] | + * | dds` | `Eigen::VectorXd` | Joint accelerations [in rad/s^2] | + * + * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:--------------------------------:| + * |`tau` | `Eigen::VectorXd` | Joint torque [in N/m] | */ class FixedBaseDynamics : public DynamicalSystem { @@ -91,7 +102,7 @@ class FixedBaseDynamics : public DynamicalSystem * | `base_link` | `string` | Name of the link considered as fixed base in the model. If not defined the default link will be used. Please check [here](https://robotology.github.io/idyntree/master/classiDynTree_1_1Model.html#a1a8dc1c97b99ffc51dbf93ecff20e8c1) | No | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler); + bool initialize(std::weak_ptr handler); /** * Set the model of the robot. @@ -118,7 +129,7 @@ class FixedBaseDynamics : public DynamicalSystem * @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); + bool dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative); /** * Set the state of the dynamical system. diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h index 7c293cca05..a424e52614 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_STEP_INTEGRATOR_H #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_STEP_INTEGRATOR_H +#include #include #include @@ -43,20 +44,20 @@ class FixedStepIntegrator : public Integrator> using StateDerivative = typename internal::traits>::StateDerivative; protected: - double m_dT{-1}; /**< Fixed step size */ + std::chrono::nanoseconds m_dT{std::chrono::nanoseconds::min()}; /**< Fixed step size */ public: /** * Set the integration step time * @param dT integration step time */ - bool setIntegrationStep(const double& dT); + bool setIntegrationStep(const std::chrono::nanoseconds& dT); /** * get the integration step time * @return the integration step time */ - double getIntegrationStep() const; + const std::chrono::nanoseconds& getIntegrationStep() const; /** * Integrate the dynamical system from initialTime to finalTime. @@ -65,12 +66,13 @@ class FixedStepIntegrator : public Integrator> * @param finalTime final time of the integration. * @return true in case of success, false otherwise. */ - bool integrate(double initialTime, double finalTime); + bool integrate(const std::chrono::nanoseconds& initialTime, + const std::chrono::nanoseconds& finalTime); }; -template bool FixedStepIntegrator<_Derived>::setIntegrationStep(const double& dT) +template bool FixedStepIntegrator<_Derived>::setIntegrationStep(const std::chrono::nanoseconds& dT) { - if (dT <= 0) + if (dT <= std::chrono::nanoseconds::zero()) { log()->error("[FixedStepIntegrator::setIntegrationStep] The integration must be a strict " "positive number"); @@ -82,13 +84,15 @@ template bool FixedStepIntegrator<_Derived>::setIntegrationStep return true; } -template double FixedStepIntegrator<_Derived>::getIntegrationStep() const +template +const std::chrono::nanoseconds& FixedStepIntegrator<_Derived>::getIntegrationStep() const { return m_dT; } template -bool FixedStepIntegrator<_Derived>::integrate(double initialTime, double finalTime) +bool FixedStepIntegrator<_Derived>::integrate(const std::chrono::nanoseconds& initialTime, + const std::chrono::nanoseconds& finalTime) { constexpr auto errorPrefix = "[FixedStepIntegrator::integrate]"; @@ -104,15 +108,14 @@ bool FixedStepIntegrator<_Derived>::integrate(double initialTime, double finalTi return false; } - if (m_dT <= 0) + if (m_dT <= std::chrono::nanoseconds::zero()) { log()->error("{} Please set the integration step.", errorPrefix); return false; } - int iterations = std::ceil((finalTime - initialTime) / m_dT); - - double currentTime = initialTime; + const std::size_t iterations = (finalTime - initialTime) / m_dT; + std::chrono::nanoseconds currentTime = initialTime; for (std::size_t i = 0; i < iterations - 1; i++) { // advance the current time @@ -126,7 +129,7 @@ bool FixedStepIntegrator<_Derived>::integrate(double initialTime, double finalTi } // Consider last step separately to be sure that the last solution point is in finalTime - const double dT = finalTime - currentTime; + const std::chrono::nanoseconds dT = finalTime - currentTime; if (!static_cast<_Derived*>(this)->oneStepIntegration(currentTime, dT)) { log()->error("{} Error while integrating the last step.", errorPrefix); diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h index 7c1889ce7e..229a20b7e5 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseDynamicsWithCompliantContacts.h @@ -8,6 +8,7 @@ #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 @@ -118,7 +119,7 @@ class FloatingBaseDynamicsWithCompliantContacts * | `base_link` | `string` | Name of the link considered as fixed base in the model. If not defined the default link will be used. Please check [here](https://robotology.github.io/idyntree/master/classiDynTree_1_1Model.html#a1a8dc1c97b99ffc51dbf93ecff20e8c1) | No | * @return true in case of success/false otherwise. */ - bool initialize(std::weak_ptr handler); + bool initialize(std::weak_ptr handler); /** * Set the model of the robot. @@ -145,7 +146,7 @@ class FloatingBaseDynamicsWithCompliantContacts * @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); + bool dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative); /** * Set the state of the dynamical system. diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h index 7185b57718..7aae011a6b 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -28,17 +29,22 @@ class FloatingBaseSystemKinematics; } } -// Please read it as -// BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( -// FloatingBaseSystemKinematics, -// (base position, base orientation, joint positions), -// (base linear velocity, base angular velocity, joint velocities), -// (base twist expressed in mixed representation, joint velocities)) -BLF_DEFINE_CONTINUOUS_DYNAMICAL_SYSTEM_INTERAL_STRUCTURE( - FloatingBaseSystemKinematics, - (Eigen::Vector3d, manif::SO3d, Eigen::VectorXd), - (Eigen::Vector3d, manif::SO3d::Tangent, Eigen::VectorXd), - (Eigen::Matrix, Eigen::VectorXd)); +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 = FloatingBaseSystemKinematics; +}; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal namespace BipedalLocomotion { @@ -47,20 +53,26 @@ 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 - * - 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; - * - 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; - * - manif::SO3d::Tangent: base angular velocity w.r.t. the inertial frame; (Left trivialized) - * 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]. + * The FloatingBaseSystemKinematics 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 velocity of the origin of the base link whose coordinates are expressed in the Inertial frame (MIXED RERPESENTATION) | + * | `omega` | `manif::SO3d::Tangent` | base angular velocity 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 FloatingBaseSystemKinematics : public DynamicalSystem { @@ -74,7 +86,7 @@ class FloatingBaseSystemKinematics : public DynamicalSystem handler); + bool initialize(std::weak_ptr handler); /** * Set the state of the dynamical system. @@ -104,7 +116,7 @@ class FloatingBaseSystemKinematics : public DynamicalSystem #include #include #include #include +#include namespace BipedalLocomotion { @@ -60,21 +62,24 @@ class ForwardEuler : public FixedStepIntegrator> /** 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) + template + inline typename std::enable_if::value, void>::type + addArea(const StateDerivative& dx, const std::chrono::nanoseconds& dT, State& x) { - static_assert(sizeof...(Tp) == sizeof...(Td)); + static_assert(std::tuple_size::value == std::tuple_size::value); } - template - inline typename std::enable_if < I::type - addArea(const std::tuple& dx, const double& dT, std::tuple& x) + template + inline typename std::enable_if<(I < std::tuple_size::value), void>::type + addArea(const StateDerivative& dx, const std::chrono::nanoseconds& dT, State& x) { - static_assert(sizeof...(Tp) == sizeof...(Td)); + static_assert(std::tuple_size::value == std::tuple_size::value); // the order matters since we assume that all the velocities are left trivialized. - std::get(x) = (std::get(dx) * dT) + std::get(x); + using std::get; + + // convert the dT in seconds + get(x) = (get(dx) * std::chrono::duration(dT).count()) + get(x); addArea(dx, dT, x); } @@ -85,11 +90,12 @@ class ForwardEuler : public FixedStepIntegrator> * @param dT sampling time. * @return true in case of success, false otherwise. */ - bool oneStepIntegration(double t0, double dT); + bool oneStepIntegration(const std::chrono::nanoseconds& t0, const std::chrono::nanoseconds& dT); }; template -bool ForwardEuler<_DynamicalSystem>::oneStepIntegration(double t0, double dT) +bool ForwardEuler<_DynamicalSystem>::oneStepIntegration(const std::chrono::nanoseconds& t0, + const std::chrono::nanoseconds& dT) { constexpr auto errorPrefix = "[ForwardEuler::oneStepIntegration]"; if (this->m_dynamicalSystem == nullptr) diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h index 10611d274d..b851f8f198 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H +#include #include #include @@ -81,7 +82,8 @@ template class Integrator * @param finalTime final time of the integration. * @return true in case of success, false otherwise. */ - bool integrate(double initialTime, double finalTime); + bool integrate(const std::chrono::nanoseconds& initialTime, + const std::chrono::nanoseconds& finalTime); }; template @@ -119,7 +121,9 @@ const typename Integrator<_Derived>::State& Integrator<_Derived>::getSolution() return m_dynamicalSystem->getState(); } -template bool Integrator<_Derived>::integrate(double initialTime, double finalTime) +template +bool Integrator<_Derived>::integrate(const std::chrono::nanoseconds& initialTime, + const std::chrono::nanoseconds& finalTime) { return static_cast<_Derived*>(this)->integrate(initialTime, finalTime); } diff --git a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h index 1a31da7e85..e06fd9f443 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h @@ -13,6 +13,7 @@ #include #include +#include #include @@ -27,11 +28,16 @@ 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::ContinuousDynamicalSystem::internal +{ +template <> struct traits +{ + using State = GenericContainer::named_tuple; + using StateDerivative = GenericContainer::named_tuple; + using Input = GenericContainer::named_tuple; + using DynamicalSystem = LinearTimeInvariantSystem; +}; +} // namespace BipedalLocomotion::ContinuousDynamicalSystem::internal namespace BipedalLocomotion { @@ -42,13 +48,21 @@ 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. + * The LinearTimeInvariantSystem inherits from a generic DynamicalSystem where the State is + * described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:-------------------------------------------------:| + * | `x` | `Eigen::VectorXd` | A generic vector belonging to \f$\mathbb{R}^n\f$ | + * + * The `StateDerivative` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:----------------------------------------------------------:| + * | `dx` | `Eigen::VectorXd` | A state vector derivative belonging to \f$\mathbb{R}^n\f$ | + * + * The `Input` is described by a BipedalLocomotion::GenericContainer::named_tuple + * | Name | Type | Description | + * |:----:|:-----------------:|:-------------------------------------------------:| + * | `u` | `Eigen::VectorXd` | A control vector belonging to \f$\mathbb{R}^m\f$ | */ class LinearTimeInvariantSystem : public DynamicalSystem { @@ -75,7 +89,7 @@ class LinearTimeInvariantSystem : public DynamicalSystem handler); + bool initialize(std::weak_ptr handler); /** * Set the state of the dynamical system. @@ -107,7 +121,7 @@ class LinearTimeInvariantSystem : public DynamicalSystem +#include +#include + +using namespace BipedalLocomotion; +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion::ParametersHandler; + +bool CentroidalDynamics::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[CentroidalDynamics::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()); + } + + if (!ptr->getParameter("mass", m_mass)) + { + log()->info("{} The mass is not found. The default one will be used {} kg.", + logPrefix, + m_mass); + } + + return true; +} + +bool CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) +{ + using namespace BipedalLocomotion::GenericContainer::literals; + + const auto& [comPosition, comVelocity, angularMomentum] = m_state; + auto& [comVelocityOut, comAcceleration, angularMomentumRate] = stateDerivative; + + comVelocityOut = comVelocity; + comAcceleration = m_gravity; + angularMomentumRate.setZero(); + + const auto& contacts = m_controlInput.get_from_hash<"contacts"_h>(); + const auto& externalDisturbance = m_controlInput.get_from_hash<"external_force"_h>(); + if (externalDisturbance) + { + comAcceleration += externalDisturbance.value(); + } + + for (const auto& [key, contact] : contacts) + { + for (const auto& corner : contact.corners) + { + comAcceleration.noalias() += 1 / m_mass * corner.force; + angularMomentumRate.noalias() + += (contact.pose.act(corner.position) - comPosition).cross(corner.force); + } + } + + return true; +} + +bool CentroidalDynamics::setState(const State& state) +{ + m_state = state; + return true; +} + +const CentroidalDynamics::State& CentroidalDynamics::getState() const +{ + return m_state; +} + +bool CentroidalDynamics::setControlInput(const Input& controlInput) +{ + m_controlInput = controlInput; + return true; +} diff --git a/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp b/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp index 945605a4d3..af764f3c49 100644 --- a/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp +++ b/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp @@ -7,8 +7,12 @@ #include #include +#include #include +#include +#include + using namespace BipedalLocomotion::ContinuousDynamicalSystem; bool FirstOrderSmoother::initialize( @@ -29,7 +33,7 @@ bool FirstOrderSmoother::initialize( return false; } - double samplingTime{-1}; + std::chrono::nanoseconds samplingTime; if (!ptr->getParameter("sampling_time", samplingTime)) { log()->error("{} Unable to get the 'sampling_time' parameter.", logPrefix); @@ -54,6 +58,8 @@ bool FirstOrderSmoother::initialize( bool FirstOrderSmoother::reset(Eigen::Ref initialPoint) { + using namespace BipedalLocomotion::GenericContainer::literals; + constexpr auto logPrefix = "[FirstOrderSmoother::reset]"; m_isInitialStateSet = false; @@ -75,7 +81,8 @@ bool FirstOrderSmoother::reset(Eigen::Ref initialPoint) log()->error("{} Unable to set the linear system matrices.", logPrefix); return false; } - if (!m_linearSystem->setState(initialPoint)) + + if (!m_linearSystem->setState({initialPoint})) { log()->error("{} Unable to initialize the system.", logPrefix); return false; @@ -99,7 +106,8 @@ bool FirstOrderSmoother::advance() return false; } - if (!m_integrator.integrate(0, m_integrator.getIntegrationStep())) + using namespace std::chrono_literals; + if (!m_integrator.integrate(0s, m_integrator.getIntegrationStep())) { log()->error("[FirstOrderSmoother::advance] Unable to propagate the dynamical system."); return false; @@ -120,7 +128,7 @@ bool FirstOrderSmoother::setInput(const Eigen::VectorXd& input) return false; } - return m_linearSystem->setControlInput(input); + return m_linearSystem->setControlInput({input}); } const Eigen::VectorXd& FirstOrderSmoother::getOutput() const diff --git a/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp index ce5197e207..bc6e3a524f 100644 --- a/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp +++ b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp @@ -17,7 +17,7 @@ using namespace BipedalLocomotion; using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::ParametersHandler; -bool FixedBaseDynamics::initialize(std::weak_ptr handler) +bool FixedBaseDynamics::initialize(std::weak_ptr handler) { constexpr auto logPrefix = "[FixedBaseDynamics::initialize]"; @@ -129,7 +129,7 @@ bool FixedBaseDynamics::setMassMatrixRegularization(const Eigen::Ref handler) +bool FloatingBaseDynamicsWithCompliantContacts::initialize( + std::weak_ptr handler) { constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::initialize]"; @@ -142,7 +143,7 @@ bool FloatingBaseDynamicsWithCompliantContacts::setMassMatrixRegularization( return true; } -bool FloatingBaseDynamicsWithCompliantContacts::dynamics(const double& time, +bool FloatingBaseDynamicsWithCompliantContacts::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) { constexpr auto logPrefix = "[FloatingBaseDynamicsWithCompliantContacts::dynamics]"; diff --git a/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp index f2fc203490..2fb4cdeab5 100644 --- a/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp +++ b/src/ContinuousDynamicalSystem/src/FloatingBaseSystemKinematics.cpp @@ -11,7 +11,7 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::ParametersHandler; -bool FloatingBaseSystemKinematics::initialize(std::weak_ptr handler) +bool FloatingBaseSystemKinematics::initialize(std::weak_ptr handler) { constexpr auto logPrefix = "[FloatingBaseSystemKinematics::initialize]"; @@ -30,21 +30,23 @@ bool FloatingBaseSystemKinematics::initialize(std::weak_ptr return true; } -bool FloatingBaseSystemKinematics::dynamics(const double& time, +bool FloatingBaseSystemKinematics::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) { + using namespace BipedalLocomotion::GenericContainer::literals; + // get the state - const Eigen::Vector3d& basePosition = std::get<0>(m_state); - const manif::SO3d& baseRotation = std::get<1>(m_state); - const Eigen::VectorXd& jointPositions = std::get<2>(m_state); + const Eigen::Vector3d& basePosition = m_state.get_from_hash<"p"_h>(); + const manif::SO3d& baseRotation = m_state.get_from_hash<"R"_h>(); + const Eigen::VectorXd& jointPositions = m_state.get_from_hash<"s"_h>(); // get the state derivative - Eigen::Vector3d& baseLinearVelocity = std::get<0>(stateDerivative); - manif::SO3d::Tangent& baseAngularVelocity = std::get<1>(stateDerivative); - Eigen::VectorXd& jointVelocityOutput = std::get<2>(stateDerivative); + Eigen::Vector3d& baseLinearVelocity = stateDerivative.get_from_hash<"dp"_h>(); + manif::SO3d::Tangent& baseAngularVelocity = stateDerivative.get_from_hash<"omega"_h>(); + Eigen::VectorXd& jointVelocityOutput = stateDerivative.get_from_hash<"ds"_h>(); - const Eigen::Matrix& baseTwist = std::get<0>(m_controlInput); - const Eigen::VectorXd& jointVelocity = std::get<1>(m_controlInput); + const Eigen::Matrix& baseTwist = m_controlInput.get_from_hash<"twist"_h>(); + const Eigen::VectorXd& jointVelocity = m_controlInput.get_from_hash<"ds"_h>(); // check the size of the vectors if (jointVelocity.size() != jointPositions.size()) diff --git a/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp b/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp index dcb45402d2..a4385a9e62 100644 --- a/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp +++ b/src/ContinuousDynamicalSystem/src/LinearTimeInvariantSystem.cpp @@ -6,6 +6,7 @@ */ #include +#include #include using namespace BipedalLocomotion::ContinuousDynamicalSystem; @@ -33,7 +34,7 @@ bool LinearTimeInvariantSystem::setSystemMatrices(const Eigen::Ref(m_state); - auto& dx = std::get<0>(stateDerivative); - const auto& u = std::get<0>(m_controlInput); + using namespace BipedalLocomotion::GenericContainer::literals; + + const Eigen::VectorXd& x = m_state.get_from_hash<"x"_h>(); + Eigen::VectorXd& dx = stateDerivative.get_from_hash<"dx"_h>(); + const Eigen::VectorXd& u = m_controlInput.get_from_hash<"u"_h>(); if (x.size() != m_A.rows()) { @@ -69,7 +72,7 @@ bool LinearTimeInvariantSystem::dynamics(const double& time, StateDerivative& st } bool LinearTimeInvariantSystem::initialize( - std::weak_ptr handler) + std::weak_ptr handler) { return true; } diff --git a/src/ContinuousDynamicalSystem/tests/FirstOrderSmoother.cpp b/src/ContinuousDynamicalSystem/tests/FirstOrderSmoother.cpp index 29980fde91..b9ea84d53c 100644 --- a/src/ContinuousDynamicalSystem/tests/FirstOrderSmoother.cpp +++ b/src/ContinuousDynamicalSystem/tests/FirstOrderSmoother.cpp @@ -5,6 +5,7 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include #include #include @@ -20,7 +21,9 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem; TEST_CASE("First order smoother") { - constexpr double dT = 0.0001; + using namespace std::chrono_literals; + + constexpr std::chrono::nanoseconds dT = 100us; constexpr double settlingTime = 0.1; constexpr double tolerance = 1e-2; @@ -56,13 +59,14 @@ TEST_CASE("First order smoother") const Eigen::Vector2d output = smoother.getOutput(); // check if the solution is similar to the expected one - REQUIRE(output.isApprox(closeFormSolution(dT * i), tolerance)); + REQUIRE(output.isApprox(closeFormSolution(std::chrono::duration(dT * i).count()), + tolerance)); if (!settilingSubSystem1) { if ((output[0] > 0.95) && (output[0] < 1.05)) { - settilingTimeSubSystem1 = i * dT; + settilingTimeSubSystem1 = std::chrono::duration(dT * i).count(); settilingSubSystem1 = true; } } else if ((output[0] < 0.95) || (output[0] > 1.05)) @@ -75,7 +79,7 @@ TEST_CASE("First order smoother") { if ((output[1] > 0.95) && (output[1] < 1.05)) { - settilingTimeSubSystem2 = i * dT; + settilingTimeSubSystem2 = std::chrono::duration(dT * i).count(); settilingSubSystem2 = true; } } else if ((output[1] < 0.95) || (output[1] > 1.05)) diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp index b86e0f793f..2aa8d0685f 100644 --- a/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp +++ b/src/ContinuousDynamicalSystem/tests/IntegratorFloatingBaseSystemKinematics.cpp @@ -5,6 +5,7 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include #include // Catch2 @@ -21,9 +22,10 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem; TEST_CASE("Integrator - Linear system") { - constexpr double dT = 0.0001; + using namespace std::chrono_literals; + constexpr std::chrono::nanoseconds dT = 100us; constexpr double tolerance = 1e-3; - constexpr double simulationTime = 0.5; + constexpr std::chrono::nanoseconds simulationTime = 500ms; auto system = std::make_shared(); @@ -65,12 +67,12 @@ TEST_CASE("Integrator - Linear system") const auto& [basePosition, baseRotation, jointPosition] = integrator.getSolution(); const auto& [basePositionExact, baseRotationExact, jointPositionExact] - = closeFormSolution(dT * i); + = closeFormSolution(std::chrono::duration(dT * i).count()); REQUIRE(baseRotation.rotation().isApprox(baseRotationExact, tolerance)); REQUIRE(basePosition.isApprox(basePositionExact, tolerance)); REQUIRE(jointPosition.isApprox(jointPositionExact, tolerance)); - REQUIRE(integrator.integrate(0, dT)); + REQUIRE(integrator.integrate(0s, dT)); } } diff --git a/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp b/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp index cc2e610790..affa94b208 100644 --- a/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp +++ b/src/ContinuousDynamicalSystem/tests/IntegratorLinearSystem.cpp @@ -5,6 +5,7 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include #include // Catch2 @@ -19,9 +20,11 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem; TEST_CASE("Integrator - Linear system") { - constexpr double dT = 0.0001; + using namespace std::chrono_literals; + + constexpr std::chrono::nanoseconds dT = 100us; constexpr double tolerance = 1e-3; - constexpr double simulationTime = 2; + constexpr std::chrono::nanoseconds simulationTime = 2s; // Create the linear system /** @@ -67,7 +70,8 @@ TEST_CASE("Integrator - Linear system") { const auto& [solution] = integrator.getSolution(); - REQUIRE(solution.isApprox(closeFormSolution(dT * i), tolerance)); - REQUIRE(integrator.integrate(0, dT)); + REQUIRE(solution.isApprox(closeFormSolution(std::chrono::duration(dT * i).count()), + tolerance)); + REQUIRE(integrator.integrate(0s, dT)); } } diff --git a/src/ContinuousDynamicalSystem/tests/MultiStateWeightProvider.cpp b/src/ContinuousDynamicalSystem/tests/MultiStateWeightProvider.cpp index d67ea9b59d..f436f9a857 100644 --- a/src/ContinuousDynamicalSystem/tests/MultiStateWeightProvider.cpp +++ b/src/ContinuousDynamicalSystem/tests/MultiStateWeightProvider.cpp @@ -5,6 +5,8 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ +#include + // Catch2 #include @@ -17,7 +19,8 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem; TEST_CASE("Multistate weight provider") { - constexpr double dT = 0.01; + using namespace std::chrono_literals; + constexpr std::chrono::nanoseconds dT = 10ms; constexpr double settlingTime = 0.1; constexpr double tolerance = 1e-2; diff --git a/src/GenericContainer/CMakeLists.txt b/src/GenericContainer/CMakeLists.txt index bc6c088f19..aba50da509 100644 --- a/src/GenericContainer/CMakeLists.txt +++ b/src/GenericContainer/CMakeLists.txt @@ -3,11 +3,14 @@ # BSD-3-Clause license. # set target name + +set(H_PREFIX include/BipedalLocomotion/GenericContainer) + add_bipedal_locomotion_library( NAME GenericContainer - PUBLIC_HEADERS include/BipedalLocomotion/GenericContainer/Vector.h include/BipedalLocomotion/GenericContainer/TemplateHelpers.h + PUBLIC_HEADERS ${H_PREFIX}/Vector.h ${H_PREFIX}/TemplateHelpers.h ${H_PREFIX}/NamedTuple.h PUBLIC_LINK_LIBRARIES iDynTree::idyntree-core Eigen3::Eigen - SUBDIRECTORIES tests + SUBDIRECTORIES tests IS_INTERFACE) diff --git a/src/GenericContainer/include/BipedalLocomotion/GenericContainer/NamedTuple.h b/src/GenericContainer/include/BipedalLocomotion/GenericContainer/NamedTuple.h new file mode 100644 index 0000000000..8e9e297f4c --- /dev/null +++ b/src/GenericContainer/include/BipedalLocomotion/GenericContainer/NamedTuple.h @@ -0,0 +1,399 @@ +/** + * @file NamedTuple.h + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +// the original version of the code can be found https://github.com/GiulioRomualdi/named_tuple + +#ifndef BIPEDAL_LOCOMOTION_GENERIC_CONTAINER_NAMED_TUPLE_H +#define BIPEDAL_LOCOMOTION_GENERIC_CONTAINER_NAMED_TUPLE_H + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ + +namespace GenericContainer +{ + +using hash_type = std::uint64_t; + +namespace detail +{ + +// The following code has been taken from https://gist.github.com/Manu343726/081512c43814d098fe4b +// it allows to generate a compile time string hash +constexpr hash_type fnv_basis = 14695981039346656037ull; +constexpr hash_type fnv_prime = 109951162821ull; + +// FNV-1a 64 bit hash +constexpr hash_type sid_hash(const char* str, hash_type hash = fnv_basis) noexcept +{ + return *str ? sid_hash(str + 1, (hash ^ *str) * fnv_prime) : hash; +} + +} // namespace detail + +/** + * named_param is a struct that associate a compile time hash to a value. + * @note Type Ts must be default constructible. + */ +template struct named_param +{ + static constexpr hash_type hash = hash_; + using Type = Type_; + + Type param; /**< Parameter associated to the given hash */ + + named_param() = default; + + template + named_param(const OtherType& param_) + : param(param_){}; + + template named_param& operator=(const OtherType& param_) + { + param = param_; + return *this; + } +}; + +/** + * named_tuple is a class that inherits from tuple. Its main purpose is to make more readable and + * less error prone code that uses `std::tuple`. + * \code{.cpp} + * using namespace BipedalLocomotion::GenericContainer::literals; + * + * auto foo = + * BipedalLocomotion::GenericContainer::make_named_tuple(BipedalLocomotion::GenericContainer::named_param<"name"_h, std::string>(), + * BipedalLocomotion::GenericContainer::named_param<"number"_h, int>()); + * + * // it is still possible to access the elements of the named_tuple with structured binding + * // declaration + * auto& [a, b] = foo; + * b = 150; + * + * // moreover you can call std::get<> as usual + * int n = std::get<1>(foo); + * + * // Differently from a normal tuple it is possible to access to the element via the hash as + * // follows + * foo.get_from_hash<"name"_h>() = "Smith"; + * const std::string& temp_string = foo.get_from_hash<"name"_h>(); + * \endcode + */ +template struct named_tuple : public std::tuple +{ + + /** Underlying tuple that can be generate from this named_tuple */ + using underlying_tuple = std::tuple; + + /** + * Constructor + */ + constexpr named_tuple() + : std::tuple() + { + } + + /** + * Constructor + */ + named_tuple(Params&&... args) + : std::tuple(std::forward(args)...) + { + } + + /** + * Construct from a tuple. + * @param t a tuple having the same number of elements of of named_tuple. + * @note This constructor requires the type of the elements of the tuple to be convertible into + * Params. + */ + template + named_tuple(const std::tuple& t) + : std::tuple(t) + { + static_assert(sizeof...(Params) == sizeof...(Args), + "Invalid number of parameters in the std::tuple"); + } + + /** + * Construct from a tuple. + * @param t a tuple having the same number of elements of named_tuple. + * @note This constructor requires the type of the elements of the tuple to be convertible into + * Params. + */ + template + named_tuple(std::tuple&& t) + : std::tuple(std::forward>(t)) + { + static_assert(sizeof...(Params) == sizeof...(Args), + "Invalid number of parameters in the std::tuple"); + } + + /** + * Copy from a tuple. + * @param t tuple + */ + template named_tuple& operator=(const std::tuple& t) + { + static_assert(sizeof...(Params) == sizeof...(Args), + "Invalid number of parameters in the std::tuple"); + this->copy_from_tuple(t); + return *this; + } + + /** + * Extracts the Ith element from the named_tuple. It must be an integer value in `[0, + * sizeof...(Types))`. + * @param t the named_tuple + * @return A reference to the selected element of t. + */ + template + const typename std::tuple_element::type& get() const noexcept + { + const auto& param = (std::get(static_cast&>(*this))); + return param.param; + } + + /** + * Extracts the Ith element from the named_tuple. I must be an integer value in `[0, + * sizeof...(Types))`. + * @param t the named_tuple + * @return A reference to the selected element of t. + */ + template typename std::tuple_element::type& get() noexcept + { + auto& param = (std::get(static_cast&>(*this))); + return param.param; + } + + /** + * Extracts the element from the named_tuple associated to the hash `Hash`. `Hash` must be + * among one of the hash values associated to the tuple when created. + * @@return A reference to the selected element of t. + */ + template const auto& get_from_hash() const noexcept + { + constexpr std::size_t index = get_element_index(); + static_assert((index != error), "Wrong named tuple key."); + return this->get(); + } + + /** + * Extracts the element from the named_tuple associated to the hash `Hash`. `Hash` must be + * among one of the hash values associated to the tuple when created. + * @@return A reference to the selected element of t. + */ + template auto& get_from_hash() noexcept + { + constexpr std::size_t index = get_element_index(); + static_assert((index != error), "Wrong named tuple key."); + return this->get(); + } + + /** + * Return the associated std::tuple + * @return the tuple associated to the named_tuple + */ + underlying_tuple to_tuple() const noexcept + { + underlying_tuple temp; + this->copy_to_tuple(temp); + return temp; + } + +private: + /** + * Copy an element of a named_tuple in tuple. + * @param t a tuple + * @note This ends the compile time recursive function + */ + template + inline typename std::enable_if::type + copy_to_tuple(underlying_tuple& t) const + { + } + + /** + * Copy an element of a named_tuple in tuple. + * @param t a tuple + * @note This is a compile time recursive function + */ + template + inline typename std::enable_if<(I < sizeof...(Params)), void>::type + copy_to_tuple(underlying_tuple& t) const + { + // copy each element + std::get(t) = this->get(); + this->copy_to_tuple(t); + } + + /** + * Copy an element of a tuple in named_tuple. + * @param t a tuple + * @note This ends the compile time recursive function + */ + template + inline typename std::enable_if::type + copy_from_tuple(const std::tuple& t) + { + } + + /** + * Copy an element of a tuple in named_tuple. + * @param t a tuple + * @note This is a compile time recursive function + */ + template + inline typename std::enable_if<(I < sizeof...(Params)), void>::type + copy_from_tuple(const std::tuple& t) + { + // copy each element + this->get() = std::get(t); + this->copy_from_tuple(t); + } + + /** + * Get the index of the element given the hash. + * @return an error flag. + * @note In this function is returned means that the hash has not be found. + */ + template + constexpr typename std::enable_if::type static get_element_index() noexcept + { + return error; + } + + /** + * Get the index of the element given the hash. + * @return The index of the element associated to the given hash. + * @note This function is resolved at compile time. So if the hash is not found an error flag + * will be returned. + */ + template + constexpr typename std::enable_if<(I < sizeof...(Params)), + const std::size_t>::type static get_element_index() noexcept + { + using element_type = typename std::tuple_element>::type; + if constexpr (Hash == element_type::hash) + { + return I; + } + return get_element_index(); + } + + static constexpr std::size_t error = -1; +}; + +/** + * Creates a named_tuple object, deducing the target type from the types of arguments. + * @params args zero or more arguments to construct the named_tuple from + * @return A named_tuple object containing the given values, created as if by + * `named_tuple(std::forward(args)...).` + */ +template constexpr auto make_named_tuple(Args&&... args) +{ + return named_tuple(std::forward(args)...); +} + +namespace literals +{ + +/** + * Forms a hash from a char* + * @note To use the literal operator please refer to the following code + * \code{.cpp} + * using namespace BipedalLocomotion::GenericContainer::literals; + * + * constexpr BipedalLocomotion::GenericContainer::hash_type hash = "foo"_h; + * \endcode + */ +constexpr hash_type operator"" _h(const char* tag, std::size_t s) +{ + return ::BipedalLocomotion::GenericContainer::detail::sid_hash(tag); +} + +} // namespace literals +} // namespace GenericContainer +} // namespace BipedalLocomotion + +namespace std +{ + +/** + * Extracts the Ith element from the named_tuple. It must be an integer value in `[0, + * sizeof...(Types))`. + * @param t the named_tuple + * @return A reference to the selected element of t. + */ +template +typename std::tuple_element< + I, + typename ::BipedalLocomotion::GenericContainer::named_tuple::underlying_tuple>::type& +get(::BipedalLocomotion::GenericContainer::named_tuple& t) noexcept +{ + return t.template get(); +} + +/** + * Extracts the Ith element from the named_tuple. It must be an integer value in `[0, + * sizeof...(Types))`. + * @param t the named_tuple + * @return A reference to the selected element of t. + */ +template +const typename tuple_element< + I, + typename ::BipedalLocomotion::GenericContainer::named_tuple::underlying_tuple>::type& +get(const ::BipedalLocomotion::GenericContainer::named_tuple& t) noexcept +{ + return t.template get(); +} + +/** + * Template specialization to make the elements of the named_tuple accessible with the Structured + * binding declaration + */ +template +struct tuple_size<::BipedalLocomotion::GenericContainer::named_tuple> + : integral_constant +{ +}; + +/** + * Template specialization to make the elements of the named_tuple accessible with the Structured + * binding declaration + */ +template +struct tuple_element> + : std::tuple_element< + Index, + typename ::BipedalLocomotion::GenericContainer::named_tuple::underlying_tuple> +{ +}; + +} // namespace std + +/** + * Create the hash from a string + * @param x_ key + */ +#define BLF_STRING_TO_HASH(x_) ::BipedalLocomotion::GenericContainer::detail::sid_hash(#x_) + +/** + * Create a named parameter given type and key + * @param x_ key + * @param type_ type of the parameter + */ +#define BLF_NAMED_PARAM(x_, type_) \ + ::BipedalLocomotion::GenericContainer::named_param + +#endif // BIPEDAL_LOCOMOTION_GENERIC_CONTAINER_NAMED_TUPLE_H diff --git a/src/GenericContainer/tests/CMakeLists.txt b/src/GenericContainer/tests/CMakeLists.txt index 1284b9bdfc..1428b09b9b 100644 --- a/src/GenericContainer/tests/CMakeLists.txt +++ b/src/GenericContainer/tests/CMakeLists.txt @@ -7,6 +7,11 @@ add_bipedal_test( SOURCES GenericContainerTest.cpp LINKS BipedalLocomotion::GenericContainer iDynTree::idyntree-core) +add_bipedal_test( + NAME NamedTuple + SOURCES NamedTupleTest.cpp + LINKS BipedalLocomotion::GenericContainer) + if (FRAMEWORK_HAS_YARP) add_bipedal_test( NAME GenericContainerPlusYarp diff --git a/src/GenericContainer/tests/NamedTupleTest.cpp b/src/GenericContainer/tests/NamedTupleTest.cpp new file mode 100644 index 0000000000..3bdb78c091 --- /dev/null +++ b/src/GenericContainer/tests/NamedTupleTest.cpp @@ -0,0 +1,125 @@ +/** + * @file NamedTupleTest.cpp + * @authors Giulio Romualdi + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +// Catch2 +#include +#include + +#include + +using namespace BipedalLocomotion::GenericContainer; + +class DummyClass +{ +public: + using State = named_tuple, + named_param, + named_param, + named_param>; + + const State& getState() const + { + return m_state; + } + + State& getState() + { + return m_state; + } + + void setState(const State& state) + { + m_state = state; + } + +private: + + State m_state; +}; + +TEST_CASE("Test Named Parameter") +{ + using namespace BipedalLocomotion::GenericContainer::literals; + named_param<"name"_h, std::string> param; + + param = "Olivia"; +} + +TEST_CASE("Test Numed Tuple - tuple operator") +{ + using namespace BipedalLocomotion::GenericContainer::literals; + DummyClass dummyClass; + + dummyClass.setState( + std::tuple{"Elisa", 12, 1.231, Eigen::Vector2d()}); + + REQUIRE(std::get<0>(dummyClass.getState()) == "Elisa"); + REQUIRE(dummyClass.getState().get_from_hash<"name"_h>() == "Elisa"); + + const DummyClass& dummyClassRef = dummyClass; + REQUIRE(std::get<0>(dummyClassRef.getState()) == "Elisa"); + REQUIRE(dummyClassRef.getState().get_from_hash<"name"_h>() == "Elisa"); +} + +TEST_CASE("Test NamedTuple") +{ + using namespace BipedalLocomotion::GenericContainer::literals; + + auto foo = make_named_tuple(named_param<"name"_h, std::string>(), + named_param<"number"_h, int>(), + named_param<"floating_number"_h, double>(), + named_param<"vector"_h, Eigen::VectorXd>()); + + SECTION("Get") + { + auto& [a, b, c, d] = foo; + a = "Smith"; + b = 150; + c = 3.14; + d = Eigen::VectorXd::Ones(10); + + REQUIRE(a == foo.get_from_hash<"name"_h>()); + REQUIRE(std::get<0>(foo) == foo.get_from_hash<"name"_h>()); + + REQUIRE(b == foo.get_from_hash<"number"_h>()); + REQUIRE(std::get<1>(foo) == foo.get_from_hash<"number"_h>()); + + REQUIRE(c == foo.get_from_hash<"floating_number"_h>()); + REQUIRE(std::get<2>(foo) == foo.get_from_hash<"floating_number"_h>()); + + REQUIRE(d == foo.get_from_hash<"vector"_h>()); + REQUIRE(std::get<3>(foo) == foo.get_from_hash<"vector"_h>()); + } + + SECTION("Set") + { + foo.get_from_hash<"name"_h>() = "Olivia"; + REQUIRE(std::get<0>(foo) == foo.get_from_hash<"name"_h>()); + + foo.get_from_hash<"number"_h>() = 42; + REQUIRE(std::get<1>(foo) == foo.get_from_hash<"number"_h>()); + + foo.get_from_hash<"floating_number"_h>() = 8.15; + REQUIRE(std::get<2>(foo) == foo.get_from_hash<"floating_number"_h>()); + + foo.get_from_hash<"vector"_h>() = Eigen::Vector4d::Constant(3); + REQUIRE(std::get<3>(foo) == foo.get_from_hash<"vector"_h>()); + } + + SECTION("From and to tuple") + { + auto tempTuple = foo.to_tuple(); + decltype(foo) newNamedTuple(tempTuple); + + decltype(foo) newNamedTuple2; + newNamedTuple2 = tempTuple; + } +} diff --git a/src/IK/include/BipedalLocomotion/IK/JointLimitsTask.h b/src/IK/include/BipedalLocomotion/IK/JointLimitsTask.h index 19b4c03948..1b9081f64c 100644 --- a/src/IK/include/BipedalLocomotion/IK/JointLimitsTask.h +++ b/src/IK/include/BipedalLocomotion/IK/JointLimitsTask.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_IK_JOINT_LIMITS_TASK_H #define BIPEDAL_LOCOMOTION_IK_JOINT_LIMITS_TASK_H +#include #include #include @@ -48,7 +49,7 @@ class JointLimitsTask : public IKLinearTask bool m_isLimitConsideredForAllJoints{false}; - double m_samplingTime; + std::chrono::nanoseconds m_samplingTime; std::string m_robotVelocityVariableName; diff --git a/src/IK/src/JointLimitsTask.cpp b/src/IK/src/JointLimitsTask.cpp index b6517f2d46..eb9a77f093 100644 --- a/src/IK/src/JointLimitsTask.cpp +++ b/src/IK/src/JointLimitsTask.cpp @@ -5,6 +5,7 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include #include #include @@ -154,9 +155,11 @@ bool JointLimitsTask::initialize( return false; } - if (!ptr->getParameter("sampling_time", m_samplingTime) || m_samplingTime < 0) + if (!ptr->getParameter("sampling_time", m_samplingTime) + || m_samplingTime <= std::chrono::nanoseconds::zero()) { - log()->error("{} Error while retrieving the 'sampling_time'. Please remember that must be a " + log()->error("{} Error while retrieving the 'sampling_time'. Please remember that must be " + "a " "positive number", errorPrefix); return false; @@ -202,6 +205,8 @@ bool JointLimitsTask::update() m_isValid = false; + const double dT = std::chrono::duration(m_samplingTime).count(); + if (!m_kinDyn->getJointPos(m_jointPosition)) { log()->error("{} Unable to get the joint position.", errorPrefix); @@ -211,10 +216,10 @@ bool JointLimitsTask::update() if (m_isLimitConsideredForAllJoints) { m_b.head(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() - = m_klim.asDiagonal() * (m_upperLimits - m_jointPosition) / m_samplingTime; + = m_klim.asDiagonal() * (m_upperLimits - m_jointPosition) / dT; m_b.tail(m_kinDyn->getNrOfDegreesOfFreedom()).noalias() - = m_klim.asDiagonal() * (-m_lowerLimits + m_jointPosition) / m_samplingTime; + = m_klim.asDiagonal() * (-m_lowerLimits + m_jointPosition) / dT; } else { Eigen::DenseBase>::SegmentReturnType upperLimitPart @@ -227,7 +232,7 @@ bool JointLimitsTask::update() if (m_upperLimits[i] == OsqpEigen::INFTY) { upperLimitPart(i) = m_klim(i) * (m_upperLimits(i) - m_jointPosition(i)) // - / m_samplingTime; + / dT; } } for (int i = 0; i < m_lowerLimits.size(); i++) @@ -235,7 +240,7 @@ bool JointLimitsTask::update() if (m_lowerLimits[i] == -OsqpEigen::INFTY) { lowerLimitPart(i) = m_klim(i) * (m_lowerLimits(i) - m_jointPosition(i)) // - / m_samplingTime; + / dT; } } } diff --git a/src/IK/tests/CMakeLists.txt b/src/IK/tests/CMakeLists.txt index 315bb9d17c..ff076c220e 100644 --- a/src/IK/tests/CMakeLists.txt +++ b/src/IK/tests/CMakeLists.txt @@ -37,14 +37,18 @@ add_bipedal_test( SOURCES AngularMomentumTaskTest.cpp LINKS BipedalLocomotion::IK) -add_bipedal_test( - NAME QPInverseKinematics - SOURCES QPInverseKinematicsTest.cpp - LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions - BipedalLocomotion::ContinuousDynamicalSystem) +if (FRAMEWORK_COMPILE_ContinuousDynamicalSystem) -add_bipedal_test( - NAME QPFixedBaseInverseKinematics - SOURCES QPFixedBaseInverseKinematicsTest.cpp - LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions - BipedalLocomotion::ContinuousDynamicalSystem) + add_bipedal_test( + NAME QPInverseKinematics + SOURCES QPInverseKinematicsTest.cpp + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions + BipedalLocomotion::ContinuousDynamicalSystem) + + add_bipedal_test( + NAME QPFixedBaseInverseKinematics + SOURCES QPFixedBaseInverseKinematicsTest.cpp + LINKS BipedalLocomotion::IK BipedalLocomotion::ManifConversions + BipedalLocomotion::ContinuousDynamicalSystem) + +endif() diff --git a/src/IK/tests/JointLimitsTaskTest.cpp b/src/IK/tests/JointLimitsTaskTest.cpp index 47f0b52d03..7a103784c0 100644 --- a/src/IK/tests/JointLimitsTaskTest.cpp +++ b/src/IK/tests/JointLimitsTaskTest.cpp @@ -5,6 +5,8 @@ * distributed under the terms of the BSD-3-Clause license. */ +#include + // Catch2 #include @@ -24,10 +26,11 @@ using namespace BipedalLocomotion::IK; TEST_CASE("Joint Regularization task") { + using namespace std::chrono_literals; const std::string robotVelocity = "robotVelocity"; Eigen::VectorXd klim; - constexpr double dt = 0.01; + constexpr std::chrono::nanoseconds dt = 10ms; auto kinDyn = std::make_shared(); auto parameterHandler = std::make_shared(); @@ -126,10 +129,10 @@ TEST_CASE("Joint Regularization task") // check the vector b Eigen::VectorXd expectedB; - expectedB = klim.asDiagonal() * deltaLimit / dt; + expectedB = klim.asDiagonal() * deltaLimit / std::chrono::duration(dt).count(); REQUIRE(b.head(model.getNrOfDOFs()).isApprox(expectedB)); - expectedB = klim.asDiagonal() * (deltaLimit) / dt; + expectedB = klim.asDiagonal() * (deltaLimit) / std::chrono::duration(dt).count(); REQUIRE(b.tail(model.getNrOfDOFs()).isApprox(expectedB)); } diff --git a/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp b/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp index b0d7582dde..b87dbb4754 100644 --- a/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp +++ b/src/IK/tests/QPFixedBaseInverseKinematicsTest.cpp @@ -36,9 +36,10 @@ using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::IK; using namespace BipedalLocomotion::Conversions; +using namespace std::chrono_literals; constexpr auto robotVelocity = "robotVelocity"; -constexpr double dT = 0.005; +constexpr std::chrono::nanoseconds dT = 5ms; struct InverseKinematicsAndTasks { @@ -281,7 +282,7 @@ TEST_CASE("QP-IK") // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); - system.integrator->integrate(0, dT); + system.integrator->integrate(0s, dT); } // Check the end-effector pose error diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp index df2e9d106f..d5810688e1 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -38,9 +38,10 @@ using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::IK; using namespace BipedalLocomotion::Conversions; +using namespace std::chrono_literals; constexpr auto robotVelocity = "robotVelocity"; -constexpr double dT = 0.01; +constexpr std::chrono::nanoseconds dT = 10ms; struct InverseKinematicsAndTasks { @@ -348,7 +349,7 @@ TEST_CASE("QP-IK") // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); - system.integrator->integrate(0, dT); + system.integrator->integrate(0s, dT); } // check the CoM position @@ -454,7 +455,7 @@ TEST_CASE("QP-IK [With strict limits]") // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); - system.integrator->integrate(0, dT); + system.integrator->integrate(0s, dT); } } @@ -554,7 +555,7 @@ TEST_CASE("QP-IK [With builder]") // propagate the dynamical system system.dynamics->setControlInput({baseVelocity, jointVelocity}); - system.integrator->integrate(0, dT); + system.integrator->integrate(0s, dT); } auto weightProvider diff --git a/src/TSID/tests/QPFixedBaseTSIDTest.cpp b/src/TSID/tests/QPFixedBaseTSIDTest.cpp index 5a6509fb3b..ab044fb4b7 100644 --- a/src/TSID/tests/QPFixedBaseTSIDTest.cpp +++ b/src/TSID/tests/QPFixedBaseTSIDTest.cpp @@ -6,25 +6,23 @@ */ // Catch2 -#include "BipedalLocomotion/System/ConstantWeightProvider.h" #include // std +#include #include #include // BipedalLocomotion +#include +#include #include #include +#include #include - #include -#include - #include - -#include -#include +#include #include #include @@ -34,11 +32,12 @@ using namespace BipedalLocomotion::System; using namespace BipedalLocomotion::ContinuousDynamicalSystem; using namespace BipedalLocomotion::Conversions; using namespace BipedalLocomotion::TSID; +using namespace std::chrono_literals; constexpr auto robotAcceleration = "robotAccelration"; constexpr auto jointTorques = "jointTorques"; constexpr int maxNumOfContacts = 0; -constexpr double dT = 0.01; +constexpr std::chrono::nanoseconds dT = 10ms; struct TSIDAndTasks { @@ -292,7 +291,7 @@ TEST_CASE("QP-TSID") // propagate the dynamical system system.dynamics->setControlInput({tsidAndTasks.tsid->getOutput().jointTorques}); - system.integrator->integrate(0, dT); + system.integrator->integrate(0s, dT); } // Check the end-effector pose error