Implement MANNTrajectoryGenerator in ML component

[GiulioRomualdi]

This PR implements MANNTrajectoryGenerator.

The MANNTrajectoryGenerator class utilizes the MANNAutoregressive algorithm to generate a kinematically feasible trajectory for humanoid robots. The planner generates a trajectory with a duration equal to slow_down_factor * time_horizon seconds.

The diagram shows how the MANNAutoregressive is used within the MANNTrajectoryGenerator class.

To initialize the generator, the user must set the initial state using the MANNTrajectoryGenerator::setInitialState method. The generator takes MANNTrajectoryGeneratorInput as input, which is used as the input for 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, if the MANNTrajectoryGenerator generates a trajectory consisting of 'N' points and the mergePointIndex is set to 3, the first three elements of the new trajectory will be derived from the previously computed trajectory using MANNTrajectoryGeneratorOutput and the previous MANNAutoregressiveInput. Subsequently, all points from 3 to N will be evaluated using the current MANNAutoregressiveInput. This behavior is facilitated by storing the autoregressive state required for resetting the MANNAutoregressive at the designated merge point. Whenever the MANNAutoregressive::advance() function is invoked by the MANNTrajectoryGenerator, the autoregressive state is stored for future reference.

Drawio diagram adherent_autregressive.zip

[S-Dafarra]

Is there any test?

[GiulioRomualdi]

I dont know how to properly test it. What I can do is to try to generate a trajectory while asking to do not move and check if there is only a double support phase.

[paolo-viceconte]

Is there any test?

Properly checking seems tricky also to me, since a "weird" path could in principle be generated even if the implementation is correct.

What I can do is to try to generate a trajectory while asking to do not move and check if there is only a double support phase.

This makes sense, it is a very basic test but at least guarantees that the trajectory generation starts properly.

In addition, we could think of testing forward/backward/sideways walking by giving the correspondent input and checking whether after a certain amount of time the base/CoM/feet positions in the world frame increased significantly "only" in the expected direction. For instance, checking that after 10s of forward walking the CoM x increased a lot and the CoM y remained within a threshold. But still, it cannot be a very restrictive test because the performances of the architecture are robot- and training-dependent and can vary also a lot.

[GiulioRomualdi]

Hi @paolo-viceconte, we can easily implement the tests you suggested. At least we will have a test that spots some wired motion

[S-Dafarra]

In addition, we could think of testing forward/backward/sideways walking by giving the correspondent input and checking whether after a certain amount of time the base/CoM/feet positions in the world frame increased significantly "only" in the expected direction. For instance, checking that after 10s of forward walking the CoM x increased a lot and the CoM y remained within a threshold. But still, it cannot be a very restrictive test because the performances of the architecture are robot- and training-dependent and can vary also a lot.

Hi @paolo-viceconte, we can easily implement the tests you suggested. At least we will have a test that spots some wired motion

Awesome! Otherwise also a simple test where you call the methods and make sure it does not segfault 😉

[GiulioRomualdi]

Hi @S-Dafarra in 8fe6311 I implemented a test to check that the robot walks forward when the planner is called with a given set of inputs

[S-Dafarra]

Actually, what is that 51?

[GiulioRomualdi]

@paolo-viceconte

[paolo-viceconte]

51 is the length of 1 second of past trajectory stored in the autoregressive state. Since the original mocap data are collected at 50 Hz, and therefore the trajectory generation is assumed to proceed at 50 Hz, we need 50 datapoints to store the past second of trajectory. Along with the present datapoint, they sum up to 51!

[GiulioRomualdi]

This comment will become a comment of the code

[GiulioRomualdi]

This comment will become a comment of the code

[GiulioRomualdi]

Done here bb33760

[S-Dafarra]

Is it worth having it as an input parameter? Ideally, it should be one metadata of the loaded model

[GiulioRomualdi]

Yes, I agree. In the next PR, we are going to generate the trajectory from the joypad. I think we can try to handle this in that PR

[GiulioRomualdi]

Merging for the time being, keeping in mind that we need to handle this #668 (comment) in the PR related to the generation of the input to the planner (cc @paolo-viceconte)