Restructure MANNTrajectoryGenerator

- eliminate the need to pass foot positions in MANNTrajectoryGenerator::setInitialState
- enhance MANNTrajectoryGenerator with reset capabilities similar to MANNAutoregressive
- correct the position and time scaling of MANNTrajectoryGenerator to accurately scale the CoM, base, and feet positions
- update the Python bindings

bindings/python/ML/src/MANNTrajectoryGenerator.cpp

@@ -37,11 +37,13 @@ void CreateMANNTrajectoryGenerator(pybind11::module& module)
 
  py::class_<ML::MANNTrajectoryGeneratorOutput>(module, "MANNTrajectoryGeneratorOutput")
  .def(py::init())
+ .def_readwrite("timestamps", &ML::MANNTrajectoryGeneratorOutput::timestamps)
  .def_readwrite("joint_positions", &ML::MANNTrajectoryGeneratorOutput::jointPositions)
- .def_readwrite("base_pose", &ML::MANNTrajectoryGeneratorOutput::basePoses)
+ .def_readwrite("base_poses", &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,10 +56,7 @@ 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"))
+ 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);
 

src/ML/include/BipedalLocomotion/ML/MANNTrajectoryGenerator.h

@@ -44,6 +44,9 @@ struct MANNTrajectoryGeneratorOutput
 
  std::vector<Eigen::VectorXd> jointPositions; /**< Joints position in radians */
  std::vector<manif::SE3d> basePoses; /**< Vector containing the base pose for each instant. */
+
+ std::vector<std::chrono::nanoseconds> timestamps; /**< Vector containing the time stamps. */
+
  Contacts::ContactPhaseList phaseList; /**< List of the contact phases */
 };
 
@@ -96,6 +99,7 @@ class MANNTrajectoryGenerator
  */
  bool setRobotModel(const iDynTree::Model& model);
 
+ // clang-format off
  /**
  * Initialize the trajectory planner.
  * @param paramHandler pointer to the parameters handler.
@@ -108,8 +112,9 @@ class MANNTrajectoryGenerator
  * | `chest_link_frame_name` | `string` | Name of of the chest link frame in the model. | Yes |
  * | `right_foot_frame_name` | `string` | Name of of the right foot frame in the model. | Yes |
  * | `left_foot_frame_name` | `string` | Name of of the left foot frame in the model. | Yes |
- * | `forward_direction` | `string` | String cointaining 'x', 'y' or 'z' representing the foot link forward axis. Currently, only 'x' is supported. | Yes |
- * | `slow_down_factor` | `int` | The `horizon` will be `horizon * slow_down_factor`. Same for the `sampling_time` (default value 1). | No |
+ * | `forward_direction` | `string` | String containing 'x', 'y' or 'z' representing the foot link forward axis. Currently, only 'x' is supported. | Yes |
+ * | `slow_down_factor` | `double` | The `horizon` will be `horizon * slow_down_factor`. Same for the `sampling_time`. | Yes |
+ * | `scaling_factor` | `double` | Scaling factor considered in the generation of the CoM, base and contact points locations | Yes |
  * It is also required to define two groups `LEFT_FOOT` and `RIGHT_FOOT` that contains the following parameter
  * | Parameter Name | Type | Description | Mandatory |
  * |:------------------------:|:-----------------:|:----------------------------------------------------------------------------------------------:|:---------:|
@@ -123,6 +128,7 @@ class MANNTrajectoryGenerator
  * @return True in case of success, false otherwise.
  */
  bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ // clang-format on
 
  /**
  * Set the input to the planner model.
@@ -157,11 +163,8 @@ class MANNTrajectoryGenerator
  * @param basePose pose of the base.
  * @param time initial time of the planner.
  */
- void setInitialState(Eigen::Ref<const Eigen::VectorXd> jointPositions,
- const Contacts::EstimatedContact& leftFoot,
- const Contacts::EstimatedContact& rightFoot,
- const manif::SE3d& basePose,
- const std::chrono::nanoseconds& time);
+ bool setInitialState(Eigen::Ref<const Eigen::VectorXd> jointPositions, //
+ const manif::SE3d& basePose);
 
 private:
  struct Impl;