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

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

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.

ros2_controllers-not-released.jazzy.repos

ros2_controllers-not-released.rolling.repos

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. Implemented in Jtc sum periods #1395
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 · 2024-12-03T06:47:05Z

This should be a state that could be started to get reviewed. There are a couple of things left that I'm not 100% happy, but they don't necessarily have to be covered in this PR in my opinion:

I started using the feedback's actual.time_from_start to show the controller time passed since the trajectory started and desired.time_from_start for the scaled trajectory time since the trajectory started. This was mainly done since I needed a way to ovserve scaled time for testing purposes.
The scaling method assumes (as mentioned in the docs) that the robot is scaling down the trajectory, so we have to scale time when looking at the beginning of the control period (How much of one period have we actually progressed in the last period?) and then we sample one full period ahead. If the robot doesn't scale down but we still want to use scaling, this will leave us with a tracking error. In that case we should scale the period for sampling the control target. My idea would be to make those two scaling methods available with a scaling_type parameteter with the values HARDWARE and CONTROLLER. I would also be happy to move that to a follow-up PR in order to get this one finally wrapped up. @RobertWilbrandt does this summarize the discussion we had a couple of days ago?
There are a couple of builds failing that I don't quite understand. gpio controller is complaining about missing messages. Is this a cache thing? Because it is building on the master branch and also locally things are fine, so at least the semi-binary builds should be happy.

christophfroehlich · 2024-12-03T11:03:58Z

* There are a couple of builds failing that I don't quite understand. gpio controller is complaining about missing messages. Is this a cache thing? Because it is building on the master branch and also locally things are fine, so at least the semi-binary builds should be happy.

~~I had a look now, I don't know tbh. The debian build fails with the same error, but this one doesn't use caches.~~
Ah, I found the issue ;)

ros2_controllers.rolling.repos

Co-authored-by: Felix Exner (fexner) <[email protected]>

codecov · 2024-12-03T11:29:17Z

Codecov Report

Attention: Patch coverage is 58.62069% with 36 lines in your changes missing coverage. Please review.

Project coverage is 83.37%. Comparing base (36068e1) to head (7844221).
Report is 6 commits behind head on master.

Files with missing lines	Patch %	Lines
...ory_controller/src/joint_trajectory_controller.cpp	26.53%	28 Missing and 8 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1191      +/-   ##
==========================================
- Coverage   83.56%   83.37%   -0.19%     
==========================================
  Files         122      122              
  Lines       10992    11076      +84     
  Branches      937      951      +14     
==========================================
+ Hits         9185     9235      +50     
- Misses       1493     1520      +27     
- Partials      314      321       +7

