Initial path types

[Victorlouisdg]

Describe your changes

This PR introduces types that are needed for motion planning.

As an initial suggestion I've decided to treat dual arm configurations as an array np.hstack((joints_left, joints_right)). The alternative is treating them as the tuple (joints_left, joints_right). Both options have pros and cons we should consider.

Additionally, for the paths I've provided both list and array types. We should consider whether both are actually necessary.

Considerations

• DualJointConfigurationType being the hstack or two JointConfigurationType is similar to how Drake combines all state of a plant. E.g. calling sceneGraphCollisionChecker.CheckConfigCollisionFree expects a (12, ) numpy array for my dual UR5e scenes.
• Stacking might require code like this: joints_left, joints_right = joints_dual[:6], joints_dual[6:] or np.split(joints_dual, 2). I'm not sure if this or better or worse than unpacking a tuple joints_left, joints_right = joints_dual. The tuple approach could support two arms with different #DoFs.
• The list/array types are somewhat redundant, but some path operations require arrays, such as np.diff(path_array, axis=0).
• How we will handle additional DoFs, such as trajectories for gripper position?

Checklist

• Have you modified the changelog?

[adverley]

I strongly like the name path you use if there is no attached temporal dimension.

Going from paths to trajectories; trajs are indexed by time, this could be a function or dict. You might also support this instead of splitting this up into TimePathType and TimePathArrayType (or at least that is how I interpret these types).

Additional DoFs for grippers -> I also thought about initially supporting this but our Robotiq grippers don't like high frequency commands and I don't see an immediate use-case on our Schunk gripper so I think we can ignore that for now.

I like the tuple approach more than the stacked numpy array approach. An alternative might be a list of joint names and a list of joint values.

[Victorlouisdg]

The tuple approach is what I've currently implemented in my planner interfaces (see except below). The "downside" of this approach is that to extract that path for only the left or right arm, you need things like this:

path_left = [joint_left for joint_left, _ in path]
path_right = [joint_right for _, joint_right in path]
path_left_resampled = resample_path(path_left, n_servos)
path_right_resampled = resample_path(path_right, n_servos)
path_resampled = list(zip(path_left_resampled, path_right_resampled))

class SingleArmMotionPlanner(abc.ABC):
    """Base class that defines an interface for single-arm motion planners. """

    @abc.abstractmethod
    def plan_to_joint_configuration(
        self,
        start_configuration: JointConfigurationType,
        goal_configuration: JointConfigurationType,
    ) -> Union[List[JointConfigurationType], None]:
        raise NotImplementedError

class DualArmMotionPlanner(abc.ABC):
    """Base class that defines an interface for dual-arm motion planners.  """

    @abc.abstractmethod
    def plan_to_joint_configuration(
        self,
        left_start_configuration: JointConfigurationType,
        right_start_configuration: JointConfigurationType,
        left_goal_configuration: Union[JointConfigurationType, None],
        right_goal_configuration: Union[JointConfigurationType, None],
    ) -> Union[List[Tuple[JointConfigurationType, JointConfigurationType]], None]:
        raise NotImplementedError

[tlpss]

I agree that the distinction between path and trajectory is important and should be very clear, I'm not convinced of the need for a List and Array variant of all the variables, that seems confusing to me and also harder to maintain?

Wrt dual-arms, I would not provide separate types because this feels hard to scale to me (what about grippers etc?). Either we rely on the user knowing the degrees of freedom of its system and interpreting the configurations correctly, or we use some kind of naming?

Regarding Path -> Trajectory, I can live with a (named) Tuple for this, though I could also live with a dict from 'pose/config' to timestamp.

We should maybe also anticipate the need for specifying velocities/accelerations/torques in trajectories.

In general it might not be a bad idea to take a look at the relevant ROS messages? 🙂 e.g. https://docs.ros2.org/foxy/api/trajectory_msgs/index-msg.html

[tlpss]

Do we need separate Types for dual arm paths? IMO it is user responsability to know the DOFS of the system he/she is controlling. I think this would scale badly to motion planning with grippers (that can have various DOFs?).

Or is this additional code complexity worth the increased usability?

[Victorlouisdg]

For reference, ROS does it like this:

• JointTrajectoryPoint: flat array for positions/velocities/... with all DoFs stacked together and a time value.
• JointTrajectory: ~list of JointTrajectoryPoint and list of names for the DoFs

I don't really understand their docs for MultiDOFJointTrajectoryPoint, to me it seems like that's a trajectory of SE(3) poses.

