Add the possibility to update the contact list in the swing foot trajectory planner

CHANGELOG.md

@@ -7,6 +7,7 @@ All notable changes to this project are documented in this file.
 - Implement the `SchmittTrigger` in component `Math` and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624)
 - Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630)
 - Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630)
+- Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637)
 
 ### Changed
 - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621)
@@ -21,6 +22,7 @@ All notable changes to this project are documented in this file.
 - Update the `blf-position-tracking` to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630)
 - Update the python bindings to consider the time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630)
 - Robustify SubModelCreator and SubModelKinDynWrapper tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/631)
+- `SwingFootTrajectoryPlanner::advance()` must be called before getting the output (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637)
 
 ### Fixed
 - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622)

bindings/python/Contacts/src/Contacts.cpp

@@ -72,7 +72,8 @@ void CreateContact(pybind11::module& module)
  .def_readwrite("activation_time", &PlannedContact::activationTime)
  .def_readwrite("deactivation_time", &PlannedContact::deactivationTime)
  .def("__repr__", &toString)
- .def("__eq__", &PlannedContact::operator==, py::is_operator());
+ .def("__eq__", &PlannedContact::operator==, py::is_operator())
+ .def("is_contact_active", &PlannedContact::isContactActive);
 
  py::class_<EstimatedContact, ContactBase>(module, "EstimatedContact")
  .def(py::init())

bindings/python/Planners/src/SwingFootPlanner.cpp

@@ -59,7 +59,8 @@ void CreateSwingFootPlanner(pybind11::module& module)
 
  py::class_<SwingFootPlanner, Source<SwingFootPlannerState>>(module, "SwingFootPlanner")
  .def(py::init())
- .def("set_contact_list", &SwingFootPlanner::setContactList, py::arg("contact_list"));
+ .def("set_contact_list", &SwingFootPlanner::setContactList, py::arg("contact_list"))
+ .def("set_time", &SwingFootPlanner::setTime, py::arg("time"));
 }
 
 } // namespace Planners

src/Contacts/include/BipedalLocomotion/Contacts/Contact.h

@@ -88,6 +88,14 @@ struct PlannedContact : ContactBase
  * @return True if the contacts are the same, false otherwise.
  */
  bool operator==(const PlannedContact& other) const;
+
+ /**
+ * @brief Check if the contact is active at a give time instant
+ *
+ * @param t time instant at which we check if the contact is active.
+ * @return True if `activationTime <= t < deactivationTime`.
+ */
+ [[nodiscard]] bool isContactActive(const std::chrono::nanoseconds& t) const;
 };
 
 /**

src/Contacts/src/Contact.cpp

@@ -5,14 +5,14 @@
  * distributed under the terms of the BSD-3-Clause license.
  */
 
-#include <BipedalLocomotion/Contacts/Contact.h>
 #include <chrono>
 
+#include <BipedalLocomotion/Contacts/Contact.h>
+
 using namespace BipedalLocomotion::Contacts;
 
 bool PlannedContact::operator==(const PlannedContact& other) const
 {
-
  bool eq = true;
  eq = eq && this->activationTime == other.activationTime;
  eq = eq && this->name == other.name;
@@ -23,6 +23,11 @@ bool PlannedContact::operator==(const PlannedContact& other) const
  return eq;
 }
 
