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

bpapaspyros · 2025-01-15T13:19:46Z

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

.vscode/c_cpp_properties.json

bpapaspyros · 2025-01-15T13:29:59Z

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

.vscode/c_cpp_properties.json

source/state_representation/include/state_representation/StateType.hpp

source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp

source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp

This reverts commit 61455af.

source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp

domire8 · 2025-01-29T07:21:38Z

source/state_representation/src/trajectory/CartesianTrajectory.cpp

+  if (new_point.is_empty()) {
+    throw exceptions::EmptyStateException("Point is empty");
+  }
+  if (this->get_size() > 0) {


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

Cartesian

Emtpy constructor: no name, reference frame default to world and will stay there (consistent with the rest of the stack)

Constructor with name and reference frame

Constructor with name, point, duration (in that order): reference frame taken from point

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 · 2025-01-29T16:23:45Z

@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 · 2025-01-30T06:50:24Z

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 · 2025-01-30T07:59:15Z

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 · 2025-01-30T08:10:58Z

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

…ory-classes

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

source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp

domire8 · 2025-01-31T06:14:55Z

source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp

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


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 · 2025-01-31T06:17:19Z

source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp

+ * @class CartesianTrajectoryPoint
+ * @brief Struct to represent a Cartesian trajectory point
+ */
+struct CartesianTrajectoryPoint : public TrajectoryPoint {};


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

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 · 2025-01-31T06:42:41Z

source/state_representation/src/trajectory/CartesianTrajectory.cpp

+  if (new_point.is_empty()) {
+    throw exceptions::EmptyStateException("Point is empty");
+  } else if (new_point.get_reference_frame() != this->reference_frame_) {
+    throw exceptions::IncompatibleReferenceFramesException(
+        "Incompatible reference frames: " + new_point.get_reference_frame() + " and " + this->reference_frame_
+    );


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 }

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

source/state_representation/src/trajectory/CartesianTrajectory.cpp

domire8 · 2025-01-31T06:50:09Z

source/state_representation/src/trajectory/JointTrajectory.cpp

+void JointTrajectory::add_point(const JointState& new_point, const std::chrono::nanoseconds& duration) {
+  if (new_point.is_empty()) {
+    throw exceptions::EmptyStateException("Point is empty");
+  } else if (this->joint_names_ != new_point.get_names()) {


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

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

source/state_representation/src/trajectory/JointTrajectory.cpp

bpapaspyros and others added 9 commits January 14, 2025 16:03

feat: move up to C++20

61455af

refactor: format Trajectory.hpp

1c0c110

refactor: simplify Trajectory to a flatter base class

b957af4

feat: add Cartesian/JointTrajectory to StateType

cee8f97

feat: implement CartesianTrajectory

c76f70d

feat: implement JointTrajectory class

12782ac

fix: override cleanup functions and allow derived classes to clean

a6761ef

feat: add set_point(s) functionality

094c7e3

test: expand test case coverage

9c11b71

bpapaspyros self-assigned this Jan 15, 2025

refactor: rename directory from trajectories->trajectory

eeae5ab

bpapaspyros changed the title ~~Feat/restructure trajectory classes~~ feat: split trajectory classes and implement Cartesian/JointState specializations Jan 15, 2025

docs: update CHANGELOG.md

d2dcae7

bpapaspyros changed the base branch from main to feat/trajectory-msgs-dev January 15, 2025 13:26

bpapaspyros commented Jan 15, 2025

View reviewed changes

.vscode/c_cpp_properties.json Outdated Show resolved Hide resolved

bpapaspyros requested review from eeberhard and domire8 January 15, 2025 13:30

bpapaspyros marked this pull request as ready for review January 15, 2025 13:30

fix: make get_point(s) public in TrajectoryBase

6326d0d

bpapaspyros linked an issue Jan 15, 2025 that may be closed by this pull request

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

Open

bpapaspyros added 2 commits January 15, 2025 15:03

docs: update CHANGELOG.md

c41fd67

fix: python update bindings

216f1b9

bpapaspyros changed the title ~~feat: split trajectory classes and implement Cartesian/JointState specializations~~ feat!: split trajectory classes and implement Cartesian/JointState specializations Jan 15, 2025

domire8 reviewed Jan 16, 2025

View reviewed changes

bpapaspyros added 5 commits January 16, 2025 08:23

Revert "feat: move up to C++20"

0dbf0f1

This reverts commit 61455af.

refactor: tidy up headers (public/protected/private)

e4f3fb0

refactor: redifine number sequence of StateType enum

f1844f6

fix: make TrajectoryBase contructors protected

94ef3f3

docs: docstrings

5c45bec

bpapaspyros added 9 commits January 28, 2025 09:01

feat: implement add_points functions for derived classes

f0e75bc

test: fix tests on reset

1e8e12c

refactor: variable renaming

8bc0280

fix: improve get_times_from_start

f95ad5e

fix: tidy up TrajectoryBase [] overload and exceptions

3d7e7f2

fix: use add_point in CartesianTrajectory constructor

abc89bf

fix: condition and state update after delete_point

fe5c9a2

Merge branch 'main' into feat/restructure-trajectory-classes

c4e59c8

feat: delete_point by providing and index

a903f18

domire8 reviewed Jan 29, 2025

View reviewed changes

bpapaspyros added 4 commits January 29, 2025 12:46

fix: use vectors instead of queues in getters

f0ffbcc

fix: rearrange constructor arguments

8610d36

refactor: change reference frame/joint names/robot name setting logic

63cb968

fix: correct flag value when deleting results in empty queue

1f8b30e

bpapaspyros added 3 commits January 30, 2025 15:41

fix: further restructure constructors

4534679

Merge branch 'feat/trajectory-msgs-dev' into feat/restructure-traject…

e85cee4

…ory-classes

test: clean up and improve test structure and test coverage

739ba8a

bpapaspyros marked this pull request as ready for review January 30, 2025 18:29

bpapaspyros added 3 commits January 30, 2025 19:58

refactor: improve condition checks in vector operations

914a826

refactor: re-arrange includes

0ff1210

fix: better checks for empty vector arguments

93419ca

domire8 reviewed Jan 31, 2025

View reviewed changes

bpapaspyros added 4 commits January 31, 2025 10:49

refactor: improve/optimize code structure and readability

2ed6228

fix: more constructor touch ups and docstring updates

55afa45

feat: add getters for individual duration & time from start

822401b

feat: add function that returns the total trajectory duration

bd0c01c

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

bpapaspyros commented Jan 15, 2025 •

edited

Loading

bpapaspyros commented Jan 15, 2025

domire8 left a comment

domire8 Jan 29, 2025

bpapaspyros commented Jan 29, 2025

domire8 commented Jan 30, 2025

bpapaspyros commented Jan 30, 2025

domire8 commented Jan 30, 2025

domire8 left a comment

domire8 Jan 31, 2025

domire8 Jan 31, 2025

bpapaspyros Jan 31, 2025

domire8 Jan 31, 2025

bpapaspyros Jan 31, 2025 •

edited

Loading

domire8 Jan 31, 2025

bpapaspyros Jan 31, 2025

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

Are you sure you want to change the base?

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

Conversation

bpapaspyros commented Jan 15, 2025 • edited Loading

Description

Review guidelines

Checklist before merging:

bpapaspyros commented Jan 15, 2025

domire8 left a comment

Choose a reason for hiding this comment

domire8 Jan 29, 2025

Choose a reason for hiding this comment

Cartesian

Joint

bpapaspyros commented Jan 29, 2025

domire8 commented Jan 30, 2025

bpapaspyros commented Jan 30, 2025

domire8 commented Jan 30, 2025

domire8 left a comment

Choose a reason for hiding this comment

domire8 Jan 31, 2025

Choose a reason for hiding this comment

domire8 Jan 31, 2025

Choose a reason for hiding this comment

bpapaspyros Jan 31, 2025

Choose a reason for hiding this comment

domire8 Jan 31, 2025

Choose a reason for hiding this comment

bpapaspyros Jan 31, 2025 • edited Loading

Choose a reason for hiding this comment

domire8 Jan 31, 2025

Choose a reason for hiding this comment

bpapaspyros Jan 31, 2025

Choose a reason for hiding this comment

bpapaspyros commented Jan 15, 2025 •

edited

Loading

bpapaspyros Jan 31, 2025 •

edited

Loading