[ros2] Support for dynamic joint limits in MotionRequests + other features

[mechwiz]

I created this PR in anticipation of a few PRs I have in the works for MoveIt and MTC so that they can support:

1. Dynamic joint limit updates (specifically for vel/accel/jerk) in motion requests and also via service call. This is useful when creating trajectories for arms that require restricted limits when being loaded vs when unloaded.
2. Passing pre-planned full trajectories to the hybrid planner in order to take advantage of its local planner features without having to plan the same trajectory multiple times
3. Option to skip trajectory smoothing for a motion-request

@AndyZe, @JafarAbdi

[henningkayser]

I don't really follow why this should be needed, but why not add a reference trajectory here? https://github.com/ros-planning/moveit_msgs/blob/master/msg/MotionPlanRequest.msg#L25

[AndyZe]

So, the moveit_msgs/MotionSequenceRequest contains a JointTrajectory if you dig deep enough.

MotionSequenceRequest-> MotionSequenceItem[] -> MotionPlanRequest-> GenericTrajectory[]-> JointTrajectory[]

I guess this new field is probably not needed :(

What do you think @henningkayser? Is it too hard to dig all the way down to JointTrajectory in the current message type?

[mechwiz]

I'm cool with either approach. I guess using what currently exists (even if you do need to dig down somewhat deep) makes the most sense so I can make that change

[mechwiz]

I'm reconsidering this. I think having a dedicated "trajectory" field is more intuitive to the user to assign than having to dig deep in the motionplanrequest. This is for three reasons:

1. The way the HP will check to see if it should process the already planned trajectory (as opposed to planning one) is by specifically looking in this "trajectory" field to see if has a multi-point trajectory. This makes this field pretty important to this action. Having it buried down within the MotionSequenceRequest could therefore be less intuitive to the user.
2. Having this trajectory field outside the MotionSequenceRequest highlights that this action isn't so much a request to plan as it is to just bypass planning and execute via the local planner.
3. The action already conains a field for the planning_group so wouldn't be a big stretch to add the accompanying trajectory on the same level if applicable.

All this said, if you think using the reference-trajectory field is best, I can run with that and make the necessary changes.

[henningkayser]

This makes me thing that a separate post processing config would make sense by now, there are already so many related fields.

[AndyZe]

Hmm, say TOTG and Ruckig are configured to do smoothing by default in ompl_planning.yaml. I'm trying to think of how the motion planning pipeline would know to skip both of those smoothers but not the other OMPL planning request adapters.

Maybe it should be a vector of strings containing the plugins to skip, like this:

string[] ompl_adapters_to_skip

So to skip those two plugins, you would have:

["default_planner_request_adapters/AddTimeOptimalParameterization"
"default_planner_request_adapters/AddRuckigTrajectorySmoothing"]

[mechwiz]

I'm not sure we'd want to skip a time-parameterization adapter like TOTG. I think this is only relevant for smoothing (which occurs after the time-parameterization step) in which case I think ruckig is the only one currently. In any case though, if multiple smoothing adapters do exist down the road, then it's not too hard to use the "skip_smoothing" field when the adapter is called like I have done on my fork here for Ruckig https://github.com/mechwiz/moveit2/blob/mwiz/hp_fixes/moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp#L68

[mechwiz]

bumping this again.
I feel like defining a vector of strings for this is maybe a little overkill. I can think of usecases where even though a smoothing adapter (like Ruckig) is applied after TOTG in the list of ompl adapters, there may be exceptions/times where we don't want to smooth the resulting trajectory (whether to save on time or because its a very simple motion like closing/opening a gripper, etc..). Are there other usecases where we'd want to skip an adapter that is not smoothing?

[AndyZe]

Hmm, say TOTG and Ruckig are configured to do smoothing by default in ompl_planning.yaml. I'm trying to think of how the motion planning pipeline would know to skip both of those smoothers but not the other OMPL planning request adapters.

Maybe it should be a vector of strings containing the plugins to skip, like this:

string[] ompl_adapters_to_skip

So to skip those two plugins, you would have:

["default_planner_request_adapters/AddTimeOptimalParameterization"
"default_planner_request_adapters/AddRuckigTrajectorySmoothing"]

[AndyZe]

So, the moveit_msgs/MotionSequenceRequest contains a JointTrajectory if you dig deep enough.

MotionSequenceRequest-> MotionSequenceItem[] -> MotionPlanRequest-> GenericTrajectory[]-> JointTrajectory[]

I guess this new field is probably not needed :(

What do you think @henningkayser? Is it too hard to dig all the way down to JointTrajectory in the current message type?

[AndyZe]

Same comment about MotionSequenceRequest containing a JointTrajectory already, if you dig deep enough.