Flag	Coverage Δ
unittests	`83.37% <58.62%> (-0.19%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...jectory_controller/joint_trajectory_controller.hpp	`100.00% <ø> (ø)`
...ectory_controller/test/test_trajectory_actions.cpp	`97.93% <100.00%> (+0.16%)`	⬆️
...ory_controller/test/test_trajectory_controller.cpp	`99.74% <100.00%> (+<0.01%)`	⬆️
...ory_controller/src/joint_trajectory_controller.cpp	`80.10% <26.53%> (-3.81%)`	⬇️

... and 3 files with indirect coverage changes

fmauch · 2024-12-03T15:48:08Z

~~I had a look now, I don't know tbh. The debian build fails with the same error, but this one doesn't use caches.~~ Ah, I found the issue ;)

facepalm I thought I changed that back some time ago already.

christophfroehlich

Some first remarks

Add a short description to the release_notes please.
Add the new ROS interfaces here

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

christophfroehlich · 2024-12-03T23:27:14Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1580,6 +1683,32 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(
  }
 }

+bool JointTrajectoryController::set_scaling_factor(const double scaling_factor)


This method has no test coverage

christophfroehlich · 2024-12-03T23:27:46Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -902,6 +967,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
  // parse remaining parameters
  default_tolerances_ = get_segment_tolerances(logger, params_);

+  // Set scaling interfaces


This part has no test coverage

joint_trajectory_controller/doc/speed_scaling.rst

firesurfer · 2024-12-04T19:23:19Z

joint_trajectory_controller/doc/speed_scaling.rst

+=============
+
+The ``joint_trajectory_controller`` (JTC) supports dynamically scaling its trajectory execution speed.
+That means, when specifying a scaling factor :math:`{f}` of less than 1, execution will proceed only


Is it possible to specify a value > 1 for simulation purposes ? We are using this quite a lot in our setup.
What would the maximum rate be? I saw the limit with the old implementation at 2.5

Yes, it can be greater than 1. I haven't investigated a limit, though.

firesurfer · 2024-12-04T19:25:55Z

joint_trajectory_controller/doc/speed_scaling.rst

+
+.. note::
+   The current implementation only works for position-based interfaces.
+


Perhaps I am too tired after the ROScon. But could it be that it doesn't become 100% clear how to select a specfic mode?

What do you mean by "how to select a specific mode"? The current implementation will simply not do speed scaling when any other interface than position is configured for the controller.

If I just read the documentation I would think:

a) There are multiple modes I can choose from with the hint that on-robot scaling probably produces the better results
b) If there are multiple modes I would think: Ok somehow I have to tell the JTC to select which mode.

joint_trajectory_controller/src/joint_trajectory_controller.cpp

firesurfer · 2024-12-04T19:34:37Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+      "of speed scaling, please only use a position interface when configuring this controller.");
+    scaling_factor_ = 1.0;
+  }
+  if (!params_.speed_scaling_state_interface_name.empty())


Does it make sense to allow changing the scaling_factor via the topic and from the teach panel ?

It should be either topic or hardware, at least that was my intention. I'll revisit that to verify that that;s what I've implemented.

Edit: It's in the JointTrajectoryController::set_scaling_factor function. If speed scaling is set with a speed scaling state interface configured, but not with a command interface it will print a warning. This way it will also update the value on the teach pendant.

firesurfer · 2024-12-04T19:49:32Z

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -1012,3 +1021,74 @@ INSTANTIATE_TEST_SUITE_P(
    std::make_tuple(
      std::vector<std::string>({"effort"}),
      std::vector<std::string>({"position", "velocity", "acceleration"}))));
+
+TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_execution_time_succeeds)


I have the feeling that just adding one test for such a complex feature is probably not enough.

I think it is missing some parts:

It just checks the if the trajectory took the right amount of time -> It does not check if the trajectory was executed in the right way (e.g. had a spike what ever)

Executing multiple trajectories in series

Proper behavior regarding state and goal tolerances (and goal time tolerances)

2/3. are motivated by: UniversalRobots/Universal_Robots_ROS2_Driver#882

Co-authored-by: Christoph Fröhlich <[email protected]>

Also run two trajectories

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

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

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

ros2_controllers-not-released.jazzy.repos Outdated Show resolved Hide resolved

ros2_controllers-not-released.rolling.repos Outdated Show resolved Hide resolved

firesurfer mentioned this pull request Jul 4, 2024

Ported scaled_jtc #1188

Closed

Cleanup some logging

0d08c38

fmauch mentioned this pull request Nov 27, 2024

Jtc sum periods #1395

Merged

fmauch added 3 commits December 1, 2024 12:49

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

35dbc4f

Add test for scaled trajectory execution

c56ce2b

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

b395104

fmauch marked this pull request as ready for review December 3, 2024 06:36

github-actions bot requested review from anfemosa, ARK3r, bijoua29, christophfroehlich, destogl, livanov93 and Raivias December 3, 2024 06:36

fmauch requested a review from firesurfer December 3, 2024 06:47

christophfroehlich reviewed Dec 3, 2024

View reviewed changes

ros2_controllers.rolling.repos Outdated Show resolved Hide resolved

Use master branch of control_msgs repo

b101360

Co-authored-by: Felix Exner (fexner) <[email protected]>

christophfroehlich requested changes Dec 4, 2024

View reviewed changes

christophfroehlich reviewed Dec 4, 2024

View reviewed changes

joint_trajectory_controller/doc/speed_scaling.rst Outdated Show resolved Hide resolved

firesurfer reviewed Dec 4, 2024

View reviewed changes

fmauch and others added 5 commits December 4, 2024 21:32

Apply suggestions from code review

21c9c4d

Co-authored-by: Christoph Fröhlich <[email protected]>

Update tolerances documentation

9d5c981

Only log set scaling factor if it is changed

a2d319b

Remove duplicate reading period from parameter

5d2b852

Make trajectory test use tolerances

7844221

Also run two trajectories

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 commented Oct 14, 2024 •

edited

Loading

fmauch commented Dec 3, 2024

christophfroehlich commented Dec 3, 2024 •

edited

Loading

codecov bot commented Dec 3, 2024 •

edited

Loading

fmauch commented Dec 3, 2024

christophfroehlich left a comment •

edited

Loading

christophfroehlich Dec 3, 2024

christophfroehlich Dec 3, 2024

firesurfer Dec 4, 2024

fmauch Dec 4, 2024

firesurfer Dec 4, 2024

fmauch Dec 4, 2024

firesurfer Dec 5, 2024

firesurfer Dec 4, 2024

fmauch Dec 4, 2024 •

edited

Loading

firesurfer Dec 4, 2024


		.. note::
		The current implementation only works for position-based interfaces.

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 commented Oct 14, 2024 • edited Loading

fmauch commented Dec 3, 2024

christophfroehlich commented Dec 3, 2024 • edited Loading

codecov bot commented Dec 3, 2024 • edited Loading

Codecov Report

fmauch commented Dec 3, 2024

christophfroehlich left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmauch Dec 4, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fmauch commented Jul 3, 2024 •

edited

Loading

fmauch commented Oct 14, 2024 •

edited

Loading

christophfroehlich commented Dec 3, 2024 •

edited

Loading

codecov bot commented Dec 3, 2024 •

edited

Loading

christophfroehlich left a comment •

edited

Loading

fmauch Dec 4, 2024 •

edited

Loading