Add new Advanceable exposing UnicyclePlanner

.github/workflows/ci.yml

@@ -22,6 +22,7 @@ env:
  osqp_TAG: v0.6.2
  OsqpEigen_TAG: v0.6.3
  tomlplusplus_TAG: 4f21332bdd7555bbf9add1aafe82909a2f8aa52b~
+ UnicyclePlanner_TAG: d3f6c80afe21a9958da769c8dd8a2bbfee5ea922
 
  # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg
  VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg
@@ -126,7 +127,7 @@ jobs:
  with:
  path: ${{ github.workspace }}/install/deps
  # Including ${{ runner.temp }} is a workaround taken from https://github.com/robotology/whole-body-estimators/pull/62 to fix macos configuration failure on https://github.com/dic-iit/bipedal-locomotion-framework/pull/45
- key: source-deps-${{ runner.os }}-${{runner.temp}}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}
+ key: source-deps-${{ runner.os }}-${{runner.temp}}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-ycm-${{ env.YCM_TAG }}-yarp-${{ env.YARP_TAG }}-iDynTree-${{ env.iDynTree_TAG }}-catch2-${{ env.Catch2_TAG }}-qhull-${{ env.Qhull_TAG }}-casADi-${{ env.CasADi_TAG }}-manif-${{ env.manif_TAG }}-matioCpp-${{ env.matioCpp_TAG }}-LieGroupControllers-${{ env.LieGroupControllers_TAG }}-osqp-${{ env.osqp_TAG }}-osqp-eigen-${{ env.OsqpEigen_TAG }}-tomlplusplus-${{ env.tomlplusplus_TAG }}-unicycle-${{UnicyclePlanner_TAG}}
 
 
  - name: Source-based Dependencies [Windows]
@@ -271,6 +272,19 @@ jobs:
  -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
  cmake --build . --config ${{ matrix.build_type }} --target install
 
+ # UnicycleFootstepPlanner
+ cd ${GITHUB_WORKSPACE}
+ git clone https://github.com/robotology/unicycle-footstep-planner
+ cd unicycle-footstep-planner
+ git checkout ${UnicyclePlanner_TAG}
+ mkdir build
+ cd build
+ cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \
+ -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \
+ -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+ -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+ cmake --build . --config ${{ matrix.build_type }} --target install
+
  - name: Source-based Dependencies [Ubuntu/macOS]
  if: steps.cache-source-deps.outputs.cache-hit != 'true' && (startsWith(matrix.os, 'ubuntu') || matrix.os == 'macos-latest')
  shell: bash
@@ -377,6 +391,16 @@ jobs:
  cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
  cmake --build . --config ${{ matrix.build_type }} --target install
 
+ # UnicycleFootstepPlanner
+ cd ${GITHUB_WORKSPACE}
+ git clone https://github.com/robotology/unicycle-footstep-planner
+ cd unicycle-footstep-planner
+ git checkout ${UnicyclePlanner_TAG}
+ mkdir build
+ cd build
+ cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
+ cmake --build . --config ${{ matrix.build_type }} --target install
+
  - name: Source-based Dependencies [Ubuntu]
  if: steps.cache-source-deps.outputs.cache-hit != 'true' && startsWith(matrix.os, 'ubuntu')
  shell: bash

.github/workflows/conda-forge-ci.yml

@@ -35,7 +35,7 @@ jobs:
  # Compilation related dependencies
  mamba install cmake compilers make ninja pkg-config
  # Actual dependencies
- mamba install idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog catch2 nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl tomlplusplus
+ mamba install idyntree yarp libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog catch2 nlohmann_json manif manifpy pybind11 numpy pytest scipy opencv pcl tomlplusplus unicycle-footstep-planner
 
  - name: Linux-only Dependencies [Linux]
  if: contains(matrix.os, 'ubuntu')

.gitignore

