-
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 #320 from diegoferigo/feature/unicycle-planner
Add new Advanceable exposing `UnicyclePlanner`
- Loading branch information
Showing
15 changed files
with
1,297 additions
and
25 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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,5 +1,5 @@ | ||
| # build folder | ||
| build/* | ||
| # build folders | ||
| build* | ||
|
|
||
| # emacs | ||
| *~ | ||
|
|
||
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
18 changes: 18 additions & 0 deletions
18
bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.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,18 @@ | ||
| /** | ||
| * @file UnicyclePlanner.h | ||
| * @authors Diego Ferigo | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. | ||
| */ | ||
|
|
||
| #ifndef BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H | ||
| #define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_H | ||
|
|
||
| #include <pybind11/pybind11.h> | ||
|
|
||
| namespace BipedalLocomotion::bindings::Planners | ||
| { | ||
| void CreateUnicyclePlanner(pybind11::module& module); | ||
| } // namespace BipedalLocomotion::bindings::Planners | ||
|
|
||
| #endif // BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_UNICYCLE_PLANNER_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,114 @@ | ||
| /** | ||
| * @file UnicyclePlanner.cpp | ||
| * @authors Diego Ferigo | ||
| * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and | ||
| * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. | ||
| */ | ||
|
|
||
| #include "BipedalLocomotion/Planners/UnicyclePlanner.h" | ||
| #include "BipedalLocomotion/Contacts/ContactList.h" | ||
| #include "BipedalLocomotion/ParametersHandler/IParametersHandler.h" | ||
| #include "BipedalLocomotion/System/Advanceable.h" | ||
|
|
||
| #include <pybind11/eigen.h> | ||
| #include <pybind11/pybind11.h> | ||
| #include <pybind11/stl.h> | ||
|
|
||
| namespace BipedalLocomotion::bindings::Planners | ||
| { | ||
|
|
||
| void CreateUnicyclePlanner(pybind11::module& module) | ||
| { | ||
| namespace py = ::pybind11; | ||
| using namespace BipedalLocomotion::Planners; | ||
| using namespace BipedalLocomotion::System; | ||
| using namespace BipedalLocomotion::ParametersHandler; | ||
|
|
||
| py::class_<UnicycleKnot>(module, "UnicycleKnot") | ||
| .def(py::init<const Eigen::Vector2d&, const Eigen::Vector2d&, double>(), | ||
| py::arg("position") = std::make_tuple(0.0, 0.0), | ||
| py::arg("velocity") = std::make_tuple(0.0, 0.0), | ||
| py::arg("time") = 0.0) | ||
| .def_readwrite("time", &UnicycleKnot::time) | ||
| .def_readwrite("x", &UnicycleKnot::x) | ||
| .def_readwrite("y", &UnicycleKnot::y) | ||
| .def_readwrite("dx", &UnicycleKnot::dx) | ||
| .def_readwrite("dy", &UnicycleKnot::dy) | ||
| .def("__eq__", &UnicycleKnot::operator==, py::is_operator()) | ||
| .def("__repr__", [](const UnicycleKnot& k) { | ||
| return std::string("UnicycleKnot(") + // | ||
| "x=" + std::to_string(k.x) + ", " + // | ||
| "y=" + std::to_string(k.y) + ", " + // | ||
| "dx=" + std::to_string(k.dx) + ", " + // | ||
| "dy=" + std::to_string(k.dy) + ", " + // | ||
| "time=" + std::to_string(k.time) + // | ||
| ")"; | ||
| }); | ||
|
|
||
| py::class_<UnicyclePlannerInput>(module, "UnicyclePlannerInput") | ||
| .def(py::init<const std::vector<UnicycleKnot>&, | ||
| const double, | ||
| const std::optional<Contacts::PlannedContact>&, | ||
| const std::optional<Contacts::PlannedContact>&, | ||
| const double>(), | ||
| py::arg("knots") = std::vector<UnicycleKnot>{}, | ||
| py::arg("tf") = 0.0, | ||
| py::arg("initial_left_contact") = std::nullopt, | ||
| py::arg("initial_right_contact") = std::nullopt, | ||
| py::arg("t0") = 0.0) | ||
| .def_readwrite("t0", &UnicyclePlannerInput::t0) | ||
| .def_readwrite("tf", &UnicyclePlannerInput::tf) | ||
| .def_readwrite("initial_left_contact", &UnicyclePlannerInput::initialLeftContact) | ||
| .def_readwrite("initial_right_contact", &UnicyclePlannerInput::initialRightContact) | ||
| .def_readwrite("knots", &UnicyclePlannerInput::knots) | ||
| .def("__repr__", [](const UnicyclePlannerInput& i) { | ||
| auto printContact | ||
| = [](const std::optional<Contacts::PlannedContact>& c) -> std::string { | ||
| return c ? "PlannedContact(" + c->name + ")" : "None"; | ||
| }; | ||
| return std::string("UnicyclePlannerInput(") + // | ||
| "t0=" + std::to_string(i.t0) + ", " + // | ||
| "tf=" + std::to_string(i.tf) + ", " + // | ||
| "knots=KnotList(" + std::to_string(i.knots.size()) + "), " + // | ||
| "initial_left_contact=" + printContact(i.initialLeftContact) + ", " + // | ||
| "initial_right_contact=" + printContact(i.initialRightContact) + // | ||
| ")"; | ||
| }); | ||
|
|
||
| py::class_<UnicyclePlannerOutput>(module, "UnicyclePlannerOutput") | ||
| .def(py::init<const Contacts::ContactList&, const Contacts::ContactList&>(), | ||
| py::arg("left") = Contacts::ContactList(), | ||
| py::arg("right") = Contacts::ContactList()) | ||
| .def_readwrite("left", &UnicyclePlannerOutput::left) | ||
| .def_readwrite("right", &UnicyclePlannerOutput::right) | ||
| .def("__repr__", [](const UnicyclePlannerOutput& o) { | ||
| auto printContactList = [](const Contacts::ContactList& l) -> std::string { | ||
| return "ContactList(" + std::to_string(l.size()) + ")"; | ||
| }; | ||
| return std::string("UnicyclePlannerOutput(") + // | ||
| "left=" + printContactList(o.left) + ", " + // | ||
| "right=" + printContactList(o.right) + // | ||
| ")"; | ||
| }); | ||
|
|
||
| py::class_<Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>>( // | ||
| module, | ||
| "UnicyclePlannerAdvanceable"); | ||
|
|
||
| py::class_<UnicyclePlanner, Advanceable<UnicyclePlannerInput, UnicyclePlannerOutput>>( // | ||
| module, | ||
| "UnicyclePlanner") | ||
| .def(py::init()) | ||
| .def( | ||
| "initialize", | ||
| [](UnicyclePlanner& impl, std::shared_ptr<const IParametersHandler> handler) -> bool { | ||
| return impl.initialize(handler); | ||
| }, | ||
| py::arg("handler")) | ||
| .def("get_output", &UnicyclePlanner::getOutput) | ||
| .def("is_output_valid", &UnicyclePlanner::isOutputValid) | ||
| .def("set_input", &UnicyclePlanner::setInput, py::arg("input")) | ||
| .def("advance", &UnicyclePlanner::advance); | ||
| } | ||
|
|
||
| } // namespace BipedalLocomotion::bindings::Planners |
Oops, something went wrong.