Scaled jtc #1191

fmauch · 2024-07-03T18:22:51Z

This is my take2 on #301.

I plan to finish this until Friday, @firesurfer feel free to poke me :-)

Currently missing

tests for scaled execution
There are some ToDos in the documentation:
- Algorithmics
- Handling tolerances

Things that will get changed, as discussed in the latest working group meeting

Allowing scaling factors above 1 ~~might get postponed to a later point.~~
Report the scaling factor in the controller state
Use a custom datatype for the scaling_factor subscriber (std_msgs are deprecated)
Change subscription to transient_local. This will require documentation, as a standard ros2 topic pub will not work with this.

This adds a scaling factor between 0 and 1 to the JTC so that the trajectory time inside the controller is extended respectively. A value of 0.5 means that trajectory execution will take twice as long as the trajectory states. The scaling factor itself is read from the hardware for now.

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

This avoids writing the explicit conversion by hand Internally that basically does: static_cast<rcl_duration_value_t>(static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld)

…etting of scaling factor Co-authored-by: Manuel M <mamueluth@gmail.com>

That was changed in a previous commit

used The current implementation only works correctly when a position interface is used. When the hardware in fact slows down the robot (as UR does), also velocity interfaces should be scaled correctly, but that't rather an edge case right now.

It is accessed from the RT thread only, anyway.

See UniversalRobots/Universal_Robots_ROS2_Driver#882

fmauch · 2024-07-03T19:05:12Z

Thoughts on tests I should implement:

Execution with scaling factor of 0.5 takes twice as long (and does not fail, when no tolerances are set)
Execution with scaling < 1 and goal time tolerance leads to an early halt and failed execution
Execution with scaling < 1 and path tolerances set should not fail (if goal_time_tolerance is large enough)
Execution with scaling > 1 should not fail independent of the tolerances set <-- is that true?

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

joint_trajectory_controller/src/joint_trajectory_controller.cpp

firesurfer · 2024-07-04T05:46:13Z

hi @fmauch Great to see this being worked on again. I added some comments to the code.

Regarding the tests:

Execution with scaling factor of 0.5 takes twice as long (and does not fail, when no tolerances are set)

Makes sense

Execution with scaling < 1 and goal time tolerance leads to an early halt and failed execution

Isn't this what we patched in: UniversalRobots/Universal_Robots_ROS2_Driver#882. As far as I can tell you also backported this patch. (And I still think the goal time should be also scaled ;) )

Execution with scaling > 1 should not fail independent of the tolerances set <-- is that true?

Same as above. From heavily using this controller in our system I have the opinion that in most use cases it makes sense (and is what a user would expect) that the goal time should also be scaled. But perhaps I understood your suggestion wrong.

fmauch · 2024-07-04T06:59:55Z

From heavily using this controller in our system I have the opinion that in most use cases it makes sense (and is what a user would expect) that the goal time should also be scaled. But perhaps I understood your suggestion wrong.

Thank you for your input. That's why I put my brain dump on that here, so we can discuss this :-) And that's also why I didn't write the documentation on that, yet.

I'll wrap my head around this a little more and try to get a better feeling for the current behavior, then come back to this.

fmauch · 2024-07-04T07:00:24Z

ros2_controllers-not-released.jazzy.repos

+
+  control_msgs:
+    type: git
+    url: https://github.com/fmauch/control_msgs.git
+    version: scaled_jtc


Suggested change

control_msgs:

type: git

url: https://github.com/fmauch/control_msgs.git

version: scaled_jtc

fmauch · 2024-07-04T07:00:36Z

ros2_controllers-not-released.rolling.repos

+
+  control_msgs:
+    type: git
+    url: https://github.com/fmauch/control_msgs.git
+    version: scaled_jtc


Suggested change

control_msgs:

type: git

url: https://github.com/fmauch/control_msgs.git

version: scaled_jtc

firesurfer · 2024-07-05T06:23:38Z

@fmauch

Execution with scaling factor of 0.5 takes twice as long.

Yes. we sample the trajectory half as fast

Execution with scaling factor of 2 should take 50% of the time.

Yes we sample the trajectory twice as fast

Execution with scaling factor of 0 should take as long as expected

I would call this paused. I have the feeling this is one of the major usecases of this controller

Independent of goal and path tolerances the action should not fail -- the goal time is scaled, as well.

In case the trajectory is within the (scaled) goal time, it should not fail -> yes. Otherwise the combination of a scaled trajectory + a set goal time tolerance would always fail.
Is there a specific use case you have in mind that would not be covered by that ?

fmauch · 2024-07-05T07:21:42Z

My concern is mainly this:

If you have an application and you execute a trajectory on a robot that does speed scaling. The trajectory gets slowed down due to safety reasons, which means it finishes later than originally requested. This will slow down the whole application and no longer behave as programmed. This might be considered a fault.

