Skip to content

Commit

Permalink
Use std::chrono::nanoseconds instead of double to handle the time in …
Browse files Browse the repository at this point in the history
…ContinuousDynamicalSystem
  • Loading branch information
GiulioRomualdi committed Apr 5, 2023
1 parent dda0377 commit ef0bfef
Show file tree
Hide file tree
Showing 29 changed files with 130 additions and 82 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,15 @@
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H
#define BIPEDAL_LOCOMOTION_BINDINGS_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H

#include <chrono>
#include <memory>
#include <string>

#include <BipedalLocomotion/ContinuousDynamicalSystem/FixedStepIntegrator.h>
#include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
#include <BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h>

#include <pybind11/chrono.h>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
Expand Down Expand Up @@ -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.");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)):
Expand All @@ -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))
Original file line number Diff line number Diff line change
Expand Up @@ -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)])

Expand All @@ -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))
4 changes: 3 additions & 1 deletion bindings/python/IK/tests/test_QP_inverse_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,11 @@
#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H
#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_CENROIDAL_DYNAMICS_H

#include <chrono>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>

#include <BipedalLocomotion/Contacts/Contact.h>
#include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
Expand All @@ -29,8 +30,8 @@ namespace BipedalLocomotion::ContinuousDynamicalSystem::internal
{
template <> struct traits<CentroidalDynamics>
{
using Contacts = std::unordered_map<std::string, //
BipedalLocomotion::Contacts::DiscreteGeometryContact>;
using Contacts = std::map<std::string, //
BipedalLocomotion::Contacts::DiscreteGeometryContact>;
using ExternalForce = std::optional<Eigen::Vector3d>;

using State = GenericContainer::named_tuple<BLF_NAMED_PARAM(com_pos, Eigen::Vector3d),
Expand Down Expand Up @@ -127,7 +128,7 @@ class CentroidalDynamics : public DynamicalSystem<CentroidalDynamics>
* @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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H
#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_DYNAMICAL_SYSTEM_H

#include <chrono>
#include <memory>
#include <tuple>

Expand Down Expand Up @@ -132,7 +133,7 @@ template <class _Derived> 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 <class _Derived>
Expand Down Expand Up @@ -160,7 +161,7 @@ bool DynamicalSystem<_Derived>::setControlInput(const typename DynamicalSystem<_
}

template <class _Derived>
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);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <chrono>
#include <memory>
#include <tuple>
#include <vector>
Expand Down Expand Up @@ -128,7 +129,7 @@ class FixedBaseDynamics : public DynamicalSystem<FixedBaseDynamics>
* @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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <chrono>
#include <cmath>

#include <BipedalLocomotion/ContinuousDynamicalSystem/Integrator.h>
Expand Down Expand Up @@ -43,20 +44,20 @@ class FixedStepIntegrator : public Integrator<FixedStepIntegrator<_Derived>>
using StateDerivative = typename internal::traits<FixedStepIntegrator<_Derived>>::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.
Expand All @@ -65,12 +66,13 @@ class FixedStepIntegrator : public Integrator<FixedStepIntegrator<_Derived>>
* @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 <class _Derived> bool FixedStepIntegrator<_Derived>::setIntegrationStep(const double& dT)
template <class _Derived> 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");
Expand All @@ -82,13 +84,15 @@ template <class _Derived> bool FixedStepIntegrator<_Derived>::setIntegrationStep
return true;
}

template <class _Derived> double FixedStepIntegrator<_Derived>::getIntegrationStep() const
template <class _Derived>
const std::chrono::nanoseconds& FixedStepIntegrator<_Derived>::getIntegrationStep() const
{
return m_dT;
}

template <class _Derived>
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]";

Expand All @@ -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
Expand All @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 <chrono>
#include <memory>
#include <vector>

Expand Down Expand Up @@ -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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class FloatingBaseSystemKinematics : public DynamicalSystem<FloatingBaseSystemKi
* @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);
};

} // namespace ContinuousDynamicalSystem
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FORWARD_EULER_H
#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_FORWARD_EULER_H

#include <chrono>
#include <tuple>
#include <type_traits>

Expand Down Expand Up @@ -63,20 +64,22 @@ class ForwardEuler : public FixedStepIntegrator<ForwardEuler<_DynamicalSystem>>

template <std::size_t I = 0>
inline typename std::enable_if<I == std::tuple_size<State>::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<State>::value == std::tuple_size<StateDerivative>::value);
}

template <std::size_t I = 0>
inline typename std::enable_if<(I < std::tuple_size<State>::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<State>::value == std::tuple_size<StateDerivative>::value);

// the order matters since we assume that all the velocities are left trivialized.
using std::get;
get<I>(x) = (get<I>(dx) * dT) + get<I>(x);

// convert the dT in seconds
get<I>(x) = (get<I>(dx) * std::chrono::duration<double>(dT).count()) + get<I>(x);
addArea<I + 1>(dx, dT, x);
}

Expand All @@ -87,11 +90,12 @@ class ForwardEuler : public FixedStepIntegrator<ForwardEuler<_DynamicalSystem>>
* @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 <class _DynamicalSystem>
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)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#ifndef BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H
#define BIPEDAL_LOCOMOTION_CONTINUOUS_DYNAMICAL_SYSTEM_INTEGRATOR_H

#include <chrono>
#include <memory>

#include <BipedalLocomotion/ContinuousDynamicalSystem/DynamicalSystem.h>
Expand Down Expand Up @@ -81,7 +82,8 @@ template <class _Derived> 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 <class _Derived>
Expand Down Expand Up @@ -119,7 +121,9 @@ const typename Integrator<_Derived>::State& Integrator<_Derived>::getSolution()
return m_dynamicalSystem->getState();
}

template <class _Derived> bool Integrator<_Derived>::integrate(double initialTime, double finalTime)
template <class _Derived>
bool Integrator<_Derived>::integrate(const std::chrono::nanoseconds& initialTime,
const std::chrono::nanoseconds& finalTime)
{
return static_cast<_Derived*>(this)->integrate(initialTime, finalTime);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ class LinearTimeInvariantSystem : public DynamicalSystem<LinearTimeInvariantSyst
* @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);
};

} // namespace ContinuousDynamicalSystem
Expand Down
2 changes: 1 addition & 1 deletion src/ContinuousDynamicalSystem/src/CentroidalDynamics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ bool CentroidalDynamics::initialize(std::weak_ptr<const IParametersHandler> 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;

Expand Down
Loading

0 comments on commit ef0bfef

Please sign in to comment.