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

[domire8]

Ok so I think I would like to go like that:

Cartesian

1. Emtpy constructor: no name, reference frame default to world and will stay there (consistent with the rest of the stack)
2. Constructor with name and reference frame
3. Constructor with name, point, duration (in that order): reference frame taken from point
4. Constructor with name, vector of points, vector of durations (in that order): verify all reference frames are the same, set reference frame from that

• get_point, add_point, add_points, get_points, insert_points do the obvious checks
• reset is here to reset a state to empty and have no data, it doesn't mean we reset the reference frames or joint frames. hence, for me, reset should just do that, not mess with reference frame
• set_points is a bit more tricky, I would say that the size of vectors passed as arguments need to correspond to the existing size of the trajectory. If one wants to change all points and size on a trajectory object, one can just call reset and then add_points.
• If we really want to go all the way, we could introduce a set_reference_frame(CartesianPose pose) that multiplies all points with the pose in order to actually perform a change of reference frame and not just a renaming.

Joint

all the same, except that we can probably allow a set_joint_names(std::vector<std::string>) directly.

What do you think?

[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]

Suggested change

	if (index > this->points_.size()) {
	throw std::out_of_range("Index out of range");
	}
	this->assert_index_in_range(index);

What we have done in other places is to wrap that check that we have several times in a helper.

control-libraries/source/state_representation/include/state_representation/IOState.hpp

Line 142 in f72569e

static void assert_index_in_range(unsigned int io_index, unsigned int size);

[domire8]

Is this necessary or just nice? I'm just wondering. And while we are here, you might as well define a constructor from a duration and CartesianState, that will help reduce code in the source file

[bpapaspyros]

Yes, I think this is just nicer to have a dedicates struct for this class which could also allow us to more easily specify fields if ever needed.

Also, my original understanding (because of the attribute name) was that JointStates have a robot name, not necessarily a state name, that is common to all. In that case, we would only need to store a name for Cartesian states. But still, I find it a bit nicer to keep it there for future use

[domire8]

These lines are also duplicated, which speaks for a static helper up top that does the same

assert_valid(const CartesianState& point, const std::string& reference_frame) {
  throw if point empty
  throw if reference frame not the same
}

[bpapaspyros]

I introduced quite a few assertion functions. We might see that some of them can be further merged, but for starters everything should be a bit more readable now

[domire8]

Does that operator work to compare all strings from a vector?

[bpapaspyros]

yes, side note is that in c++20 these operators are automatically generated because of the introduction of the spaceship <=> (https://en.cppreference.com/w/cpp/container/vector). TL;DR we are both backwards and forwards compatible and this works