ROS also doesn't seem to have a distinction between a Path and Trajectory. We could also consider this, and by default map a path without time to times uniformly between 0.0 and 1.0. Then you could easily retime your path by multiplying by the desired total duration.

[Victorlouisdg]

I've pushed a much-simplified proposal. I've chosen to use 2D arrays for paths for the simple reason that you can have both row-wise access (e.g. to execute a servoJ()) and column-wise access (e.g. to plot with matplotlib).

I've also removed the dual arm types as this would indeed scale badly to gripper DoFs, mobile base DoFs, etc. However, we have to consider what the consequences are for the DualArmMotionPlanner interface. Currently I've used this pattern for the plan_() functions:

(start_left, start_right, goal_left | None, goal_right | None) -> list[(config_left, config_right) | None

Using flat arrays as arguments here would mean we can't use None anymore to signal that no planning should be done for that arm. However, we could also add boolean arguments for the same functionality, something like:

(start_joints, goal_joints, plan_for_left=True, plan_for_right=True) -> 2D-array

The DualArmMotionPlanner would however need to know how to unstack the joints, e.g. to plan for a single arm or to call the inverse kinematics of the left/right arm.

[Victorlouisdg]

I've pushed a new iteration of the types with JointTrajectory and DualArmJointTrajectory dataclasses. There's two things to consider here:

• How will we handle additional DoFs, e.g. grippers, mobile bases?
• positions is a required attribute of JointTrajectory, velocities, accelerations and efforts are optional. Does this cover all use cases? Might someone ever want a JointTrajectory without positions and only velocities, and do we have to support that in this dataclass?

@tlpss @adverley @m-decoster

[m-decoster]

Some proposed changes in commit 3ca5049:

• Separate paths and trajectories
• This reduces code duplication in the DualArmTrajectory and should scale better to other set-ups
• It also allows for easier addition of grippers as illustrated
• Make positions an optional attribute (user responsibility to make sure at least one JointPathType is valid)

Do we also want to support derivatives of the PoseTrajectory (and how)? @tlpss

[Victorlouisdg]

I really like the proposal in Mathieu's last commit, and think we are getting close to a final design. It also matches well with the SingleArmMotionPlanner and DualArmMotionPlanner interfaces I'm working with right now. Their plan function signatures would have to be changed slightly to something like this:

SingleArmMotionPlanner

def plan_to_joint_configuration(self, 
    start_joints: JointConfigurationType, 
    goal_joints: JointConfigurationType)
    -> SingleArmTrajectoryType | None:

DualArmMotionPlanner

def plan_to_joint_configuration(self, 
    start_joints_left: JointConfigurationType,
    start_joints_right: JointConfigurationType,
    goal_joints_left: Optional[JointConfigurationType],
    goal_joints_left: Optional[JointConfigurationType]
) -> DualArmTrajectory | None:

Many planners however don't consider time, so this should come this the warning/caveat that these trajectories might not always be safe to directly execute.

[tlpss]

I also like the proposal, though imo the time parameterization is the combination of timestamps, velocities,... So I would not split those.

So either we move them to the trajectory classes, in which case the path container is is simply a data type alias:

JointPathType: np.ndarray

@dataclass
class SingleArmTrajectory:
    path: JointPathType
    gripper_path: Optional[JointPathType] = None

    times: TimesType  # time (seconds) from start of trajectory
    **velocities: Optional[JointPathType] = None
    accelerations: Optional[JointPathType] = None
    efforts: Optional[JointPathType] = None**

Or we create a 'container/base class/ whatever' to group all the time information for abstract trajectories and create subclasses for the types we want to consider:

@dataclass 
TrajectoryContainer/TrajectoryBase/...
times:
velocities:
acc:
efforts

@dataclass 
SingleArmTrajectory(TrajectoryContainer):
path: 
gripper_path:

Or we can do the above with composition ofc.

@dataclass 
TrajectoryTimeParameterization
times:
velocities:
acc:
efforts

@dataclass SingleArmTrajectory:
path:
gripper_path:
time_parameterization: TrajectoryTimeParameterization

[adverley]

In discussion with @Victorlouisdg; we prefer composition above inheritance and we embed the time parametrization explicitly instead of having a separate type. We propose the following (which is a bit the combination of your first and third suggestion Thomas).

JointPathType: np.ndarray

@dataclass 
class PoseTrajectory:
	poses
	velocities
	angular_velocities
	... 

@dataclass 
class JointTrajectory:
	positions: JointPathType
	times: TimesType
	velocities: Optional[JointPathType]
	acc: Optional[JointPathType]
	efforts: Optional[JointPathType]

@dataclass 
class SingleArmTrajectory:
	arm_trajectory: JointTrajectory
	gripper_trajectory: Optional[JointTrajectory]

@dataclass 
class DualArmTrajectory:
	left_arm_trajectory: SingleArmTrajectory
	right_arm_trajectory: SingleArmTrajectory

@dataclass
class PoseTrajectory:
	poses: PoseTrajectory

Usage of DualArmTrajectory is quite convoluted with these names:

dual_arm_trajectory.left_arm_trajectory.arm_trajectory.positions

VL does not like these big names.

[tlpss]

@adverley one disadvantage now is that the time parameterization can be different for the left and right path, which is something we don't want to support I think?

IMO a trajectory can have only 1 time parameterization. We split the path DOFs for readability, not because we want to interpret them as separate trajectories I think?

And I tend to agree that accessing the data is convoluted.

[Victorlouisdg]

We also want to support force trajectories e.g. for a force controllable gripper. I see only two options:

• Andreas' comment: times are grouped with positions & it's derivatives (velocities etc.), this means left/right arm/gripper can all have their own timestamps and duration
• Mathieu's commit: times are a top-tevel attribute of SingleArmTrajectory & DualArmTrajectoryand the derivatives for each model/DoFs are grouped in a JointPathContainer or similar. This means that the user must ensure that the length of all paths matches the len(times).

[m-decoster]

one disadvantage now is that the time parameterization can be different for the left and right path, which is something we don't want to support I think?

@tlpss If we want to group times, velocities, etc., how do you propose to make sure that the time parameterization is the same for the left and right path? Because the velocities can be different for left and right. I also don't see another option than the two @Victorlouisdg mentions.

VL does not like these big names.

@adverley @Victorlouisdg Not sure if this matches your preferences for naming conventions, but we could do something like

@dataclass 
class JointTrajectory:
	positions: JointPathType
	times: TimesType
	velocities: Optional[JointPathType]
	acc: Optional[JointPathType]
	efforts: Optional[JointPathType]
	
@dataclass 
class SingleArmTrajectory:
	arm: JointTrajectory
	gripper: Optional[JointTrajectory]

@dataclass 
class DualArmTrajectory:
	left: SingleArmTrajectory
	right: SingleArmTrajectory

because it is implied that the left member of a dual arm trajectory refers to the left arm, which makes accessing members a bit cleaner:

dual_arm_trajectory.left.arm.positions

It's terse, but implicit.

But: this brings me to another issue: how do you define which arm is left and which is right? ;) Would a 2-tuple be an option? Or do we just leave it to the programmer to define left and right in their setup?

