Restructure the SwingFootPlanner, the Contact component and the CentroidalMPCOutput

bindings/python/ML/src/MANNAutoregressive.cpp

@@ -52,16 +52,10 @@ void CreateMANNAutoregressive(pybind11::module& module)
                                    ML::MANNAutoregressiveOutput>>(module, "MANNAutoregressive")
         .def(py::init())
         .def("reset",
-             py::overload_cast<const ML::MANNInput&,
-                               const Contacts::EstimatedContact&,
-                               const Contacts::EstimatedContact&,
-                               const manif::SE3d&,
-                               const manif::SE3Tangentd&>(&ML::MANNAutoregressive::reset),
-             py::arg("input"),
-             py::arg("left_foot"),
-             py::arg("right_foot"),
-             py::arg("base_pose"),
-             py::arg("base_velocity"))
+             py::overload_cast<Eigen::Ref<const Eigen::VectorXd>, const manif::SE3d&>(
+                 &ML::MANNAutoregressive::reset),
+             py::arg("joint_positions"),
+             py::arg("base_pose"))
         .def("set_robot_model", [](ML::MANNAutoregressive& impl, ::pybind11::object& obj) {
             iDynTree::Model* cls = py::detail::swig_wrapped_pointer_to_pybind<iDynTree::Model>(obj);
 

bindings/python/ML/src/MANNTrajectoryGenerator.cpp

@@ -40,8 +40,9 @@ void CreateMANNTrajectoryGenerator(pybind11::module& module)
         .def_readwrite("joint_positions", &ML::MANNTrajectoryGeneratorOutput::jointPositions)
         .def_readwrite("base_pose", &ML::MANNTrajectoryGeneratorOutput::basePoses)
         .def_readwrite("phase_list", &ML::MANNTrajectoryGeneratorOutput::phaseList)
-        .def_readwrite("com_trajectory" , &ML::MANNTrajectoryGeneratorOutput::comTrajectory)
-        .def_readwrite("angular_momentum_trajectory" , &ML::MANNTrajectoryGeneratorOutput::angularMomentumTrajectory);
+        .def_readwrite("com_trajectory", &ML::MANNTrajectoryGeneratorOutput::comTrajectory)
+        .def_readwrite("angular_momentum_trajectory",
+                       &ML::MANNTrajectoryGeneratorOutput::angularMomentumTrajectory);
 
     BipedalLocomotion::bindings::System::CreateAdvanceable<ML::MANNTrajectoryGeneratorInput, //
                                                            ML::MANNTrajectoryGeneratorOutput> //
@@ -54,20 +55,19 @@ void CreateMANNTrajectoryGenerator(pybind11::module& module)
         .def("set_initial_state",
              &ML::MANNTrajectoryGenerator::setInitialState,
              py::arg("joint_positions"),
-             py::arg("left_foot"),
-             py::arg("right_foot"),
-             py::arg("base_pose"),
-             py::arg("time"))
-        .def("set_robot_model", [](ML::MANNTrajectoryGenerator& impl, ::pybind11::object& obj) {
-            iDynTree::Model* cls = py::detail::swig_wrapped_pointer_to_pybind<iDynTree::Model>(obj);
+             py::arg("base_pose"))
+        .def("set_robot_model",
+             [](ML::MANNTrajectoryGenerator& impl, ::pybind11::object& obj) {
+                 iDynTree::Model* cls
+                     = py::detail::swig_wrapped_pointer_to_pybind<iDynTree::Model>(obj);
 
-            if (cls == nullptr)
-            {
-                throw ::pybind11::value_error("Invalid input for the function. Please provide "
-                                              "an iDynTree::Model object.");
-            }
-            return impl.setRobotModel(*cls);
-        });
+                 if (cls == nullptr)
+                 {
+                     throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                                   "an iDynTree::Model object.");
+                 }
+                 return impl.setRobotModel(*cls);
+             });
 }
 
 } // namespace ML

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/ReducedModelControllers/src/CentroidalMPC.cpp

@@ -29,7 +29,7 @@ void CreateCentroidalMPC(pybind11::module& module)
     py::class_<CentroidalMPCOutput>(module, "CentroidalMPCState")
         .def(py::init())
         .def_readwrite("contacts", &CentroidalMPCOutput::contacts)
