-
Notifications
You must be signed in to change notification settings - Fork 38
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #716 from ami-iit/balancing_position
Restructure the balancing-position-control script
- Loading branch information
Showing
21 changed files
with
657 additions
and
157 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
63 changes: 63 additions & 0 deletions
63
bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,63 @@ | ||
/** | ||
* @file Constants.h | ||
* @authors Giulio Romualdi | ||
* @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
* distributed under the terms of the BSD-3-Clause license. | ||
*/ | ||
|
||
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H | ||
#define BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H | ||
|
||
#include <pybind11/eigen.h> | ||
#include <pybind11/pybind11.h> | ||
|
||
#include <BipedalLocomotion/Math/Wrench.h> | ||
|
||
namespace BipedalLocomotion | ||
{ | ||
namespace bindings | ||
{ | ||
namespace Math | ||
{ | ||
|
||
template <typename Scalar> struct WrenchTrampoline | ||
{ | ||
BipedalLocomotion::Math::Wrench<Scalar> wrench; | ||
}; | ||
|
||
template <typename Scalar> void CreateWrench(pybind11::module& module, const std::string& suffix) | ||
{ | ||
namespace py = ::pybind11; | ||
using namespace BipedalLocomotion::Math; | ||
|
||
py::class_<WrenchTrampoline<Scalar>>(module, ("Wrench" + suffix).c_str()) | ||
.def(py::init<>()) | ||
.def(py::init([](const Eigen::Matrix<Scalar, 6, 1>& vector) -> WrenchTrampoline<Scalar> { | ||
WrenchTrampoline<Scalar> tmp; | ||
tmp.wrench = vector; | ||
return tmp; | ||
})) | ||
.def("get_local_cop", | ||
[](const WrenchTrampoline<Scalar>& impl) -> Eigen::Matrix<Scalar, 3, 1> { | ||
return impl.wrench.getLocalCoP(); | ||
}) | ||
.def_property( | ||
"force", | ||
[](const WrenchTrampoline<Scalar>& impl) | ||
-> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.force(); }, | ||
[](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& force) { | ||
impl.wrench.force() = force; | ||
}) | ||
.def_property( | ||
"torque", | ||
[](const WrenchTrampoline<Scalar>& impl) | ||
-> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.torque(); }, | ||
[](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& torque) { | ||
impl.wrench.torque() = torque; | ||
}); | ||
} | ||
} // namespace Math | ||
} // namespace bindings | ||
} // namespace BipedalLocomotion | ||
|
||
#endif // BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,16 @@ | ||
# Copyright (C) 2023 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(TARGET BipedalLocomotion::SimplifiedModelControllers) | ||
|
||
set(H_PREFIX include/BipedalLocomotion/bindings/SimplifiedModelControllers) | ||
|
||
add_bipedal_locomotion_python_module( | ||
NAME SimplifiedModelControllersBindings | ||
SOURCES src/CoMZMPController.cpp src/Module.cpp | ||
HEADERS ${H_PREFIX}/CoMZMPController.h ${H_PREFIX}/Module.h | ||
LINK_LIBRARIES BipedalLocomotion::SimplifiedModelControllers | ||
TESTS tests/test_com_zmp_controller.py) | ||
|
||
endif() |
26 changes: 26 additions & 0 deletions
26
...trollers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
/** | ||
* @file CoMZMPController.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_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H | ||
#define BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H | ||
|
||
#include <pybind11/pybind11.h> | ||
|
||
namespace BipedalLocomotion | ||
{ | ||
namespace bindings | ||
{ | ||
namespace SimplifiedModelControllers | ||
{ | ||
|
||
void CreateCoMZMPController(pybind11::module& module); | ||
|
||
} // namespace SimplifiedModelControllers | ||
} // namespace bindings | ||
} // namespace BipedalLocomotion | ||
|
||
#endif // BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_COM_ZMP_CONTROLLER_H |
26 changes: 26 additions & 0 deletions
26
...edModelControllers/include/BipedalLocomotion/bindings/SimplifiedModelControllers/Module.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
/** | ||
* @file Module.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_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H | ||
#define BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H | ||
|
||
#include <pybind11/pybind11.h> | ||
|
||
namespace BipedalLocomotion | ||
{ | ||
namespace bindings | ||
{ | ||
namespace SimplifiedModelControllers | ||
{ | ||
|
||
void CreateModule(pybind11::module& module); | ||
|
||
} // namespace Contacts | ||
} // namespace bindings | ||
} // namespace SimplifiedModelControllers | ||
|
||
#endif // BIPEDAL_LOCOMOTION_BINDINGS_SIMPLIFIED_MODEL_CONTROLLERS_MODULE_H |
65 changes: 65 additions & 0 deletions
65
bindings/python/SimplifiedModelControllers/src/CoMZMPController.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,65 @@ | ||
/** | ||
* @file CoMZMPController.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 <BipedalLocomotion/SimplifiedModelControllers/CoMZMPController.h> | ||
#include <BipedalLocomotion/bindings/System/Advanceable.h> | ||
|
||
#include <pybind11/pybind11.h> | ||
#include <pybind11/stl.h> | ||
|
||
namespace BipedalLocomotion | ||
{ | ||
namespace bindings | ||
{ | ||
namespace SimplifiedModelControllers | ||
{ | ||
|
||
void CreateCoMZMPController(pybind11::module& module) | ||
{ | ||
namespace py = ::pybind11; | ||
using namespace BipedalLocomotion::SimplifiedModelControllers; | ||
using namespace BipedalLocomotion::System; | ||
|
||
py::class_<CoMZMPControllerInput>(module, "CoMZMPControllerInput") | ||
.def(py::init()) | ||
.def_readwrite("desired_CoM_position", &CoMZMPControllerInput::desiredCoMPosition) | ||
.def_readwrite("desired_CoM_velocity", &CoMZMPControllerInput::desiredCoMVelocity) | ||
.def_readwrite("desired_ZMP_position", &CoMZMPControllerInput::desiredZMPPosition) | ||
.def_readwrite("CoM_position", &CoMZMPControllerInput::CoMPosition) | ||
.def_readwrite("ZMP_position", &CoMZMPControllerInput::ZMPPosition) | ||
.def_readwrite("angle", &CoMZMPControllerInput::angle); | ||
|
||
BipedalLocomotion::bindings::System::CreateAdvanceable<CoMZMPControllerInput, | ||
Eigen::Vector2d>(module, | ||
"CoMZMPController"); | ||
py::class_<CoMZMPController, // | ||
Advanceable<CoMZMPControllerInput, Eigen::Vector2d>>(module, "CoMZMPController") | ||
.def(py::init()) | ||
.def("set_set_point", | ||
&CoMZMPController::setSetPoint, | ||
py::arg("CoM_velocity"), | ||
py::arg("CoM_position"), | ||
py::arg("ZMP_position")) | ||
.def("set_feedback", | ||
py::overload_cast<Eigen::Ref<const Eigen::Vector2d>, | ||
Eigen::Ref<const Eigen::Vector2d>, | ||
const manif::SO2d&>(&CoMZMPController::setFeedback), | ||
py::arg("CoM_position"), | ||
py::arg("ZMP_position"), | ||
py::arg("I_R_B")) | ||
.def("set_feedback", | ||
py::overload_cast<Eigen::Ref<const Eigen::Vector2d>, | ||
Eigen::Ref<const Eigen::Vector2d>, | ||
const double>(&CoMZMPController::setFeedback), | ||
py::arg("CoM_position"), | ||
py::arg("ZMP_position"), | ||
py::arg("angle")); | ||
} | ||
|
||
} // namespace SimplifiedModelControllers | ||
} // namespace bindings | ||
} // namespace BipedalLocomotion |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
/** | ||
* @file Module.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 <pybind11/pybind11.h> | ||
|
||
#include <BipedalLocomotion/bindings/SimplifiedModelControllers/CoMZMPController.h> | ||
|
||
namespace BipedalLocomotion | ||
{ | ||
namespace bindings | ||
{ | ||
namespace SimplifiedModelControllers | ||
{ | ||
void CreateModule(pybind11::module& module) | ||
{ | ||
module.doc() = "Simplified Model Controller module."; | ||
|
||
CreateCoMZMPController(module); | ||
} | ||
} // namespace Contacts | ||
} // namespace bindings | ||
} // namespace SimplifiedModelControllers |
26 changes: 26 additions & 0 deletions
26
bindings/python/SimplifiedModelControllers/tests/test_com_zmp_controller.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
import pytest | ||
import bipedal_locomotion_framework.bindings as blf | ||
|
||
pytestmark = pytest.mark.CoMZMPController | ||
|
||
|
||
def test_com_zmp_controller(): | ||
controller = blf.simplified_model_controllers.CoMZMPController() | ||
|
||
# Defining the param handler | ||
controller_param_handler = blf.parameters_handler.StdParametersHandler() | ||
controller_param_handler.set_parameter_vector_float("zmp_gain", [3.0, 4.0]) | ||
controller_param_handler.set_parameter_vector_float("com_gain", [1.0, 2.0]) | ||
|
||
# define the input | ||
controller_input = blf.simplified_model_controllers.CoMZMPControllerInput() | ||
controller_input.desired_CoM_position = [0.01, 0] | ||
controller_input.desired_CoM_velocity = [0.0, 0] | ||
controller_input.desired_ZMP_position = [0.01, 0] | ||
controller_input.CoM_position = [0.01, 0] | ||
controller_input.ZMP_position = [0.01, 0] | ||
controller_input.angle = 0 | ||
assert controller.initialize(controller_param_handler) | ||
assert controller.set_input(controller_input) | ||
assert controller.advance() | ||
assert controller.is_output_valid() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.