Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement NamedTuple and the CentroidalDynamics classes #642

Merged
merged 8 commits into from
Apr 6, 2023
3 changes: 3 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
@@ -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)
23 changes: 12 additions & 11 deletions bindings/python/ContinuousDynamicalSystem/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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>
@@ -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.");
Original file line number Diff line number Diff line change
@@ -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))
Original file line number Diff line number Diff line change
@@ -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))
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
@@ -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()
2 changes: 1 addition & 1 deletion cmake/BipedalLocomotionFrameworkDependencies.cmake
Original file line number Diff line number Diff line change
@@ -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
7 changes: 5 additions & 2 deletions src/ContinuousDynamicalSystem/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
)
Loading