Merge pull request #609 from ami-iit/bindings/spline

Finalize the python bindings for the Spline
ami-iit · Feb 23, 2023 · c4dbd05 · c4dbd05
2 parents fb1d5b7 + a4d86ee
commit c4dbd05
Show file tree

Hide file tree

Showing 7 changed files with 97 additions and 61 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,8 @@ All notable changes to this project are documented in this file.
 - Implement the `QPFixedBaseInverseKinematics` in the `IK` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/599)
 - 🤖 [ergoCubSN000] Add configuration files for the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/600)
 - Add functions to split a model in a set of submodels in the Estimator component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/604)
+- Add the possibity to call the advanceable capabilities of the `QuinticSpline` from the python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
+- Implement the `CubicSpline` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609)
 
 ### Changed
 - Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581)

diff --git a/bindings/python/IK/src/Module.cpp b/bindings/python/IK/src/Module.cpp
@@ -14,6 +14,7 @@
 #include <BipedalLocomotion/bindings/IK/JointTrackingTask.h>
 #include <BipedalLocomotion/bindings/IK/Module.h>
 #include <BipedalLocomotion/bindings/IK/QPInverseKinematics.h>
+#include <BipedalLocomotion/bindings/IK/R3Task.h>
 #include <BipedalLocomotion/bindings/IK/SE3Task.h>
 #include <BipedalLocomotion/bindings/IK/SO3Task.h>
 
@@ -31,6 +32,7 @@ void CreateModule(pybind11::module& module)
  CreateCoMTask(module);
  CreateSE3Task(module);
  CreateSO3Task(module);
+ CreateR3Task(module);
  CreateJointTrackingTask(module);
  CreateAngularMomentumTask(module);
  CreateIntegrationBasedIK(module);

diff --git a/bindings/python/Planners/CMakeLists.txt b/bindings/python/Planners/CMakeLists.txt
@@ -11,14 +11,14 @@ if(FRAMEWORK_COMPILE_Planners AND FRAMEWORK_COMPILE_Unicycle)
  SOURCES
  src/DCMPlanner.cpp
  src/TimeVaryingDCMPlanner.cpp
- src/QuinticSpline.cpp
+ src/Spline.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}/Spline.h
  ${H_PREFIX}/SwingFootPlanner.h
  ${H_PREFIX}/UnicyclePlanner.h
  ${H_PREFIX}/Module.h

diff --git a/...omotion/bindings/Planners/QuinticSpline.h → ...edalLocomotion/bindings/Planners/Spline.h b/...omotion/bindings/Planners/QuinticSpline.h → ...edalLocomotion/bindings/Planners/Spline.h
@@ -1,12 +1,12 @@
 /**
- * @file QuinticSpline.h
+ * @file Spline.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_PLANNERS_QUINTIC_SPLINE_H
-#define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_QUINTIC_SPLINE_H
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_SPLINE_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_PLANNERS_SPLINE_H
 
 #include <pybind11/pybind11.h>
 
@@ -17,6 +17,8 @@ namespace bindings
 namespace Planners
 {
 
+void CreateSpline(pybind11::module& module);
+void CreateCubicSpline(pybind11::module& module);
 void CreateQuinticSpline(pybind11::module& module);
 
 } // namespace Planners

diff --git a/bindings/python/Planners/src/Module.cpp b/bindings/python/Planners/src/Module.cpp
@@ -9,7 +9,7 @@
 
 #include <BipedalLocomotion/bindings/Planners/DCMPlanner.h>
 #include <BipedalLocomotion/bindings/Planners/Module.h>
-#include <BipedalLocomotion/bindings/Planners/QuinticSpline.h>
+#include <BipedalLocomotion/bindings/Planners/Spline.h>
 #include <BipedalLocomotion/bindings/Planners/SwingFootPlanner.h>
 #include <BipedalLocomotion/bindings/Planners/TimeVaryingDCMPlanner.h>
 #include <BipedalLocomotion/bindings/Planners/UnicyclePlanner.h>
@@ -24,6 +24,8 @@ void CreateModule(pybind11::module& module)
 {
  module.doc() = "Planners module.";
 
+ CreateSpline(module);
+ CreateCubicSpline(module);
  CreateQuinticSpline(module);
  CreateDCMPlanner(module);
  CreateTimeVaryingDCMPlanner(module);

diff --git a/bindings/python/Planners/src/QuinticSpline.cpp b/bindings/python/Planners/src/QuinticSpline.cpp
diff --git a/bindings/python/Planners/src/Spline.cpp b/bindings/python/Planners/src/Spline.cpp
@@ -0,0 +1,83 @@
+/**
+ * @file Spline.cpp
+ * @authors Giulio Romualdi, Diego Ferigo
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <Eigen/Dense>
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/Planners/QuinticSpline.h>
+#include <BipedalLocomotion/Planners/CubicSpline.h>
+#include <BipedalLocomotion/Planners/Spline.h>
+
+#include <BipedalLocomotion/bindings/Planners/Spline.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace Planners
+{
+
+void CreateSpline(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::Planners;
+
+ py::class_<SplineState>(module, "SplineState")
+ .def(py::init())
+ .def_readwrite("position", &SplineState::position)
+ .def_readwrite("velocity", &SplineState::velocity)
+ .def_readwrite("acceleration", &SplineState::acceleration);
+
+ py::class_<Spline>(module, "Spline")
+ .def("set_advance_time_step", &Spline::setAdvanceTimeStep, py::arg("dt"))
+ .def("set_knots", &Spline::setKnots)
+ .def("set_initial_conditions",
+ &Spline::setInitialConditions,
+ py::arg("velocity"),
+ py::arg("acceleration"))
+ .def("set_final_conditions",
+ &Spline::setFinalConditions,
+ py::arg("velocity"),
+ py::arg("acceleration"))
+ .def("evaluate_point",
+ py::overload_cast<const double&,
+ Eigen::Ref<Eigen::VectorXd>,
+ Eigen::Ref<Eigen::VectorXd>,
+ Eigen::Ref<Eigen::VectorXd>>(&Spline::evaluatePoint),
+ py::arg("time"),
+ py::arg("position"),
+ py::arg("velocity"),
+ py::arg("acceleration"))
+ .def("get_output", &Spline::getOutput)
+ .def("is_output_valid", &Spline::isOutputValid)
+ .def("advance", &Spline::advance);
+}
+
+void CreateCubicSpline(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::Planners;
+
+ py::class_<CubicSpline, Spline>(module, "CubicSpline")
+ .def(py::init());
+}
+
+void CreateQuinticSpline(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::Planners;
+
+ py::class_<QuinticSpline, Spline>(module, "QuinticSpline")
+ .def(py::init());
+}
+
+} // namespace Planners
+} // namespace bindings
+} // namespace BipedalLocomotion