Create bindings for TimeVaryingDCMPlanner class

bindings/python/CMakeLists.txt

@@ -8,9 +8,11 @@ pybind11_add_module(pybind11_BipedalLocomotion MODULE
     bipedal_locomotion.cpp
     # Support files
     Contacts.cpp
+    DCMPlanner.cpp
     ParametersHandler.cpp
     QuinticSpline.cpp
     SwingFootPlaner.cpp
+    TimeVaryingDCMPlanner.cpp
     )
 
 target_link_libraries(pybind11_BipedalLocomotion PUBLIC

bindings/python/DCMPlanner.cpp

@@ -0,0 +1,38 @@
+/**
+ * @file DCMPlanner.cpp
+ * @authors DiegoFerigo
+ * @copyright 2020 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 <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include "BipedalLocomotion/Planners/DCMPlanner.h"
+#include "BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h"
+#include "BipedalLocomotion/System/Advanceable.h"
+#include "bipedal_locomotion.h"
+
+void BipedalLocomotion::bindings::CreateDCMPlanner(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::Planners;
+    using namespace BipedalLocomotion::System;
+
+    py::class_<DCMPlannerState>(module, "DCMPlannerState")
+        .def(py::init())
+        .def_readwrite("dcm_position", &DCMPlannerState::dcmPosition)
+        .def_readwrite("dcm_velocity", &DCMPlannerState::dcmVelocity)
+        .def_readwrite("vrp_position", &DCMPlannerState::vrpPosition)
+        .def_readwrite("omega", &DCMPlannerState::omega)
+        .def_readwrite("omega_dot", &DCMPlannerState::omegaDot);
+
+    py::class_<Advanceable<DCMPlannerState>>(module, "DCMPlannerStateAdvanceable");
+
+    py::class_<DCMPlanner, Advanceable<DCMPlannerState>>(module, "DCMPlanner")
+        .def("set_initial_state", &DCMPlanner::setInitialState, py::arg("state"))
+        .def("set_contact_phase_list",
+             &DCMPlanner::setContactPhaseList,
+             py::arg("contact_phase_list"));
+}

bindings/python/TimeVaryingDCMPlanner.cpp

@@ -0,0 +1,29 @@
+/**
+ * @file TimeVaryingDCMPlanner.cpp
+ * @authors DiegoFerigo
+ * @copyright 2020 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 <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include "BipedalLocomotion/Planners/DCMPlanner.h"
+#include "BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h"
+#include "BipedalLocomotion/System/Advanceable.h"
+#include "bipedal_locomotion.h"
+
+void BipedalLocomotion::bindings::CreateTimeVaryingDCMPlanner(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::Planners;
+
+    py::class_<TimeVaryingDCMPlanner, DCMPlanner>(module, "TimeVaryingDCMPlanner")
+        .def(py::init())
+        .def("initialize", &TimeVaryingDCMPlanner::initialize, py::arg("handler"))
+        .def("compute_trajectory", &TimeVaryingDCMPlanner::computeTrajectory)
+        .def("get", &TimeVaryingDCMPlanner::get)
+        .def("is_valid", &TimeVaryingDCMPlanner::isValid)
+        .def("advance", &TimeVaryingDCMPlanner::advance);
+}

bindings/python/bipedal_locomotion.cpp

@@ -29,5 +29,11 @@ PYBIND11_MODULE(bindings, m)
 
     // SwingFootPlanner.cpp
     bindings::CreateSwingFootPlanner(m);
+
+    // DCMPlanner.cpp
+    bindings::CreateDCMPlanner(m);
+
+    // TimeVaryingDCMPlanner.cpp
+    bindings::CreateTimeVaryingDCMPlanner(m);
 }
 } // namespace BipedalLocomotion

bindings/python/bipedal_locomotion.h

@@ -24,4 +24,10 @@ void CreateContactPhaseList(pybind11::module& module);
 
 // SwingFootPlanner.cpp
 void CreateSwingFootPlanner(pybind11::module& module);
+
+// DCMPlanner.cpp
+void CreateDCMPlanner(pybind11::module& module);
+
+// TimeVaryingDCMPlanner.cpp
+void CreateTimeVaryingDCMPlanner(pybind11::module& module);
 } // namespace BipedalLocomotion::bindings

bindings/python/tests/test_time_verying_dcm_planner.py

@@ -0,0 +1,103 @@
+import pytest
+pytestmark = pytest.mark.planners
+
+import bipedal_locomotion.bindings as bl
+import numpy as np
+
+
+def test_dcm_planner_state():
+
+    state = bl.DCMPlannerState()
+
+    state.dcm_position = [1., -2, 3.14]
+    assert state.dcm_position == pytest.approx([1., -2, 3.14])
+
+    state.dcm_velocity = [0.1, -0.2, 0.314]
+    assert state.dcm_velocity == pytest.approx([0.1, -0.2, 0.314])
+
+    state.vrp_position = [-0.5, 0.0, 42.42]
+    assert state.vrp_position == pytest.approx([-0.5, 0.0, 42.42])
+
+    state.omega = 42.0
+    assert state.omega == pytest.approx(42.0)
+
+    state.omega_dot = 2.7182
+    assert state.omega_dot == pytest.approx(2.7182)
+
+
+def test_time_varying_dcm_planner():
+
+    # Create the map of contact lists
+    contact_list_map = dict()
+
+    # Left foot
+    contact_list_map["left"] = bl.ContactList()
+
+    # L1: first footstep
+    assert contact_list_map["left"].add_contact(
+        new_transform=np.array([0, -0.8, 0] + [0, 0, 0, 1]),
+        activation_time=0.0,
+        deactivation_time=1.0)
+
+    # L2: second footstep
+    assert contact_list_map["left"].add_contact(
+        new_transform=np.array([0.25, -0.8, 0.2] + [0, 0, 0, 1]),
+        activation_time=2.0,
+        deactivation_time=7.0)
+
+    # Right foot
+    contact_list_map["right"] = bl.ContactList()
+
+    # R1: first footstep
+    assert contact_list_map["right"].add_contact(
+        new_transform=np.array([0, 0.8, 0] + [0, 0, 0, 1]),
+        activation_time=0.0,
+        deactivation_time=3.0)
+
+    # R2: second footstep
+    assert contact_list_map["right"].add_contact(
+        new_transform=np.array([0.25, 0.8, 0.2] + [0, 0, 0, 1]),
+        activation_time=4.0,
+        deactivation_time=7.0)
+
+    # Create the contact phase list
+    phase_list = bl.ContactPhaseList()
+    phase_list.set_lists(contact_list_map)
+
+    # Set the parameters
+    handler = bl.StdParametersHandler()
+    handler.set_parameter_float(name="planner_sampling_time", value=0.05)
+    handler.set_parameter_int(name="number_of_foot_corners", value=4)
+
+    # Set the foot corners
+    handler.set_parameter_vector_float(name="foot_corner_0", value=[0.1, 0.05, 0.0])
+    handler.set_parameter_vector_float(name="foot_corner_1", value=[0.1, -0.05, 0.0])
+    handler.set_parameter_vector_float(name="foot_corner_2", value=[-0.1, -0.05, 0.0])
+    handler.set_parameter_vector_float(name="foot_corner_3", value=[-0.1, 0.05, 0.0])
+
+    # Set the weight of the cost function
+    handler.set_parameter_float("omega_dot_weight", 1.0)
+    handler.set_parameter_float("dcm_tracking_weight", 1.0)
+    handler.set_parameter_float("omega_dot_rate_of_change_weight", 10.0)
+    handler.set_parameter_float("vrp_rate_of_change_weight", 100.0)
+    handler.set_parameter_float("dcm_rate_of_change_weight", 1.0)
+
+    # Set the initial state
+    initial_state = bl.DCMPlannerState()
+    initial_state.dcm_position = np.array([0, 0, 0.53])
+    initial_state.dcm_velocity = np.zeros(3)
+    initial_state.vrp_position = initial_state.dcm_position
+    initial_state.omega = np.sqrt(9.81 / initial_state.dcm_position[2])
+
+    # Initialize the planner
+    planner = bl.TimeVaryingDCMPlanner()
+    assert planner.initialize(handler=handler)
+
+    assert planner.set_contact_phase_list(contact_phase_list=phase_list)
+    planner.set_initial_state(state=initial_state)
+
+    assert planner.compute_trajectory()
+
+    for _ in range(150):
+
+        assert planner.advance()