From ef0bfefd63f44cee7a5140576fc4b273ec7676a1 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 4 Apr 2023 19:05:27 +0200 Subject: [PATCH] Use std::chrono::nanoseconds instead of double to handle the time in ContinuousDynamicalSystem --- .../ContinuousDynamicalSystem/Integrator.h | 4 ++- .../test_floating_base_system_kinematics.py | 8 +++-- .../tests/test_linear_system.py | 6 ++-- .../IK/tests/test_QP_inverse_kinematics.py | 4 ++- .../CentroidalDynamics.h | 9 +++--- .../DynamicalSystem.h | 5 ++-- .../FixedBaseDynamics.h | 3 +- .../FixedStepIntegrator.h | 29 ++++++++++--------- ...loatingBaseDynamicsWithCompliantContacts.h | 3 +- .../FloatingBaseSystemKinematics.h | 2 +- .../ContinuousDynamicalSystem/ForwardEuler.h | 14 +++++---- .../ContinuousDynamicalSystem/Integrator.h | 8 +++-- .../LinearTimeInvariantSystem.h | 2 +- .../src/CentroidalDynamics.cpp | 2 +- .../src/FirstOrderSmoother.cpp | 6 ++-- .../src/FixedBaseDynamics.cpp | 2 +- ...atingBaseDynamicsWithCompliantContacts.cpp | 2 +- .../src/FloatingBaseSystemKinematics.cpp | 2 +- .../src/LinearTimeInvariantSystem.cpp | 2 +- .../tests/FirstOrderSmoother.cpp | 12 +++++--- ...IntegratorFloatingBaseSystemKinematics.cpp | 10 ++++--- .../tests/IntegratorLinearSystem.cpp | 12 +++++--- .../tests/MultiStateWeightProvider.cpp | 5 +++- .../BipedalLocomotion/IK/JointLimitsTask.h | 3 +- src/IK/src/JointLimitsTask.cpp | 17 +++++++---- src/IK/tests/JointLimitsTaskTest.cpp | 9 ++++-- .../QPFixedBaseInverseKinematicsTest.cpp | 5 ++-- src/IK/tests/QPInverseKinematicsTest.cpp | 9 +++--- src/TSID/tests/QPFixedBaseTSIDTest.cpp | 17 +++++------ 29 files changed, 130 insertions(+), 82 deletions(-) diff --git a/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h b/bindings/python/ContinuousDynamicalSystem/include/BipedalLocomotion/bindings/ContinuousDynamicalSystem/Integrator.h index 984edfadbe..3eb50b2cc7 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 @@ -92,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/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h index b24b7d2f2b..c08a5bf6ad 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/CentroidalDynamics.h @@ -8,10 +8,11 @@ #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 @@ -29,8 +30,8 @@ namespace BipedalLocomotion::ContinuousDynamicalSystem::internal { template <> struct traits { - using Contacts = std::unordered_map; + using Contacts = std::map; using ExternalForce = std::optional; using State = GenericContainer::named_tuple * @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/DynamicalSystem.h b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h index 5c2839f6d9..a431dafb49 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H +#include #include #include @@ -132,7 +133,7 @@ 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 @@ -160,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 b436959437..7344131972 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FixedBaseDynamics.h @@ -8,6 +8,7 @@ #ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_BASE_DYNAMICS_H #define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FIXED_BASE_DYNAMICS_H +#include #include #include #include @@ -128,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 945847cdb7..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 @@ -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 4e2fa9940b..7aae011a6b 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h @@ -116,7 +116,7 @@ class FloatingBaseSystemKinematics : public DynamicalSystem #include #include @@ -63,20 +64,22 @@ class ForwardEuler : public FixedStepIntegrator> template inline typename std::enable_if::value, void>::type - addArea(const StateDerivative& dx, const double& dT, State& x) + addArea(const StateDerivative& dx, const std::chrono::nanoseconds& dT, State& x) { static_assert(std::tuple_size::value == std::tuple_size::value); } template inline typename std::enable_if<(I < std::tuple_size::value), void>::type - addArea(const StateDerivative& dx, const double& dT, State& x) + addArea(const StateDerivative& dx, const std::chrono::nanoseconds& dT, State& x) { static_assert(std::tuple_size::value == std::tuple_size::value); // the order matters since we assume that all the velocities are left trivialized. using std::get; - get(x) = (get(dx) * dT) + get(x); + + // convert the dT in seconds + get(x) = (get(dx) * std::chrono::duration(dT).count()) + get(x); addArea(dx, dT, x); } @@ -87,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 dc2b70f532..e06fd9f443 100644 --- a/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h +++ b/src/ContinuousDynamicalSystem/include/BipedalLocomotion/ContinuousDynamicalSystem/LinearTimeInvariantSystem.h @@ -121,7 +121,7 @@ class LinearTimeInvariantSystem : public DynamicalSystem hand return true; } -bool CentroidalDynamics::dynamics(const double& time, StateDerivative& stateDerivative) +bool CentroidalDynamics::dynamics(const std::chrono::nanoseconds& time, StateDerivative& stateDerivative) { using namespace BipedalLocomotion::GenericContainer::literals; diff --git a/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp b/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp index 13297ac930..af764f3c49 100644 --- a/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp +++ b/src/ContinuousDynamicalSystem/src/FirstOrderSmoother.cpp @@ -11,6 +11,7 @@ #include #include +#include using namespace BipedalLocomotion::ContinuousDynamicalSystem; @@ -32,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); @@ -105,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; diff --git a/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp index 276d1a3b2a..bc6e3a524f 100644 --- a/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp +++ b/src/ContinuousDynamicalSystem/src/FixedBaseDynamics.cpp @@ -129,7 +129,7 @@ bool FixedBaseDynamics::setMassMatrixRegularization(const Eigen::Ref #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/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/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