+bool PlannedContact::isContactActive(const std::chrono::nanoseconds& t) const
+{
+ return (this->activationTime <= t) && (t < this->deactivationTime);
+}
+
 std::pair<bool, std::chrono::nanoseconds> EstimatedContact::getContactDetails() const
 {
  return std::make_pair(isActive, switchTime);

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

@@ -65,12 +65,29 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
  stepHeight is the maximum of the trajectory. */
  double m_footApexTime{0.5}; /**< Number between 0 and 1 representing the foot apex instant */
 
+ double m_footLandingVelocity{0.0}; /**< Landing velocity in \f$m/s\f$ */
+ double m_footLandingAcceleration{0.0}; /**< Landing acceleration in \f$m/s^2\f$ */
+
+ double m_footTakeOffVelocity{0.0}; /**< Take off velocity in \f$m/s\f$ */
+ double m_footTakeOffAcceleration{0.0}; /**< Take off acceleration in \f$m/s^2\f$ */
+
+ bool m_isOutputValid{false}; /**< True if getOutput returns meaningful data */
+
  /**
  * Update the SE3 Trajectory.
  * @return True in case of success/false otherwise.
  */
  bool updateSE3Traj();
 
+ /**
+ * Create a new SE3Trajectory considering the previous and next contact
+ * @return True in case of success/false otherwise.
+ */
+ bool createSE3Traj(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);
+
 public:
  /**
  * Initialize the planner.
@@ -81,20 +98,42 @@ class SwingFootPlanner : public System::Source<SwingFootPlannerState>
  * | `sampling_time` | `double` | Sampling time of the planner in seconds | Yes |
  * | `step_height` | `double` | Height of the swing foot. It is not the maximum height of the foot. If apex time is 0.5 `step_height` is the maximum | Yes |
  * | `foot_apex_time` | `double` | Number between 0 and 1 representing the foot apex instant. If 0 the apex happens at take off if 1 at touch down | Yes |
- * | `foot_landing_velocity` | `double` | Landing vertical velocity (default value 0.0) | No |
- * | `foot_landing_acceleration` | `double` | Landing vertical acceleration (default value 0.0) | No |
- * | `foot_take_off_velocity` | `double` | Take-off vertical velocity (default value 0.0) | No |
- * | `foot_take_off_acceleration` | `double` | Take-off vertical acceleration (default value 0.0) | No |
- * | `interpolation_method` | `string` | Define the interpolation method for the trajectory of the position. Accepted parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No |
+ * | `foot_landing_velocity` | `double` | Landing vertical velocity (default value 0.0) | No  |
+ * | `foot_landing_acceleration` | `double` | Landing vertical acceleration (default value 0.0) | No  |
+ * | `foot_take_off_velocity` | `double` | Take-off vertical velocity (default value 0.0) | No  |
+ * | `foot_take_off_acceleration` | `double` | Take-off vertical acceleration (default value 0.0) | No  |
+ * | `interpolation_method` | `string` | Define the interpolation method for the trajectory of the position. Accepted parameters: `min_acceleration`, `min_jerk` (default value `min_acceleration`) | No  |
  * @return True in case of success/false otherwise.
  */
  bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) final;
 
  /**
  * Set the contact list
- * @param contactList contains the list fora given contact
+ * @param contactList contains the list for a given contact.
+ * @return true in case of success, false otherwise.
+ * @note The contact list can be updated at run-time, i.e., when the planner is running. However
+ * the new contact list must satify a set of hypothesis.
+ * If the contact list stored in the class is empty, then it is the first time the
+ * contact list is added to the planner. In this case we accept all kinds of ContactList
+ * If the contact list is not empty, we check if it is possible to update the list. Given some
+ * limitations of the framework (mainly due to the SO3 trajectory generation) for the time
+ * being, we support only the two following cases:
+ * - Given the current time instant, both the stored and the new contact lists must have an
+ * active contact at the same pose.
+ * - If the contact is not active (swing phase) the next contact must satisfy the following two
+ * hypothesis
+ * 1. the orientation of the next contact must be the same as the orientation of the next
+ * contact in the contact list stored in the class.
+ * 2. the impact time of the next contact must be the same as the one of the next contact in
+ * the contact list stored in the class.
+ */
+ bool setContactList(const Contacts::ContactList& contactList);
+
+ /**
+ * Reset the time.
+ * @param time internal time of the system.
  */
- void setContactList(const Contacts::ContactList& contactList);
+ void setTime(const std::chrono::nanoseconds& time);
 
  /**
  * @brief Get the object.