[tlpss]

@m-decoster Ah yes I now realize that I made a weird/wrong assumption. In this case I have to say I like your original proposal most:

class JointPathContainer:
    positions: Optional[JointPathType] = None
    velocities: Optional[JointPathType] = None
    accelerations: Optional[JointPathType] = None
    efforts: Optional[JointPathType] = None

class SingleArmTrajectory:
    path: JointPathContainer
    gripper_path: Optional[JointPathContainer]
    times: TimesType  # time (seconds) from start of trajectory


@dataclass
class DualArmTrajectory:
    path_left: JointPathContainer
    path_right: JointPathContainer
    gripper_left: Optional[JointPathContainer]
    gripper_right: Optional[JointPathContainer]
    times: TimesType  # time (seconds) from start of trajectory 

Regarding the Left/right, yeah I think we want to distinguish them somehow. Left/right seems easier to work with than first/second for example, though I can imagine there are cases where you don't have a 'left' and 'right' arm. I don't have a strong opinion on that, and would suggest we just go for something and start integrating using the types. We can make changes along the way if needed?

[m-decoster]

LGTM! Maybe with gripper_path_left in the DualArmTrajectory to be consistent with gripper_path in SingleArmTrajectory (my bad in my commit) - but just gripper_left works too.

[Victorlouisdg]

I think the current proposal is really great. There's just one detail, do we make times a required attribute without default value or an optional attribute with None as default value? (Currently it's different between PoseTrajectory and the JointTrajectory classes.)

I prefer making it required in all Trajectory classes. This way consumers don't need to worry about checking whether each trajectories has times or not. A sensible default for times is np.linspace(0, 1, len(path)). We could set this in the dataclass __post_init__() method if None was passed to the constructor.

[tlpss]

Would there be a use case for having such a default time parameterization? IF not, I think it would be semantically more consistent to have 'actual' timings for trajectories.

[Victorlouisdg]

I've made times required for all Trajectory dataclasses and have given it no default value. I believe this was the last thing to decide, so this PR is now ready to merge.