search

roboticslab-uc3m / kinematics-dynamics

Kinematics and dynamics solvers and controllers.
https://robots.uc3m.es/kinematics-dynamics/
GNU Lesser General Public License v2.1
19 stars 12 forks source link

Review TrajectoryLib #134

Closed jgvictores closed 3 years ago

jgvictores commented 6 years ago

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

Blocks #133, and more to come.

jgvictores commented 6 years ago

Blocks #135 in addition to #133.

jgvictores commented 6 years ago

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

jgvictores commented 6 years ago

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 commented 6 years ago

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

jgvictores commented 6 years ago

imag4137

jgvictores commented 6 years ago

Also blocks https://github.com/roboticslab-uc3m/teo-grasp/issues/4

jgvictores commented 6 years ago

@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 commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

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

jgvictores commented 6 years ago

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 92a0c0b018ce55c936b812b0d204281125516822.

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

jgvictores commented 6 years ago

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 commented 6 years ago

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 commented 6 years ago

Began IJointTrajectory at bd660a4dc94cf10b76ca84037211c2d4f2653c20, currently at issue-164-joint branch.

PeterBowman commented 6 years ago

https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/157 affects on how trajectories are to be generated: pass a duration parameter straight to the KDL API, or do some math with distances/velocities and obtain approximate duration first.

PeterBowman commented 5 years ago

Currently, maximum velocity and acceleration defaults are enforced:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/8462fa859847378afe6e75d7bd4fb463d61f2b2b/libraries/TrajectoryLib/KdlTrajectory.hpp#L17-L18

I believe we should enable their customization somehow (through additional interface methods or via constructor?).

PeterBowman commented 5 years ago

I believe we should enable their customization somehow (through additional interface methods or via constructor?).

Added to constructor in https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/d223d392e0cbb63b28e3e7594ee16251148045ae (not merged yet).

jgvictores commented 5 years ago

WIP at 6f3e7d8.

jgvictores commented 5 years ago

Also blocks https://github.com/roboticslab-uc3m/teo-grasp/issues/3

jgvictores commented 5 years ago

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 commented 5 years ago

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 commented 5 years ago

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?

PeterBowman commented 5 years ago

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 https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/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 commented 5 years ago

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 :chicken: before they hatch. If all goes well, I'll comment here and ask for your help for renaming while avoiding conflicts across branches.

jgvictores commented 5 years ago

Slightly blocked, focusing on https://github.com/roboticslab-uc3m/teo-main/issues/38 right now.

PeterBowman commented 5 years ago

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

jgvictores commented 4 years ago

As spoken at https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134#issuecomment-505659623, KDL's motion API seems reasonable.

Furthermore, here are some notes to future selves:

  1. I would not completely forget ideas at figure https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/134#issuecomment-357325384
  2. I'd also like to keep in mind seeing how this connects to:

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 commented 4 years ago

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

PeterBowman commented 3 years ago

@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 commented 3 years ago

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 :+1:

PeterBowman commented 3 years ago

Tactical nuke dropped, there is no more TrajectoryLib at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/692888d12996d1fdd47169979c28839b55dbc16e. Last stable implementation was available here. The issue-164-joint branch will be deleted, here is the full patchset for reference: https://github.com/roboticslab-uc3m/kinematics-dynamics/compare/a2bd56ccb7b6cbaa98adc00c2f7298eb1de4c659...d2665723906c9f4c555714e696f8f4fe8c766e9b.

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):

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/a9acac8b8246c77be885f83648b12eddcac43d3c/libraries/TrajectoryLib/KdlTrajectory.cpp#L242-L280

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

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/a2bd56ccb7b6cbaa98adc00c2f7298eb1de4c659/programs/keyboardController/LinearTrajectoryThread.cpp#L91-L95