feat!: split trajectory classes and implement Cartesian/JointState specializations

[bpapaspyros]

Description

• Split Trajectory class into TrajectoryBase, CartesianTrajectory, and JointTrajectory #216

Also resolves #217.

This PR solves the issue by splitting the existing Trajectory class to a base and 2 derived classes, namely, CartesianTrajectory and JointTrajectory.

A lot of the functionality of the base class is now marked protected and acts more as a utility for derived classes. The two new classes share similarities but require different handling when it comes to member variables and cleaning up.

This is missing python bindings for the new types. I believe it will be cleaner to add them separately.

Review guidelines

Estimated Time of Review: 20 minutes

Checklist before merging:

• Confirm that the relevant changelog(s) are up-to-date in case of any user-facing changes

[bpapaspyros]

One side effect of the new structure is that since we are storing a flat queue but expose StateType api, the [] operator no longer returns a reference.

That is, we can no longer do:

trajectory[n] = CartesianState(...)

Instead, I replaced the functionality by adding set_point and set_points.

[domire8]

Review of the headers

[bpapaspyros]

@domire8 A few things I want to go over in case you review before that

• I want to think a bit more about my constructors. They currently work and pass the tests I wrote, but we might be able to optimize more
• The exceptions need to be cleaned up a bit and perhaps refactored so that I don't always have to call functions that check for them multiple times
• I now have setters for the reference_frame, joint_names, and robot_name, but the current design makes it so that:

reset() -> clears points, doesn't touch other member vars

Let's take the CartesianTrajectory, that also means that for you to add points that now have a different reference frame you would have to go:

trajectory.set_reference_frame(some_pose)

// if new points different in size
trajectory.reset()
trajectory.add_points(points)

// if new points same in size
trajectory.set_points(points) // here we could set the points and then the reference frame 

For Joint trajectories, things are a bit more direct and the setters will directly change the joint names or robot name directly without checking something

• add_point
  • CartesianTrajectory: will fail if the first point to be added has a different or non-default reference frame from what was set in the constructor even if the queue is empty. Problem is, we can't use set_reference_frame if the queue is empty because that internally calls set_points. I can probably just bypass this and copy the reference frame but it seems awkward to have to provide a CartesianPose
  • JointTrajectory: will fail unless explicitly calling set_joint_names or set_robot_name even if those are empty due to the construction.

[domire8]

Regarding the constructors, I'd also like to see for consistency

• Empty constructor CartesianTrajectory()
• Constructor with name and optional reference frame CartesianTrajectory(name, reference_frame = "world")
• Constructor with name, point, duration CartesianTrajectory(name, point, duration) NOTE here no reference frame as we take it from the point
• Constructor with name, points, durations. NOTE here also no reference frame as we compare all points and accept if all the same.

I see that adding new points with different reference frame requires multiple function calls, but in that case the user should just create a new object honestly. I feel that it's quite clear if the reference frame is set on construction, and if you want to change it you need to call set_reference_frame with a pose and it will be applied to all points. If there are no points in the queue and one calls set_reference_frame we could even throw an exception because it doesn't make sense.

Also, I'm not sure that the robot_name makes sense here at all. We will want to add JointState objects that don't necessarily have the same name (config_1, config_2 etc.) so you won't be able to check for compatibility. I think we can only use joint names for compatiblity.

[bpapaspyros]

Regarding the constructors, I'd also like to see for consistency

• Empty constructor CartesianTrajectory()
• Constructor with name and optional reference frame CartesianTrajectory(name, reference_frame = "world")
• Constructor with name, point, duration CartesianTrajectory(name, point, duration) NOTE here no reference frame as we take it from the point
• Constructor with name, points, durations. NOTE here also no reference frame as we compare all points and accept if all the same.

I see that adding new points with different reference frame requires multiple function calls, but in that case the user should just create a new object honestly. I feel that it's quite clear if the reference frame is set on construction, and if you want to change it you need to call set_reference_frame with a pose and it will be applied to all points. If there are no points in the queue and one calls set_reference_frame we could even throw an exception because it doesn't make sense.

Also, I'm not sure that the robot_name makes sense here at all. We will want to add JointState objects that don't necessarily have the same name (config_1, config_2 etc.) so you won't be able to check for compatibility. I think we can only use joint names for compatiblity.

Noted, thanks for checking.

Regarding the robot name, my understanding was this should be common for all JointStates (although it wouldn't explicitly cause an issue). If we allow different ones, that also means we should be keeping them in their own vector to be able to reconstruct the JointStates. Sounds ok to you?

[domire8]

If we allow different ones, that also means we should be keeping them in their own vector to be able to reconstruct the JointStates. Sounds ok to you?

Yes I think so, same as for the Cartesian

[domire8]

Still a few places where I see room for improvement and reduction of code duplication. Happy to talk about it if you disagree with my points

[domire8]

👍

[domire8]

I think we are there now with the classes, they have become very compact with all the assertions and reused code. Nice work

[domire8]

Very nice tests as well, elegant. Let's get it in and continue from there! 🔥