One could argue that

Depending follow-up actions should be triggered once their post-conditions are met (the motion action finished) and nit by time.
If execution time is critical, that can be monitored as a wrapper. MoveIt! for example does that by default.

I just quickly discussed that with a colleague and he also agreed that the behavior that we currently have is probably the expected one. He was also emphasizing the second point.

So, I guess, I'll just leave that result standing. As I said, I can definitively live with it. And it's probably even better / more appropriate than "I can live with that".

fmauch · 2024-07-05T12:12:38Z

Update on the tests:

testing is currently difficult in this repository, see Test failures: subscription already associated with a wait set #1101
There is also a crash in one of the tests, I will open a separate issue with that

Running the current tests produces an error in almost all tests:

unknown file: Failure
C++ exception with description "can't subtract times with different time sources [3 != 1]" thrown in the test body.

There is currently an issue with the goal time tolerance. The action goal's tolerance always overwrites the one from he parameter. I'll add a PR for that.

christophfroehlich · 2024-07-05T12:24:56Z

* testing is currently difficult in this repository, see [Test failures: subscription already associated with a wait set #1101](https://github.com/ros-controls/ros2_controllers/issues/1101)

In the last weeks, I compiled ros2_control+controllers on jammy+humble to run the tests.

This isn't the best explanation, but it's a start.

joint_trajectory_controller/src/joint_trajectory_controller.cpp

firesurfer · 2024-07-08T06:29:36Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+    return false;
+  }
+
+  RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", scaling_factor);


I think it should be possible to disable this logging message. Some one could have the idea of continuously streaming the scaling factor and this would lead to spamming the log/terminal

We could set that output to an equality check with the current value and add a throttle to it. I think that might be a better approach than adding yet another config value.

And/Or use a different child logger that can be separately silenced

joint_trajectory_controller/src/joint_trajectory_controller.cpp

firesurfer · 2024-07-10T09:40:46Z

Update on the tests:

* testing is currently difficult in this repository, see [Test failures: subscription already associated with a wait set #1101](https://github.com/ros-controls/ros2_controllers/issues/1101)

* There is also a crash in one of the tests, I will open a separate issue with that

* Running the current tests produces an error in almost all tests:
  ```
  unknown file: Failure
  C++ exception with description "can't subtract times with different time sources [3 != 1]" thrown in the test body.
  ```

So what about the remaining issues especially regarding the tests.
From what I can tell so far:

Does not build on rolling because there the updated control_msgs are not used. Probably it would make sense to merge the control_msgs change first. Given that the scaled JTC should/will definitely go upstream there should be no reason not to merge as soon as possible.
Then in case of Jazzy where the build apparently succeeds there are multiple unrelated tests failing

Regarding the tests: @christophfroehlich I am not sure if your suggestion works. I had to explicitly make 1-2 small fixes to some ros2control packages in order to compile ros2control on iron. I would guess that's the same for humble. Is there currently any work in progress in order to get the test working on rolling again?

@fmauch I guess given that you can't run any of the tests at the moment you didn't start to implement any of the new tests we discussed before?

* There is currently an issue with the goal time tolerance. The action goal's tolerance always overwrites the one from he parameter. I'll add a PR for that.

Is there already an issue for that ? Do we need to discuss it in an extra issue?

christophfroehlich · 2024-07-10T16:23:22Z

@firesurfer we have a CI job for that, it currently builds but some tests fail/are flaky
https://github.com/ros-controls/ros2_control_ci/actions/runs/9866959705

fmauch · 2024-07-16T07:18:53Z

@fmauch I guess given that you can't run any of the tests at the moment you didn't start to implement any of the new tests we discussed before?

Precisely. #1206 could be the PR removing this barrier.

Is there already an issue for that ? Do we need to discuss it in an extra issue?

Sorry for not answering earlier: See #1192

So, I'll probably start implementing tests tomorrow, either locally rebased ontop of #1206 or on a merged master :-)

fmauch · 2024-07-18T06:28:00Z

I was sick yesterday, but I'll try my best to squeeze in the tests until the end of the week.

mergify · 2024-07-21T21:29:13Z

This pull request is in conflict. Could you fix it @fmauch?

firesurfer · 2024-08-06T11:40:10Z

@fmauch what's the current state ?

fmauch · 2024-08-08T08:14:59Z

@fmauch what's the current state ?

I've spent some time last week (or the week before?) on trying to get the tests running correctly.

With that I realized that I basically ran into #697. The JTC currently samples the trajectory at the time given to the update loop. In the scaled version, however we basically sample at $t+\Delta t$. That is a conceptual difference. In the latest working group meeting we decided that I will change the JTC accordingly, as this is actually the "proper" approach.

Another issue related to that is that we move forward in the trajectory only based on the controller periods combined with the speed scaling (which again is different to the current method which is why I will have to rewrite the tests). We can't really use the time directly, since the time we spent in the trajectory has been scaled, so we have to sum up the periods.

This will also get interesting when it comes to trajectory replacement, I think, but I haven't looked into that, yet.

So, in summary: Looks like the devil is in the details and this requires still some implementation work to do. While looking at that I did some refactoring work which I haven't pushed yet, as I want to get this together with working basic tests to make sure that the refactoring is actually behaving as expected. As I will be on vacation the next three weeks I don't know how much time I will find for that, though. But I will try.

fmauch · 2024-08-12T18:34:37Z

I'm planning to split things up a bit into separate PRs to make things with a smaller scope, especially when it comes to adapting the tests.

Use $t+\Delta t$ in the JTC
Calculate the sample time based upon passed periods instead of the absolute time. This is a requirement for the scaled JTC, since we have to sum up the scaled periods to move forward in the trajectory.
Add Speed scaling. This will basically only add the interfaces for speed scaling and change that the period the trajectory is moved forward isn't necessarily the complete last update cycle.

I'll leave out trajectory replacement for now, since this shall be implemented at a later point, anyway.

It might still make sense to merge them all in one PR, but I'll separate at least development like that.

christophfroehlich · 2024-08-12T18:57:57Z

It might still make sense to merge them all in one PR, but I'll separate at least development like that.

Ping me if you want me to create a feature/scaled_jtc branch in this repo for your individual PRs. However, the first two points seem to be independent somehow.

mergify · 2024-08-26T12:53:00Z

This pull request is in conflict. Could you fix it @fmauch?

firesurfer · 2024-09-09T06:07:20Z

@fmauch

feel free to poke me :-)

