Refactory of the swing foot planner

CHANGELOG.md

@@ -7,6 +7,8 @@ All notable changes to this project are documented in this file.
 
 ### Changed
 - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766)
+- Restructured the swing foot planner to handle corners case that came out while testing DNN-MPC integration (https://github.com/ami-iit/bipedal-locomotion-framework/pull/765)
+- Avoid returning internal references for `get_next_contact` `get_present_contact` and `get_active_contact` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/765)
 
 ### Fixed
 

bindings/python/Contacts/src/Contacts.cpp

@@ -140,18 +140,21 @@ void CreateContactList(pybind11::module& module)
         .def("edit_contact", &ContactList::editContact, py::arg("element"), py::arg("new_contact"))
         .def(
             "get_present_contact",
-            [](const ContactList& l, const std::chrono::nanoseconds& time) -> PlannedContact {
+            [](const ContactList& l,
+               const std::chrono::nanoseconds& time) -> const PlannedContact& {
                 auto it = l.getPresentContact(time);
                 if (it == l.end())
                 {
                     throw py::value_error("Unable to find a contact at the specified time.");
                 }
                 return *it;
             },
-            py::arg("time"))
+            py::arg("time"),
+            py::return_value_policy::reference_internal)
         .def(
             "get_active_contact",
-            [](const ContactList& l, const std::chrono::nanoseconds& time) -> PlannedContact {
+            [](const ContactList& l,
+               const std::chrono::nanoseconds& time) -> const PlannedContact& {
                 auto it = l.getActiveContact(time);
                 if (it == l.end())
                 {
@@ -160,10 +163,12 @@ void CreateContactList(pybind11::module& module)
                 }
                 return *it;
             },
-            py::arg("time"))
+            py::arg("time"),
+            py::return_value_policy::reference_internal)
         .def(
             "get_next_contact",
-            [](const ContactList& l, const std::chrono::nanoseconds& time) -> PlannedContact {
+            [](const ContactList& l,
+               const std::chrono::nanoseconds& time) -> const PlannedContact& {
                 auto it = l.getNextContact(time);
                 if (it == l.end())
                 {
@@ -172,7 +177,8 @@ void CreateContactList(pybind11::module& module)
                 }
                 return *it;
             },
-            py::arg("time"))
+            py::arg("time"),
+            py::return_value_policy::reference_internal)
         .def("keep_only_present_contact", &ContactList::keepOnlyPresentContact, py::arg("time"))
         .def("clear", &ContactList::clear)
         .def("remove_last_contact", &ContactList::removeLastContact)
@@ -192,7 +198,18 @@ void CreateContactList(pybind11::module& module)
                 }
                 return lhs.size() == rhs.size();
             },
-            py::is_operator());
+            py::is_operator())
+        .def(
+            "last_contact",
+            [](const ContactList& l) -> const PlannedContact& {
+                auto ptr = l.lastContact();
+                if (ptr == l.cend())
+                {
+                    throw py::value_error("Unable to find the last contact.");
+                }
+                return *ptr;
+            },
+            py::return_value_policy::reference_internal);
 }
 
 void CreateContactPhase(pybind11::module& module)

bindings/python/Planners/src/SwingFootPlanner.cpp

@@ -52,7 +52,8 @@ void CreateSwingFootPlanner(pybind11::module& module)
             [](SwingFootPlannerState& s,
                decltype(SwingFootPlannerState::mixedAcceleration)::DataType& coeffs) {
                 s.mixedAcceleration.coeffs() = coeffs;
-            });
+            })
+        .def_readwrite("time", &SwingFootPlannerState::time);
 
     BipedalLocomotion::bindings::System::CreateSource<SwingFootPlannerState> //
         (module, "SwingFootPlanner");

bindings/python/Planners/tests/test_swing_foot_planner.py

@@ -82,8 +82,7 @@ def test_swing_foot_planner():
     assert planner.initialize(handler=parameters_handler)
     planner.set_contact_list(contact_list=contact_list)
 
-    num_of_iterations = (contact_list[len(contact_list) - 1].deactivation_time.total_seconds() + 1) / dT.total_seconds()
-
+    num_of_iterations = (contact_list.last_contact().deactivation_time.total_seconds() / dT.total_seconds()) - 1
     for i in range(int(num_of_iterations)):
 
         # state = planner.get()

src/Planners/include/BipedalLocomotion/Planners/SwingFootPlanner.h

@@ -33,6 +33,7 @@ struct SwingFootPlannerState
     manif::SE3d::Tangent mixedVelocity; /**< 6D-velocity written in mixed representation */
     manif::SE3d::Tangent mixedAcceleration; /**< 6D-acceleration written in mixed representation */
     bool isInContact{true}; /** < If true the link is in contact with the environment */
+    std::chrono::nanoseconds time; /**< Time associated to the planned trajectory */
 };
 
 /**
@@ -54,8 +55,9 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
     std::chrono::nanoseconds m_staringTimeOfCurrentSO3Traj{std::chrono::nanoseconds::zero()};
 
     Contacts::ContactList m_contactList; /**< List of the contacts */
-    Contacts::ContactList::const_iterator m_currentContactPtr; /**< Pointer to the current contact.
-                                                        (internal use) */
+    Contacts::PlannedContact m_lastValidContact; /**< This contains the current contact if
+                                                      the contact is active otherwise the last contact
+                                                      before the current swing phase. */
 
     SO3PlannerInertial m_SO3Planner; /**< Trajectory planner in SO(3) */
     std::unique_ptr<Spline> m_planarPlanner; /**< Trajectory planner for the x y coordinates of the
@@ -77,16 +79,31 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
     bool m_isOutputValid{false}; /**< True if getOutput returns meaningful data */
 
     /**
-     * Update the SE3 Trajectory.
+     * Evaluate the SE3 trajectory of the swing foot.
+     * @param state will contain the pose, velocity and acceleration (expressed in mixed
+     * representation) of the swing foot computed at the current time instant.
      * @return True in case of success/false otherwise.
+     * @note This method assumes that the trajectory has been already created with the method
+     * SwingFootPlanner::createSE3Traj.
      */
-    bool updateSE3Traj();
+    bool evaluateSE3Traj(SwingFootPlannerState& state);
 
     /**
-     * Create a new SE3Trajectory considering the previous and next contact
+     * Create a new SE3Trajectory considering the previous and next contact.
+     * @param initialPose initial pose of the foot.
+     * @param initialPlanarVelocity initial planar velocity of the foot.
+     * @param initialPlanarAcceleration initial planar acceleration of the foot.
+     * @param initialVerticalVelocity initial vertical velocity of the foot.
+     * @param initialVerticalAcceleration initial vertical acceleration of the foot.
+     * @param initialAngularVelocity initial angular velocity of the foot.
+     * @param initialAngularAcceleration initial angular acceleration of the foot.
      * @return True in case of success/false otherwise.
+     * @note This method assumes that the final planar and angular velocity and acceleration are
+     * equal to zero, while the final vertical velocity and acceleration are equal to the one
+     * provided by the user.
      */
-    bool createSE3Traj(Eigen::Ref<const Eigen::Vector2d> initialPlanarVelocity,
+    bool createSE3Traj(const manif::SE3d& initialPose,
+                       Eigen::Ref<const Eigen::Vector2d> initialPlanarVelocity,
                        Eigen::Ref<const Eigen::Vector2d> initialPlanarAcceleration,
                        Eigen::Ref<const Eigen::Matrix<double, 1, 1>> initialVerticalVelocity,
                        Eigen::Ref<const Eigen::Matrix<double, 1, 1>> initialVerticalAcceleration,