-        .def_readwrite("next_planned_contact", &CentroidalMPCOutput::nextPlannedContact)
+        .def_readwrite("contact_phase_list", &CentroidalMPCOutput::contactPhaseList)
         .def_readwrite("com_trajectory", &CentroidalMPCOutput::comTrajectory);
 
     BipedalLocomotion::bindings::System::CreateSource<CentroidalMPCOutput>(module,

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/launch-yarp-robot-logger.xml

@@ -9,8 +9,8 @@ BSD-3-Clause license. -->
     <devices>
         <!-- interface -->
         <xi:include href="blf-yarp-robot-logger-interfaces/all_joints_mc.xml" />
-        <xi:include href="blf-yarp-robot-logger-interfaces/ft_clients.xml" />
-        <xi:include href="blf-yarp-robot-logger-interfaces/mas-remapper.xml" />
+        <!-- <xi:include href="blf-yarp-robot-logger-interfaces/ft_clients.xml" /> -->
+        <!-- <xi:include href="blf-yarp-robot-logger-interfaces/mas-remapper.xml" /> -->
         <xi:include href="./yarp-robot-logger.xml" />
     </devices>
 </robot>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml

@@ -16,22 +16,15 @@ BSD-3-Clause license. -->
   </group>
 
   <group name="ExogenousSignals">
-    <param name="vectors_collection_exogenous_inputs">()</param>
+    <param name="vectors_collection_exogenous_inputs">(Walking)</param>
     <param name="vectors_exogenous_inputs">()</param>
 
-    <!--group name="Balancing">
-      <param name="local">"/yarp-robot-logger/exogenous_signals/balancing:i"</param>
-      <param name="remote">"/balancing_controller/logger/data:o"</param>
-      <param name="signal_name">"balancing"</param>
-      <param name="carrier">"udp"</param>
-    </group-->
-
-    <!--<group name="Walking">
+    <group name="Walking">
       <param name="local">"/yarp-robot-logger/exogenous_signals/walking:i"</param>
-      <param name="remote">"/walking-coordinator/logger/data:o"</param>
+      <param name="remote">"/centroidal-mpc/log:o"</param>
       <param name="signal_name">"walking"</param>
       <param name="carrier">"udp"</param>
-    </group>-->
+    </group>
 
     <!--<group name="RobotDynamicsEstimator">
       <param name="local">"/yarp-robot-logger/exogenous_signals/rde:i"</param>
@@ -60,7 +53,7 @@ BSD-3-Clause license. -->
     <param name="stream_joint_states">true</param>
     <param name="stream_inertials">false</param>
     <param name="stream_cartesian_wrenches">false</param>
-    <param name="stream_forcetorque_sensors">true</param>
+    <param name="stream_forcetorque_sensors">false</param>
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">false</param>
     <param name="stream_temperatures">false</param>
@@ -79,8 +72,7 @@ BSD-3-Clause license. -->
   <action phase="startup" level="15" type="attach">
     <paramlist name="networks">
       <elem name="all_joints">all_joints_mc</elem>
-      <elem name="mas-remapper">mas-remapper</elem>
-
+      <!-- <elem name="mas-remapper">mas-remapper</elem> -->
     </paramlist>
   </action>
 

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

@@ -89,6 +89,14 @@ struct PlannedContact : ContactBase
      */
     bool operator==(const PlannedContact& other) const;
 
+    /**
+     * @brief The inequality operator.
+     *
+     * @param other The other object used for the comparison.
+     * @return True if the contacts are the different, false otherwise.
+     */
+    bool operator!=(const PlannedContact& other) const;
+
     /**
      * @brief Check if the contact is active at a give time instant
      *

src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h

@@ -219,6 +219,30 @@ class ContactList
      */
     const_iterator getPresentContact(const std::chrono::nanoseconds& time) const;
 
+    /**
+     * @brief Get the active contact given the time.
+     *
+     * It returns the active contact (i.e., the highest activation time lower than time and the deactivation
+     * time strictly higher than time)
+     * If no contacts is active at the given time, it returns an iterator to the end.
+     * @param time The present time.
+     * @return an iterator to the last contact  having an activation time lower than time.
+     * If no contact satisfy this condition, it returns a pointer to the end.
+     * @note Differently from getPresentContact the contact needs to be  active.
+     */
+    const_iterator getActiveContact(const std::chrono::nanoseconds& time) const;
+
+    /**
+     * @brief Get the next contact given the time.
+     *
+     * It returns the contact with the lowest activation time higher than time.
+     * If no contacts have an activation time higher than time, it returns an iterator to the end.
+     * @param time The present time.
+     * @return an iterator to the first contact having an activation time higher than time.
+     * If no contact satisfy this condition, it returns a pointer to the end.
+     */
+    const_iterator getNextContact(const std::chrono::nanoseconds& time) const;    
+
     /**
      * @brief Clear all the steps, except the one returned by getPresentContact
      * @param time The present time.

src/Contacts/src/Contact.cpp

@@ -14,15 +14,18 @@ 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;
     eq = eq && this->type == other.type;
     eq = eq && this->pose.coeffs() == other.pose.coeffs();
     eq = eq && this->activationTime == other.activationTime;
     eq = eq && this->deactivationTime == other.deactivationTime;
     return eq;
 }
 
+bool PlannedContact::operator!=(const PlannedContact& other) const
+{
+    return !(*this == other);
+}
+
 bool PlannedContact::isContactActive(const std::chrono::nanoseconds& t) const
 {
     return (this->activationTime <= t) && (t < this->deactivationTime);

src/Contacts/src/ContactList.cpp

@@ -231,6 +231,40 @@ ContactList::getPresentContact(const std::chrono::nanoseconds& time) const
                                       // iterator to the next element.
 }
 
+ContactList::const_iterator
+ContactList::getActiveContact(const std::chrono::nanoseconds& time) const
+{
+    // With the reverse iterator we find the last step such that the activation time is smaller
+    // equal than time
+    ContactList::const_reverse_iterator presentReverse
+        = std::find_if(rbegin(), rend(), [time](const PlannedContact& a) -> bool {
+              return a.activationTime <= time && a.deactivationTime > time;
+          });
+
+    if (presentReverse == rend())
+    {
+        // No contact has activation time lower than the specified time.
+        return end();
+    }
+
+    return --(presentReverse.base()); // This is to convert a reverse iterator to a forward
+                                      // iterator. The -- is because base() returns a forward
+                                      // iterator to the next element.
+}
+
+ContactList::const_iterator
+ContactList::getNextContact(const std::chrono::nanoseconds& time) const
+{
+    // With the reverse iterator we find the last step such that the activation time is smaller
+    // equal than time
+    ContactList::const_iterator nextContact
+        = std::find_if(begin(), end(), [time](const PlannedContact& a) -> bool {
+              return a.activationTime >= time;
+          });
+
+    return nextContact;
+}
+
 bool ContactList::keepOnlyPresentContact(const std::chrono::nanoseconds& time)
 {
     ContactList::const_iterator dropPoint = getPresentContact(time);

src/ML/include/BipedalLocomotion/ML/MANN.h

@@ -27,8 +27,8 @@ namespace ML
  * The base position trajectory, facing direction trajectory and base velocity trajectories are
  * written in a bidimensional local reference frame L in which we assume all the quantities related
  * to the ground-projected base trajectory in xi and yi to be expressed. At each step ti,
- * L is defined to have its origin in the current ground-projected robot base position and orientation defined
- * by the current facing direction (along with its orthogonal vector).
+ * L is defined to have its origin in the current ground-projected robot base position and
+ * orientation defined by the current facing direction (along with its orthogonal vector).
  */
 struct MANNInput
 {
@@ -45,6 +45,17 @@ struct MANNInput
     Eigen::VectorXd jointPositions; /**< Vector containing the actual joint position in radians. */
     Eigen::VectorXd jointVelocities; /**< Vector containing the actual joint velocity in radians per
                                         seconds. */
+
+    /**
+     * Generate a dummy MANNInput from a given joint configuration
+     * @param jointPositions vector containing the joint position in radians.
+     * @param projectedBaseHorizon number of samples of the base horizon considered in the neural
+     * network.
+     * @return the MANNInput.
+     * @warning the function assumes that the robot is not moving and facing forward.
+     */
+    static MANNInput generateDummyMANNInput(Eigen::Ref<const Eigen::VectorXd> jointPositions,
+                                            std::size_t projectedBaseHorizon);
 };
 
 /**
@@ -73,6 +84,17 @@ struct MANNOutput
     Eigen::VectorXd jointVelocities; /**< Vector containing the next joint velocity in radians per
                                         seconds */
     manif::SE2Tangentd projectedBaseVelocity; /**< Velocity of the base projected on the ground */
+
+    /**
+     * Generate a dummy MANNOutput from a given joint configuration
+     * @param jointPositions vector containing the joint position in radians.
+     * @param futureProjectedBaseHorizon number of samples of the base horizon generated as output by
+     * the neural network.
+     * @return the MANNOutput.
+     * @warning the function assumes that the robot is not moving and facing forward.
+     */
+    static MANNOutput generateDummyMANNOutput(Eigen::Ref<const Eigen::VectorXd> jointPositions,
+                                              std::size_t futureProjectedBaseHorizon);
 };
 
 /**
@@ -135,7 +157,6 @@ class MANN : public BipedalLocomotion::System::Advanceable<MANNInput, MANNOutput
     bool isOutputValid() const override;
 
 private:
-
     /** Private implementation */
     struct Impl;
     std::unique_ptr<Impl> m_pimpl;