Any updates ? :D

fmauch · 2024-09-14T06:22:15Z

Any updates ? :D

I almost finished the t+dT change on https://github.com/fmauch/ros2_controllers/tree/jtc_tdt. This change still makes tests fail which I have to investigate: fmauch@f453049#diff-8dd2135dc1f4abe6382cc7671ef9ce939196b2d8adf18ac3a77b5c0fddf1a25fR287

fmauch · 2024-10-07T13:47:04Z

Update: #1306 implements sampling at $t + \Delta T$. Furthermore, ros-controls/ros2_control#1774 should fix incorrect controller periods reported to the controller.

With this the next step would be to propagate through the trajectory based on the accumulated periods instead of the absolute time. I expect that this will be quite a lot of work to make all tests comply with that.

I will update this PR once #1306 is merged.

fmauch · 2024-10-14T04:47:08Z

Another update:

Use t + Δ t in the JTC (implemented in [JTC] Sample at t + dT instead of the beginning of the control cycle #1306)
Calculate the sample time based upon passed periods instead of the absolute time. This is a requirement for the scaled JTC, since we have to sum up the scaled periods to move forward in the trajectory. (Currently living ontop of the above on https://github.com/fmauch/ros2_controllers/tree/jtc_sum_periods)
Add Speed scaling. (All merged together currently lives on https://github.com/fmauch/ros2_controllers/tree/scaled_jtc_merged)

Edit:

Note: You will need ros-controls/ros2_control#1774 in order to get valid period times.

fmauch and others added 13 commits July 3, 2024 17:38

use reset+initRT due to missing writeFromRT

55c3bb0

Co-authored-by: Christoph Fröhlich <christophfroehlich@users.noreply.github.com>

Reformat scaling the time period

79a8b7c

This avoids writing the explicit conversion by hand Internally that basically does: static_cast<rcl_duration_value_t>(static_cast<long double>(rcl_duration_.nanoseconds) * scale_ld)

synchronization of scaling factor with hw optional, add service for s…

600ba71

…etting of scaling factor Co-authored-by: Manuel M <mamueluth@gmail.com>

Improve scaling exchange with hardware

c01b1ad

Check command interface name when setting speed scaling

c003883

Update code formatting

aa63e5d

That was changed in a previous commit

WIP: Adding documentation about speed scaling

f7d0ae0

Use a subscriber instead of a service

71ccb39

Do not put the time_data_ into a RealtimeBuffer

1fb38b8

It is accessed from the RT thread only, anyway.

Fix goal time violated

6890d6e

See UniversalRobots/Universal_Robots_ROS2_Driver#882

Merge remote-tracking branch 'origin/master' into scaled_jtc

b5a567d

fmauch mentioned this pull request Jul 3, 2024

Add speed_scaling_factor msg and field in JointTrajectoryControllerState ros-controls/control_msgs#143

Merged

fmauch added 5 commits July 3, 2024 20:38

Use custom message to subscribe speed scaling

908b539

Add scaling factor to controller state

1e66e44

Code formatting

7daf240

Allow scaling factors greater than 1

66abfd9

Add parameter validator for default scaling factor

45fa036

More formatting...

5e9592b

firesurfer reviewed Jul 4, 2024

View reviewed changes

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp Outdated Show resolved Hide resolved

firesurfer reviewed Jul 4, 2024

View reviewed changes

joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated Show resolved Hide resolved

firesurfer reviewed Jul 4, 2024

View reviewed changes

joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated Show resolved Hide resolved

fmauch added 2 commits July 4, 2024 08:34

Remove unnecessary iostream include

42394b9

REVERT_ME: Use dev branch for control_msgs

ba5fecb

fmauch commented Jul 4, 2024

View reviewed changes

firesurfer mentioned this pull request Jul 4, 2024

Ported scaled_jtc #1188

Closed

Use transient_local for the speed scaling topic subscriber

7e88744

fmauch added 2 commits July 5, 2024 16:27

Added documentation on tolerance effects

d2130e0

Added a short explanation of scaling

915bfd3

This isn't the best explanation, but it's a start.

firesurfer reviewed Jul 8, 2024

View reviewed changes

joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated Show resolved Hide resolved

firesurfer reviewed Jul 8, 2024

View reviewed changes

joint_trajectory_controller/src/joint_trajectory_controller.cpp Outdated Show resolved Hide resolved

Use a ordering-safe reference for scaling command and state interfaces

90c6d45

fmauch added 3 commits July 23, 2024 09:44

Merge branch 'master' into scaled_jtc

77e78ce

REVERT_ME Use my version of control_msgs

0a3b974

Use period directly instead of calculating an own period

09bea48

Merge branch 'master' into scaled_jtc

9658347

fmauch mentioned this pull request Oct 7, 2024

[JTC] Sample at t + dT instead of the beginning of the control cycle #1306

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Scaled jtc #1191

Scaled jtc #1191

fmauch commented Jul 3, 2024 •

edited

Loading

fmauch commented Jul 3, 2024

firesurfer commented Jul 4, 2024

fmauch commented Jul 4, 2024

fmauch Jul 4, 2024

fmauch Jul 4, 2024

firesurfer commented Jul 5, 2024

fmauch commented Jul 5, 2024

fmauch commented Jul 5, 2024 •

edited

Loading

christophfroehlich commented Jul 5, 2024

firesurfer Jul 8, 2024

fmauch Jul 8, 2024

firesurfer Jul 9, 2024

firesurfer commented Jul 10, 2024

christophfroehlich commented Jul 10, 2024

fmauch commented Jul 16, 2024

fmauch commented Jul 18, 2024

mergify bot commented Jul 21, 2024

firesurfer commented Aug 6, 2024 •

edited

Loading

fmauch commented Aug 8, 2024

fmauch commented Aug 12, 2024

christophfroehlich commented Aug 12, 2024

mergify bot commented Aug 26, 2024

firesurfer commented Sep 9, 2024

fmauch commented Sep 14, 2024

fmauch commented Oct 7, 2024 •

edited

Loading

fmauch commented Oct 14, 2024 •

edited

Loading

Scaled jtc #1191

Are you sure you want to change the base?

Scaled jtc #1191

Conversation

fmauch commented Jul 3, 2024 • edited Loading

Currently missing

Things that will get changed, as discussed in the latest working group meeting

fmauch commented Jul 3, 2024

firesurfer commented Jul 4, 2024

fmauch commented Jul 4, 2024

fmauch Jul 4, 2024

Choose a reason for hiding this comment

fmauch Jul 4, 2024

Choose a reason for hiding this comment

firesurfer commented Jul 5, 2024

fmauch commented Jul 5, 2024

fmauch commented Jul 5, 2024 • edited Loading

christophfroehlich commented Jul 5, 2024

firesurfer Jul 8, 2024

Choose a reason for hiding this comment

fmauch Jul 8, 2024

Choose a reason for hiding this comment

firesurfer Jul 9, 2024

Choose a reason for hiding this comment

firesurfer commented Jul 10, 2024

christophfroehlich commented Jul 10, 2024

fmauch commented Jul 16, 2024

fmauch commented Jul 18, 2024

mergify bot commented Jul 21, 2024

firesurfer commented Aug 6, 2024 • edited Loading

fmauch commented Aug 8, 2024

fmauch commented Aug 12, 2024

christophfroehlich commented Aug 12, 2024

mergify bot commented Aug 26, 2024

firesurfer commented Sep 9, 2024

fmauch commented Sep 14, 2024

fmauch commented Oct 7, 2024 • edited Loading

fmauch commented Oct 14, 2024 • edited Loading

fmauch commented Jul 3, 2024 •

edited

Loading

fmauch commented Jul 5, 2024 •

edited

Loading

firesurfer commented Aug 6, 2024 •

edited

Loading

fmauch commented Oct 7, 2024 •

edited

Loading

fmauch commented Oct 14, 2024 •

edited

Loading