Add seed trajectories to MotionPlanRequest

[bmagyar]

Related to moveit/moveit#804

Everything is up for debate.

My points so far:

• I used the JointOrCartesianTrajectory to avoid confusion regarding the contents of the message. Ideally trajectory_msgs could host this message.
• I chose geometry_msgs/TransformStamped to describe the points of a Cartesian trajectory to allow for any base frame to be used to define any point. This allows for multiple seeds even for different end effector(s) although leaves some questions regarding the properness of this arbitrary point.
• Something like this could be added to a conversion library to ease moving from-to the different spaces using the IK solver present.
• I added the seed_trajectories field to MotionPlanRequest.

Once we get to a good common ground I could try setting up a demo.
FYI: @rhaschke @davetcoleman @jrgnicho

[davetcoleman]

Thanks for starting this conversation!!

[bmagyar]

Thanks @davetcoleman for your feedback, you've brought up great point.

I'll keep adding commits to this PR to maintain a visible progression for anyone who wants to join in.

Should we also add tolerances for JointTrajectory or only the Cartesian part?

[bmagyar]

Technically this is incorrect & inconsistent but didn't want to define Jerk.

[bmagyar]

This version follows the pattern of JointTrajectory and JointTrajectoryPoint but I also included tolerances in the point.

[davetcoleman]

Looks much better!

[bmagyar]

The thing I don't like about using Pose types for pose_tolerance is that it makes it hard to reason about a quaternion... A Twist would be more intuitive but then we are mixing apples and oranges...
What is your take on this?

[bmagyar]

Ping

[v4hn]

Sorry for the timeout and thanks for the mail ping on this @bmagyar .
I added my feedback.

Before we merge this, I believe there should also be a pull-request that actually makes use of it.
Otherwise this will just end up being dead code, just like the trajectory_constraints field.

[bmagyar]

I shall build an example once this PR is ready to be merged to avoid having to update that one with every change here. I think the best candidate to add it to will be CHOMP and a unit test for that.

[davetcoleman]

I like the idea of having an example implementation!

[davetcoleman]

@jschleicher as we discussed in person, I think you'd have valuable input into the needs for this message and cartesian planning with moveit

[jschleicher]

Probably this heads into another direction than the original scope of this PR: I'd like to use MotionPlanRequestMessage for cartesian paths as well and encode the type of planning in the plannerId (and maybe PlanningOptions). That would supersede computeCartesianPath and put the functionality into a PlannerManager (what we did in pilz_industrial_motion). For Sequences of (Cartesian or joint-space or mixed) waypoints we use an array of MotionPlanRequests, see pilz_industrial_motion package.

For circular arcs we need an additional helper point (circle center), that we "hacky" encode in path_constraints, maybe that field could be generalized as well (see python pilz_robot_programming )

To sum it up we want to

• plan to a (joint or cartesian) goal and have a cartesian linear path (nothing new)
• plan through multiple such points with different planner (sub-)types ("Sequence")
• blend in between the subparts of a sequence
• add constraints that the trajectory doesn't need to plan through (but keep a specified distance in the case of a circle)

[bmagyar]

Hi @jschleicher , I agree with you. The scope of this PR is to pass in an initial/seed trajectory to start an optimization-based planner from. The things you suggest however are somewhat related and will be partly enabled by this PR however I think we should follow it up with an extension to PlanningOptions which could determine the interpretation of the cartesian trajectory that was passed in such that:

• plan through points (new flag)
• determine how to blend between these point-to-point plans (new flag)
• add no-go areas (could be a new set of constraints that are enabled when following points, or when a new flag is enabled)

[davetcoleman]

@bmagyar we (@ommmid ) are working on TrajOpt and need a way to pass in a seed joint-space trajectory. This PR is perfect for that, do you think you could address the remaining feedback so we can merge this? A lot of it is simply documentation requests

[davetcoleman]

I've just targeted this to the master branch.

[bmagyar]

Thanks @davetcoleman , I'll move forward with addressing the comments but unfortunately due to impeding deadlines regarding my thesis I won't be able to supply an example. It sound like the TrajOpt effort would be perfectly fit for this purpose, right?

[ommmid]

@bmagyar yes, that is exactly what we need in trajopt.

[davetcoleman]

friendly ping?

[bmagyar]

ok, I am finally out of the grasp of PhD work, let's get this baby delivered!
I'll resolve what was done, whoever wishes to review, feel free to re-open if I closed something by mistake.

@mlautman @davetcoleman can we aim to merge this by Friday?

[BryceStevenWilley]

LGTM, seems like the only part left to resolve is how to state which trajectory should be used in a Generic trajectory (if you need to make a statement like that at all).

[mlautman]

+1 after adding the CartesianPoint.msg to CMakeLists.txt

[bmagyar]

All requested changes implemented, commits squashed 👍