Review TrajectoryLib

[jgvictores]

Improve TrajectoryLib, starting by decoupling one and two limb trajectory generation.

Blocks #133, and more to come.

[jgvictores]

Blocks #135 in addition to #133.

[jgvictores]

((comment omitted, DoF applies to joint space, here we are in Cartesian space))

[jgvictores]

Note that several design decisions must be taken. We might have a meeting in person for this, I'll keep you informed.
cc: @PeterBowman @rsantos88

[jgvictores]

@PeterBowman commented that he had started refractoring TrajectoryLib a while ago. Maybe some code or ideas hat we can reuse.

[jgvictores]

[jgvictores]

Also blocks roboticslab-uc3m/teo-grasp#4

[jgvictores]

@PeterBowman expressed concerns regarding being able to add several waypoints in a single function call.
@rsantos88 this is also useful for teo-grasp in relation to porting trajectories from openrave.

With https://github.com/roboticslab-uc3m/tools also in mind, I'm raising the bar and invoking some magic keywords:

serialization/deserialization

A.k.a. marshalling and unmarshalling.

[rsantos88]

Just an idea, I think it would be interesting to make an example with the functional part of this library (in case it is usable) for example with LineTrajectory

[jgvictores]

Just an idea, I think it would be interesting to make an example with the functional part of this library (in case it is usable) for example with LineTrajectory

Yes! Definitively. As a bare minimum, we should at least test the resulting class(es).

[jgvictores]

Lots of progress (developed ITrajectoy, ICartesianTrajectoy, KdlTrajectory) at f02a0d8, actually improving BasicCartesianControl too (#135).

[jgvictores]

Just an idea, I think it would be interesting to make an example with the functional part of this library (in case it is usable) for example with LineTrajectory

Added this test at 92a0c0b.

Still pending (but coming soon!): trajectories with paths other than ICartesianTrajectory::LINE.

[jgvictores]

In addition to the test, there's also some pretty nice doxygen by now.

Next thoughts should be in the line of discrete trajectories, to address the use case of, say, wanting to retrieve the timestamp of the n-th waypoint.

[jgvictores]

Next thoughts should be in the line of discrete trajectories, to address the use case of, say, wanting to retrieve the timestamp of the n-th waypoint.

In addition to a potential (linear?) interpolation if requesting a movementTime from any of the methods that does not correspond precisely to a waypoint (and e.g. avoiding the interpolation if it does correspond.... within a certain tolerance xD).

[jgvictores]

WIP at 6f3e7d8.

[jgvictores]

Also blocks roboticslab-uc3m/teo-grasp#3

[jgvictores]

It turns out that libraries/YarpPlugins/AsibotSolver/TrajGen.hpp would be useful for the new libraries/TrajectoryLib/BasicJointTrajectory.hpp. @PeterBowman Maybe we could move things around (also rename a bit, something like OneJointTrajectory)?

[PeterBowman]

TrajGen was kept for historical reasons, no code of ours is using that ATOW. If I'm correct, we even came close to removing it since KDL provides a similar functionality: KDL::VelocityProfile_Spline (supports up to 5th order splines, which we don't). Does it make sense to integrate/move TrajGen in the IJointTrajectory inheritance tree (even expand it so that initial/final accelerations are taken into account) and, if we ever need it, support velocity splines in the ICartesianTrajectory tree by means of the existing KdlTrajectory wrapper? Concerns: code duplication on one hand, avoiding unnecessary(/unrelated?) dependencies on the other hand.

[jgvictores]

I'm not a big fan of adding dependencies, but even less a fan of code duplication. After reading your comment, this makes me inclined towards the use of KDL even for joint space trajectories. At time of writing KDL::VelocityProfile provides 5 different implementations that we may have ended up re-implementing (including the KDL::VelocityProfile_Spline you cite).

Looking at current issue-164-joint branch, maybe some renaming to be done?

• KdlTrajectory -> KdlCartesianTrajectory
• BasicJointTrajectory -> KdlJointTrajectory

[PeterBowman]

KDL::VelocityProfile provides 5 different implementations that we may have ended up re-implementing

For the record, we already use KDL::VelocityProfile_Trap, and KDL::VelocityProfile_Rectangular is WIP for #166. But, there is no other re-implementation I know of apart from said TrajLib/KDL::DL::VelocityProfile_Spline. That one originated at asibot-main, probably before KDL was first used in RL code.

maybe some renaming to be done?

If KDL already has everything we need to implement IJointTrajectory, fine, then. I'll take care of edit conflicts across branches.

[jgvictores]

That one originated at asibot-main, probably before KDL was first used in RL code.

TrajGen was intended to be a light-weight alternative to KDL with no additional dependencies. There was also a learn-by-doing motivation on my side. These reasons are IMHO historical and there is no strong motivation against using KDL even for joint space trajectories. It will actually allow Joint and Cartesian space trajectories to mutually benefit from any improvements/extensions in velocity profiles.

If KDL already has everything we need to implement IJointTrajectory, fine, then. I'll take care of edit conflicts across branches.

