Add zmp generator and populate all outputs of the unycycle trajectory generator

[LoreMoretti]

Some of the outputs of the UnicycleTrajectoryGenerator were not being populated.

This PR fixes it.

Moreover, the ZMP generator (and its outputs) is added to the UnicycleTrajectoryGenerator

[S-Dafarra]

At the code level, it looks good. What I am a bit afraid of is that from outside it is not clear that the different trajectories are not coherent with each other. At the unicycle planner level, you are calling to different generators, so somehow you can expect them to be separate. Here instead, the output is in a single data structure. Maybe it would be useful to have a trigger to turn on or off this output?

[LoreMoretti]

At the code level, it looks good. What I am a bit afraid of is that from outside it is not clear that the different trajectories are not coherent with each other. At the unicycle planner level, you are calling to different generators, so somehow you can expect them to be separate. Here instead, the output is in a single data structure. Maybe it would be useful to have a trigger to turn on or off this output?

You're right! It makes totally sense.

With the commit ecbd51f I tried to make the dcmTrajectory and the zmpTrajectory optional (and enabled by boolean flags).

I like the fact that std::optional makes the code cleaner in my opinion.

There's the drawback. This change makes the blf Unicycle Trajectory Generator not backward compatible. But I think the number of users is still low.

Let me know what you think.

[S-Dafarra]

There's the drawback. This change makes the blf Unicycle Trajectory Generator not backward compatible. But I think the number of users is still low.

Let me know what you think.

It should be fine by changing the version number. On the other hand, another alternative is to apply this change only for the data you added

[LoreMoretti]

@GiulioRomualdi can we merge this PR?

[GiulioRomualdi]

Some minor changes

[GiulioRomualdi]

Perhaps this can be be removed since there is already BipedalLocomotion::Conversions::toManifPose

[LoreMoretti]

The reason why I added it is because I used it as a helper function in the following function templates:

bipedal-locomotion-framework/src/Planners/include/BipedalLocomotion/Planners/UnicycleUtilities.h

Lines 111 to 124 in c2bae6f

	template <typename From, typename To>
	void convertVector(const std::vector<From>& inputVect, std::vector<To>& outputVect)
	{
	outputVect.resize(inputVect.size());
	for (size_t i = 0; i < inputVect.size(); ++i)
	{
	convertToBLF(inputVect[i], outputVect[i]);
	}
	}
	} // namespace Conversions
	} // namespace BipedalLocomotion::Planners::UnicycleUtilities

If I remove void convertToBLF(const iDynTree::Transform& input, manif::SE3d& output), I think I should modify the template function as:

• perform a check on <From> type.
• if it is iDynTree::Transform& then call BipedalLocomotion::Conversions::toManifPose instead of void convertToBLF(const iDynTree::Transform& input, manif::SE3d& output).

Do you have other better alternatives?
Is it worth the change?

[GiulioRomualdi]

You convinced me!