MoveitCommander support for TrajectoryConstraints

[bmagyar]

Description

TLDR:

• trajectory_constraints is a member of MotionPlanRequest yet the MoveGroupInterface doesn't allow for specifying it.
• trajectory_constraints can carry entire trajectories while path_constraints can't (in a clean way).

---

The MotionPlanRequest message has a field trajectory_constraints but class MoveGroupInterface (C++) doesn't allow for setting them. I've added support for this both to the C++ class and the Python wrapper.
What's the motivation for this? Passing in initial trajectories, initial guesses or seed trajectories to optimization-based planners such as the case of STOMP.

This was on #790 , now separated.

We should probably add some tests.

Checklist

• Required: Code is auto formatted using clang-format
• Extended the tutorials / documentation, if necessary reference
• Optional: Created tests, which fail without this PR reference
• Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic)

[v4hn]

I approve this request as is.

But I think we really have to document/define semantics of the trajectory_constraints field!
By message specification, it is just a vector of constraints that seem to play the same role as path_constraints.

Our OMPL interface rejects requests that have the field set.

If I read this benchmark code correctly, the vector should be interpreted as waypoints, where each Constraints message has to be fulfilled at some point during the trajectory in order. (excluding the goal state?)
This is not documented as far as I am aware and it would be nice to add this to this request...
In case this agrees with your interpretation: How do you pass in multiple alternative waypoint-lists/trajectories?

(@rhaschke we could provide a task constructor wrapper to fulfill such requests 😎)

[bmagyar]

Thanks for the excellent summary @v4hn ! Indeed, that is my interpretation of this field. Unfortunately even this doesn't really allow for a clean way of defining multiple trajectories.

The definition of moveit_msgs/Constraints is as follows:

string name
moveit_msgs/JointConstraint[] joint_constraints
moveit_msgs/PositionConstraint[] position_constraints
moveit_msgs/OrientationConstraint[] orientation_constraints
moveit_msgs/VisibilityConstraint[] visibility_constraints

and while both moveit_msgs/PositionConstraint and moveit_msgs/OrientationConstraint allows to fully determine the full-DoF constraint of a given link, moveit_msgs/JointConstraint only addresses a single joint. This means that while a vector of Position and Orientaton constraints could determine a trajectory, a vector of PositionConstraints only determine a single joint-space waypoint.

Of course allowing for multiple Position and OrientationConstraints points beyond controlling a single point, which is a nice and general abstraction, we should keep it that way.

Having a vector of these constraints allows to define a set of these constraints, hence a single trajectory.

I don't think this issue is critical enough to change message definitions but I'd like to mention that passing in trajectories could also be implemented by passing in a trajectory message but that implies adding a new field to MotionPlanRequest

[rhaschke]

Some minor remarks. I agree to @v4hn to better document the trajectory_constraints field, both in the message and in corresponding setter functions.

So far, the field is not used in any planner in MoveIt. As mentioned by @v4hn, OMPL even rejects planning requests having this field filled. Hence, I guess you are free to define the semantics.

I think, @v4hn and @bmagyar proposed two different semantics:

• @bmagyar wants to use this field as a trajectory seed for planning, while the waypoints of the finally optimized trajectory might violate the "constraints".
• @v4hn considered them as real constraints, i.e. the final solution should satisfy them with some waypoint in the given order.

While the latter interpretation actually asks for a sequential planner (as we implement it currently in https://github.com/ros-planning/moveit_task_constructor), the former one asks for a renaming of the field from trajectory_constraints into trajectory_seed / initial_trajectory. As pointed out in #793 (comment), a trajectory message would be more suitable for a seed.

I think this fundamental questions requires more discussion.

[rhaschke]

Which database do you refer to?

[bmagyar]

None actually. It's a ctrl+c&ctrl+v leftover

[rhaschke]

Why this relays to path constraints, while the function is about trajectory constraints?

[bmagyar]

It's a ctrl+c&ctrl+v leftover, fixed now

[rhaschke]

This variant doesn't seem to have an implementation. I think, it's superfluous.

[bmagyar]

Nice catch! Removed

[bmagyar]

So far, the field is not used in any planner in MoveIt.

It is. As mentioned above: STOMP

• @bmagyar wants to use this field as a trajectory seed for planning, while the waypoints of the finally optimized trajectory might violate the "constraints".
• @v4hn considered them as real constraints, i.e. the final solution should satisfy them with some waypoint in the given order.

Thank you @rhaschke for summing that up.
We have precedent for both use-cases at the moment. While the bit of code from the benchmark that suggests trajectory_constraints are meant to be waypoints while STOMP uses it for seed_trajectory.

Short term: I think this should be merged into kinetic-devel. This is a fix for an interface that wasn't fully transparent with the C++ one before. My motivation for doing it is not relevant.

Long term: We discovered an ambiguity in the interface, we should aim to correct it. This deserves its own issue where we discuss the solution and then implement it. We should touch on this at the maintainers meeting!
My proposed solution:

• deprecate trajectory_constraints from the message,
• add waypoints or waypoint_constraints,
• add seed_trajectories, initial_trajectories, trajectory_guesses -- we can agree on a name.

[v4hn]

I would agree that this can be merged without resolving the underlying problem.
We provided that interface before, it was just not wrapped.

Let's talk about the rest shortly next week, but feel free to open an issue already.

I'll leave it to @rhaschke to raise further discussion here or merge.