I'll give a try at using KDL in BasicJointTrajectory, I don't want to count my 🐔 before they hatch. If all goes well, I'll comment here and ask for your help for renaming while avoiding conflicts across branches.

[jgvictores]

Slightly blocked, focusing on roboticslab-uc3m/teo-main#38 right now.

[PeterBowman]

ASWJ we might consider dropping the ITrajectory wrapper class tree in favor of vanilla KDL's motion API.

[jgvictores]

As spoken at #134 (comment), KDL's motion API seems reasonable.

Furthermore, here are some notes to future selves:

1. I would not completely forget ideas at figure Review TrajectoryLib #134 (comment)
2. I'd also like to keep in mind seeing how this connects to:
   • YARP: https://www.yarp.it/yarpdataplayer.html but also https://www.yarp.it/yarpmotorgui.html
   • OpenRAVE: http://openrave.org/docs/0.8.0/architecture/trajectory/
   • Also recall spaceNavigator as well as https://github.com/roboticslab-uc3m/daw2robot

Remember to update:

1. http://robots.uc3m.es/gitbook-developer-manual/frequently-asked-questions.html#how-can-i-record-data-for-programming-by-demonstration-pbd-aka-learning-from-demonstration-lfd
2. http://robots.uc3m.es/gitbook-developer-manual/frequently-asked-questions.html#how-can-i-play-back-data-recorded-for-programming-by-demonstration-pbd-aka-learning-from-demonstration-lfd

[jgvictores]

Recent discovery: https://www.yarp.it/namespaceyarp_1_1rosmsg_1_1trajectory__msgs.html

[PeterBowman]

@jgvictores, let's ask the bold question: should we drop branch issue-164-joint (badly named, by the way) and turn this issue into "Nuke TrajectoryLib"? That is, use KDL directly instead of this thin wrapper of ours. I'm not quite proud of it and its pitfalls, had to code very carefully in the past and yet kept finding memory leaks. Extending this library (e.g. for rectangular velocity profiles) felt kinda useless as I could just call KDL methods instead.

[jgvictores]

let's ask the bold question: should we drop branch issue-164-joint (badly named, by the way) and turn this issue into "Nuke TrajectoryLib"? That is, use KDL directly instead of this thin wrapper of ours. I'm not quite proud of it and its pitfalls, had to code very carefully in the past and yet kept finding memory leaks. Extending this library (e.g. for rectangular velocity profiles) felt kinda useless as I could just call KDL methods instead.

Yesssssssssssssssssss 👍

[PeterBowman]

Tactical nuke dropped, there is no more TrajectoryLib at 692888d. Last stable implementation was available here. The issue-164-joint branch will be deleted, here is the full patchset for reference: a2bd56c...d266572.

Keep in mind the KDL motion API allows you not to configure certain parameters on construction, such as trajectory duration, maximum velocity, reference acceleration and so on. However, those must be accounted for anyway. A particular scenario involves trajectories with a starting point, a constant velocity and infinite duration (no end point):

kinematics-dynamics/libraries/TrajectoryLib/KdlTrajectory.cpp

Lines 242 to 280 in a9acac8

	if( duration == DURATION_NOT_SET )
	{
	if (velocityDrivenPath)
	{
	// assume we'll execute this trajectory indefinitely; since velocity
	// depends on the path to travel along and the total duration, let's
	// fix both to adjust the resulting velocity as requested by the user
	double vel = path->PathLength(); // distance traveled during 1 time unit
	double dummyGoal = 1e9; // somewhere far away
	double dummyDuration = dummyGoal / vel;
	velocityProfile->SetProfileDuration(0, dummyGoal, dummyDuration);
	currentTrajectory = new KDL::Trajectory_Segment(path, velocityProfile);
	}
	else
	{
	velocityProfile->SetProfile(0, path->PathLength());
	currentTrajectory = new KDL::Trajectory_Segment(path, velocityProfile);
	}
	}
	else
	{
	if (velocityDrivenPath)
	{
	// execute the trajectory given an initial velocity and duration
	double vel = path->PathLength(); // distance traveled during 1 time unit
	double guessedGoal = vel * duration;
	velocityProfile->SetProfileDuration(0, guessedGoal, duration);
	currentTrajectory = new KDL::Trajectory_Segment(path, velocityProfile);
	}
	else
	{
	// velocity profile is set under the hood
	currentTrajectory = new KDL::Trajectory_Segment(path, velocityProfile, duration);
	}
	}

SetProfileDuration can be used with a dummy waypoint which we know would never be reached, e.g. 10 meters away from start:

kinematics-dynamics/programs/keyboardController/LinearTrajectoryThread.cpp

Lines 91 to 95 in a2bd56c

	KDL::Path * path = new KDL::Path_Line(H, tw, new KDL::RotationalInterpolation_SingleAxis(), 1.0);
	KDL::VelocityProfile * profile = new KDL::VelocityProfile_Rectangular(10.0);
	profile->SetProfileDuration(0.0, 10.0, 10.0 / path->PathLength());
	trajectory = new KDL::Trajectory_Segment(path, profile);
	startTime = yarp::os::Time::now();