@@ -1,5 +1,5 @@
-# build folder
-build/*
+# build folders
+build*
 
 # emacs
 *~

CHANGELOG.md

@@ -7,6 +7,7 @@ All notable changes to this project are documented in this file.
 - Implement `ContactPhaseList::getPresentPhase()` method (https://github.com/dic-iit/bipedal-locomotion-framework/pull/396)
 - Add a synchronization mechanism for the `AdvanceableRunner` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/403)
 - Add the possibility to use spdlog with YARP (https://github.com/ami-iit/bipedal-locomotion-framework/pull/408)
+- Add new Advanceable exposing `UnicyclePlanner` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/320)
 
 ### Changed
 - Add `name` parameter to the `AdvanceableRunner` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/406)

bindings/python/Contacts/src/Contacts.cpp

@@ -9,11 +9,13 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
 
+#include <iomanip>
+
 #include <BipedalLocomotion/Contacts/Contact.h>
 #include <BipedalLocomotion/Contacts/ContactList.h>
+#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 #include <BipedalLocomotion/Contacts/ContactPhase.h>
 #include <BipedalLocomotion/Contacts/ContactPhaseList.h>
-#include <BipedalLocomotion/Contacts/ContactListJsonParser.h>
 
 #include <BipedalLocomotion/bindings/Contacts/Contacts.h>
 
@@ -26,8 +28,6 @@ namespace Contacts
 
 std::string toString(const BipedalLocomotion::Contacts::PlannedContact& contact)
 {
- std::string description;
-
  const Eigen::IOFormat FormatEigenVector //
  (Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");
 
@@ -38,12 +38,13 @@ std::string toString(const BipedalLocomotion::Contacts::PlannedContact& contact)
  pose << "SE3 (position = " << position.format(FormatEigenVector)
  << ", quaternion = " << quaternion.format(FormatEigenVector) << ")";
 
- description = "Contact (name = " + contact.name + ", pose = " + pose.str()
- + ", activation_time = " + std::to_string(contact.activationTime)
- + ", deactivation_time = " + std::to_string(contact.deactivationTime)
- + ", type = " + std::to_string(static_cast<int>(contact.type)) + ")";
+ std::stringstream description;
+ description << "Contact (name = " << contact.name << ", pose = " << pose.str()
+ << std::setprecision(7) << ", activation_time = " << contact.activationTime
+ << ", deactivation_time = " << contact.deactivationTime
+ << ", type = " << static_cast<int>(contact.type) << ")";
 
- return description;
+ return description.str();
 }
 
 void CreateContact(pybind11::module& module)
@@ -132,7 +133,18 @@ void CreateContactList(pybind11::module& module)
  return "ContactList(" + std::to_string(list.size()) + ")";
  })
  .def("__reverse__",
- [](const ContactList& l) { return py::make_iterator(l.crbegin(), l.crend()); });
+ [](const ContactList& l) { return py::make_iterator(l.crbegin(), l.crend()); })
+ .def(
+ "__eq__",
+ [](const ContactList& lhs, const ContactList& rhs) -> bool {
+ for (std::size_t i = 0; i < lhs.size(); ++i)
+ {
+ if (!(lhs[i] == rhs[i]))
+ return false;
+ }
+ return lhs.size() == rhs.size();
+ },
+ py::is_operator());
 }
 
 void CreateContactPhase(pybind11::module& module)
@@ -184,6 +196,6 @@ void CreateContactListJsonParser(pybind11::module& module)
  py::arg("filename"));
 }
 
-} // namespace contatcs
+} // namespace Contacts
 } // namespace bindings
 } // namespace BipedalLocomotion

bindings/python/Planners/CMakeLists.txt

@@ -2,16 +2,36 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-if(FRAMEWORK_COMPILE_Planners)
+if(FRAMEWORK_COMPILE_Planners AND FRAMEWORK_COMPILE_Unicycle)
 
  set(H_PREFIX include/BipedalLocomotion/bindings/Planners)
 
  add_bipedal_locomotion_python_module(
  NAME PlannersBindings
- SOURCES src/DCMPlanner.cpp src/TimeVaryingDCMPlanner.cpp src/QuinticSpline.cpp src/SwingFootPlanner.cpp src/Module.cpp
- HEADERS ${H_PREFIX}/DCMPlanner.h ${H_PREFIX}/TimeVaryingDCMPlanner.h ${H_PREFIX}/QuinticSpline.h ${H_PREFIX}/SwingFootPlanner.h ${H_PREFIX}/Module.h
- LINK_LIBRARIES BipedalLocomotion::Planners
- TESTS tests/test_quintic_spline.py tests/test_swing_foot_planner.py tests/test_time_verying_dcm_planner.py
+ SOURCES
+ src/DCMPlanner.cpp
+ src/TimeVaryingDCMPlanner.cpp
+ src/QuinticSpline.cpp
+ src/SwingFootPlanner.cpp
+ src/UnicyclePlanner.cpp
+ src/Module.cpp
+ HEADERS
+ ${H_PREFIX}/DCMPlanner.h
+ ${H_PREFIX}/TimeVaryingDCMPlanner.h
+ ${H_PREFIX}/QuinticSpline.h
+ ${H_PREFIX}/SwingFootPlanner.h
+ ${H_PREFIX}/UnicyclePlanner.h
+ ${H_PREFIX}/Module.h
+ LINK_LIBRARIES
+ BipedalLocomotion::System
+ BipedalLocomotion::Planners
+ BipedalLocomotion::Unicycle
+ BipedalLocomotion::Contacts
+ TESTS
+ tests/test_quintic_spline.py
+ tests/test_swing_foot_planner.py
+ tests/test_time_verying_dcm_planner.py
+ tests/test_unicycle_planner.py
  )
 
 endif()

bindings/python/Planners/include/BipedalLocomotion/bindings/Planners/UnicyclePlanner.h

@@ -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

bindings/python/Planners/src/Module.cpp

@@ -12,6 +12,7 @@
 #include <BipedalLocomotion/bindings/Planners/QuinticSpline.h>
 #include <BipedalLocomotion/bindings/Planners/SwingFootPlanner.h>
 #include <BipedalLocomotion/bindings/Planners/TimeVaryingDCMPlanner.h>
+#include <BipedalLocomotion/bindings/Planners/UnicyclePlanner.h>
 
 namespace BipedalLocomotion
 {
@@ -27,6 +28,7 @@ void CreateModule(pybind11::module& module)
  CreateDCMPlanner(module);
  CreateTimeVaryingDCMPlanner(module);
  CreateSwingFootPlanner(module);
+ CreateUnicyclePlanner(module);
 }
 } // namespace Planners
 } // namespace bindings

bindings/python/Planners/src/UnicyclePlanner.cpp

@@ -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