Multi dof action extension

[DLu]

Alternate approach for #16

Adds trajectory_msgs/MultiDOFJointTrajectory to FollowJointTrajectory.action goal, as well as some related fields.

Since this changes a core message, we should probably create a galactic-devel fork and merge there instead. However, adding the field shouldn't break too much since robots without a MultiDOF joint can just ignore the component.

[bmagyar]

This is a fairly major change and I'd like to see some current motivation for this.

Is it to add some cartesian constraints? Planning for a mobile manipulator including the base?

[DLu]

I am working on a project to bring mobile manipulation to MoveIt 2. This means supporting the planar joint in a number of different places. I've made changes already to srdfdom and MoveIt2's joint definitions.

The current definition of FollowJointTrajectory only encompasses a subset of joints, excluding planar and floating. One approach to including planar joints would be to create new actions that include MultDOF trajectory, which is the approach that @hvpandya took back in 2015 with #16. However, as with the discussion of that PR, it will be cleaner to just add the new parallel field in a new ROS release.

The standard for MoveIt is to have two parallel data structures, one for standard joints and one for multiDOF joints, i.e. RobotTrajectory.msg. In most cases, if a robot does not have any multiDOF joints, then the implementation will not need to change. However, for the robots that do have multiDOF joints, then this will add a welcome interface for which there is already a great deal of infrastructure.

[mamoll]

This looks good to me, but I am not a ROS Control expert. This is needed to do whole body motion planning for mobile manipulators using MoveIt.

@AndyZe do you have any thoughts on this?

[AndyZe]

Here's what @DLu is up to -- https://twitter.com/PickNikRobotics/status/1385667196999725056

[AndyZe]

First thought is that the name is misleading. MultiDOF sounds like a normal 7-DOF arm with revolute joints. The name is also confusing because MoveIt supports a single joint having more than one degree of freedom, like a shoulder joint (but it's rarely used).

Maybe call it MixedDOFJointTrajectory? Or MixedTypeJointTrajectory?

[DLu]

First thought is that the name is misleading. MultiDOF sounds like a normal 7-DOF arm with revolute joints. The name is also confusing because MoveIt supports a single joint having more than one degree of freedom, like a shoulder joint (but it's rarely used).

Maybe call it MixedDOFJointTrajectory? Or MixedTypeJointTrajectory?

This PR adds support for single joints that have more than one degree of freedom, i.e. planar/floating. It adds a Trajectory for MultiDOFJoints, not a JointTrajectory with MultiDOF, if that makes sense.

Either way, the naming reflects the existing names from trajectory_msgs.

[DLu]

Also, I've slightly modified the MultiDOFJointTolerance message to specify how to actually calculate the differences, based on the component field.

[AndyZe]

I can't think of a better way to do this. Approve assuming you make the minor changes.

[DLu]

What do you think @bmagyar ?

[gavanderhoorn]

Is the rationale for extending the existing message the fact a multi-dof joint is still a "joint", as in contrast to Cartesian (fi) waypoints, trajectories (& associated goals) which do not conceptually really fit in at the joint level any more?

I'm asking because it doesn't necessarily seem proper to me to make FollowJointTrajectory.action a union capable of supporting all these different use-cases. Or perhaps in other words: does this semantically make sense? Are there any other future extensions expected? Should there be a separate .action for this, similar to how FZI created one for Cartesian use-cases?

[bmagyar]

It definitely extends the scope of this message and opens a lot of possibilities for new use-cases.

The whole-body motion planning aspect is what was really convincing to me.

I have one ask though:
multi_dof_path_tolerance et al is a hazy name IMO. Can we come up with anything better than this to better describe it? We can't change the message type name but the local member name should be improved.

• floating_joint_path_tolerance?
• transform_path_tolerance?

[DLu]

Is the rationale for extending the existing message the fact a multi-dof joint is still a "joint", as in contrast to Cartesian (fi) waypoints, trajectories (& associated goals) which do not conceptually really fit in at the joint level any more?

The concept of "joint" is rather wide, but yes, the rationale is that planar and floating are the two multi-dof joints defined in the URDF spec, and are thus still "joints". Furthermore, MoveIt treats them as joints and can plan for them.

I'm asking because it doesn't necessarily seem proper to me to make FollowJointTrajectory.action a union capable of supporting all these different use-cases.

Here's my thinking: Let's say we have two robots, R1 and R2, where R1 has MultiDOF joints and R2 does not. Both of them offer the FollowJointTrajectory action.

Right now, R1 has no way to follow a trajectory that includes its MultiDOF joints. Extending the action definition like in this PR enables that capability. R2 does not need to change their implementation at all, because they can safely ignore the multi_dof field. Both robots can still use all nodes that employ the FollowJointTrajectory action.

In the other case where we create a new action that includes the union (both MultiDOF and Regular Trajectory) (call it FollowAllJointTrajectory.action), R1 needs to implement an additional action server, which looks a lot like the existing FollowJointTrajectory action server. But then all the other nodes that use FollowJointTrajectory may also need to be expanded to call either FollowJointTrajectory or FollowAllJointTrajectory in order to work with all the joints of R1. Furthermore, if a tool comes along that only uses FollowAllJointTrajectory then R2 will have to write an additional action server as well in order to use that functionality.

Or perhaps in other words: does this semantically make sense? Are there any other future extensions expected?

trajectory_msgs has been functionally unchanged since 2013 and its description states that they function as the building blocks for most of control_msgs. As indicated by #16, this sort of functionality has been desired since at least 2015.

Should there be a separate .action for this, similar to how FZI created one for Cartesian use-cases?

I'm not sure what you're referring to here with FZI.

[AndyZe]

The FZI reference is to this, I think:

https://discourse.ros.org/t/developing-a-cartesian-manipulator-interface/13872

The takeaway is, they made a new action server for Cartesian motions instead of modifying the existing one.

[DLu]

multi_dof_path_tolerance et al is a hazy name IMO. Can we come up with anything better than this to better describe it? We can't change the message type name but the local member name should be improved.

I made the message type name, so I went ahead and renamed it. Since the only difference between it and JointTolerance.msg is the component field, I renamed it JointComponentTolerance.msg

[DLu]

Any other questions I can help with @bmagyar

[bmagyar]

@AndyZe thanks for that reference.

The way I see it is that this would allow us to extend the FollowJointTrajectory action with non-revolute/prismatic/linear joints. The goal is not to make a shapeshifter follow trajectory action but to extend the current setup to support whole body trajectories where even in the case of bipeds where a pure joint trajectory could exist (however unrealistic), but the lower body is abstracted as a floating joint for practical reasons, similarly to mobile bases. This would play nicely with controller chaining once we have it in ros2_control.

[DLu]

Thanks!

[DLu]

(and thanks for the Galactic release)