BipedalLocomotion::ML::MANNTrajectoryGenerator class
#include <BipedalLocomotion/ML/MANNTrajectoryGenerator.h>

MANNTrajectoryGenerator is a class that uses MANNAutoregressive to generate a kinematically feasible trajectory for humanoid robots.

The planner will generate a trajectory which duration is equal to slow_down_factor * time_horizon seconds.

MANN trajectory generator

The diagram illustrates the utilization of the MANNAutoregressive within the MANNTrajectoryGenerator class.

To initialize the generator, the user needs to set the initial state using the MANNTrajectoryGenerator::setInitialState method. The MANNTrajectoryGeneratorInput, provided by the user, serves as the input for the MANNAutoregressiveInput, along with the 'mergePointIndex.' The MANNAutoregressiveInput is assumed to remain constant within the trajectory horizon. The 'mergePointIndex' indicates the index at which the new trajectory will be attached. For example, when the MANNTrajectoryGenerator generates a trajectory consisting of 'N' points, if the 'mergePointIndex' is set to 3, the first three elements of the new trajectory will be derived from the previously computed trajectory by MANNTrajectoryGeneratorOutput, utilizing the previous MANNAutoregressiveInput. Subsequently, all points from 3 to N will be evaluated using the current MANNAutoregressiveInput. This behavior is facilitated by a mechanism that stores the autoregressive state required for resetting the MANNAutoregressive at the designated merge point. Every time the MANNAutoregressive::advance() function is invoked by the MANNTrajectoryGenerator, the autoregressive state is stored for future reference.

Base classes

template<class _Input, class _Output> class BipedalLocomotion::System::Advanceable<MANNTrajectoryGeneratorInput, MANNTrajectoryGeneratorOutput>: Basic class that represents a discrete system.

Constructors, destructors, conversion operators

MANNTrajectoryGenerator(): Constructor.
~MANNTrajectoryGenerator(): Destructor.

Public functions

auto setRobotModel(const iDynTree::Model& model) -> bool: Set the robot model.
auto initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) -> bool override: Initialize the trajectory planner.
auto setInput(const Input& input) -> bool override: Set the input to the planner model.
auto advance() -> bool override: Generate the trajectory.
auto isOutputValid() const -> bool override: Check if the output is valid.
auto getOutput() const -> const Output& override: Get the output from trajectory.
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): Set the initial state of the planner.

Function documentation

bool BipedalLocomotion::ML::MANNTrajectoryGenerator::setRobotModel(const iDynTree::Model& model)

Set the robot model.

Parameters
model	model of the robot considered by the network. Please load the very same model with the same joint serialization used to train the MANN network.
Returns	true in case of success, false otherwise.

bool BipedalLocomotion::ML::MANNTrajectoryGenerator::initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override

Initialize the trajectory planner.

Parameters
paramHandler	pointer to the parameters handler.
Returns	True in case of success, false otherwise.

Note

the following parameters are required by the class

Parameter Name	Type	Description	Mandatory
`time_horizon`	`double`	Horizon of the planner.	Yes
`sampling_time`	`double`	Sampling considered in the inference.	Yes
`root_link_frame_name`	`string`	Name of of the root link frame in the model.	Yes
`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

It is also required to define two groups LEFT_FOOT and RIGHT_FOOT that contains the following parameter

Parameter Name	Type	Description	Mandatory
`number_of_corners`	`int`	Number of corners associated to the foot	Yes
`corner_<i>`	`vector<double>`	Position of the corner expressed in the foot frame. I must be from 0 to number_of_corners - 1	Yes

Finally it also required to define a group named MANN that contains the following parameter

Parameter Name	Type	Description	Mandatory
`onnx_model_path`	`string`	Path to the `onnx` model that will be loaded to perform inference	Yes
`projected_base_horizon`	`int`	Number of samples of the base horizon considered in the model	Yes

bool BipedalLocomotion::ML::MANNTrajectoryGenerator::setInput(const Input& input) override

Set the input to the planner model.

Parameters
input	input to the model
Returns	true in case of success, false otherwise.

bool BipedalLocomotion::ML::MANNTrajectoryGenerator::advance() override

Generate the trajectory.

Returns	true in case of success, false otherwise.

bool BipedalLocomotion::ML::MANNTrajectoryGenerator::isOutputValid() const override

Check if the output is valid.

Returns	true if the output is valid, false otherwise.

const Output& BipedalLocomotion::ML::MANNTrajectoryGenerator::getOutput() const override

Get the output from trajectory.

Returns	the output of the system.

void BipedalLocomotion::ML::MANNTrajectoryGenerator::setInitialState(Eigen::Ref<const Eigen::VectorXd> jointPositions, const Contacts::EstimatedContact& leftFoot, const Contacts::EstimatedContact& rightFoot, const manif::SE3d& basePose, const std::chrono::nanoseconds& time)

Set the initial state of the planner.

Parameters
jointPositions	position of the joints.
leftFoot
rightFoot
basePose	pose of the base.
time	initial time of the planner.