scaled jtc #301

fmauch · 2022-02-23T16:40:09Z

This PR adds the scaled trajectory controller that has been created for the Universal_Robots_ROS2_Driver to ros2_controllers.

The idea is basically, that the time at which the trajectory can be sampled can be set forward only by a fraction of the actual cycle time.

We think that this could be beneficial to add to ros2_controllers so we'd like to propose adding it here.

As this is merely a collection of all commits related to the custom controllers in the UR driver, the history of this isn't too nice atm. I can clean it up if desired, but I wanted to start with the opportunity to keep commits from everybody involved.

As this has been created based on a standard JTC's update function a while back, it probably needs some updating. It might also be worthwhile to merge this with the "normal" JTC and simply make the scaling an optional field. This way there would be less code duplication and things would be easier to maintain in the future.

I am well aware, that this is not in a ready-to-merge state, I created a non-draft PR, anyways as I'd like to get some feedback on how to proceed with this.

I've been discussing this with @destogl earlier, so this might be a first draft version.

destogl

Let's keep only scaled_joint_trajectory_controller in this PR and move it to joint_trajectory_controller package.

Also, adding a test or two for it would be good. Probably some existing JTC-tests can be copied and adjusted to take into account scaling factor.

destogl · 2022-03-01T09:59:45Z

scaled_controllers/include/scaled_controllers/scaled_joint_trajectory_controller.hpp

@@ -0,0 +1,66 @@
+// Copyright 2019, FZI Forschungszentrum Informatik


Can we update the copyright year? Would it make sense in this case?

yes, that should not be a problem

destogl · 2022-03-01T10:02:13Z

scaled_controllers/include/scaled_controllers/scaled_joint_trajectory_controller.hpp

+#ifndef SCALED_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
+#define SCALED_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_


I propose adding scaled controller in the joint_trajectory_controller package. The most of the logic is there, so this will enable us to track the changes and issues simpler.

I agree. I would propose adding the scaled controller to the joint_trajectory_controller package. In fact, as suggested in the description, I would rather add the speed scaling as a field in the JTC instead of creating a new controller. There is some duplication of code and it seems brittle since changes in JTC may have to be reflected in this controller.

destogl · 2022-03-01T10:04:45Z

scaled_controllers/include/scaled_controllers/speed_scaling_state_broadcaster.hpp

+#ifndef SCALED_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_
+#define SCALED_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_


I propose to remove the broadcaster for now. This is some kind of “GPIO” broadcaster, but specialized version. Furthermore, it is not necessary to have broadcaster together with the controller here because for setting value forwarding controller can be used and for getting values /dynamic_joint_state message. It is not an optimal solution, but we are not blocking anything for now.

I also agree removing the broadcaster. It seems a little overkill and it should rather be published as status of the controller.

You mean as a status of the modified JTC? Sure, that sounds reasonable.

destogl

@fmauch just pinging this again. What's your plan about this PR?

I see two main things before do proper merging:

move code to joint_trajectory_controller
rather extend joint_trajectory_controller with a parameter then duplicate its code (what do you think about this?

fmauch · 2022-04-19T18:55:18Z

Yes, I still plan to work on this. Currently I am focusing on the ROS2 driver release, though.

bmagyar · 2023-09-15T11:24:50Z

@fmauch @destogl

can we agree we want to move the scaled jtc into this repo?

christophfroehlich · 2023-09-15T11:50:49Z

@fmauch @destogl

can we agree we want to move the scaled jtc into this repo?

As far as I can remember we decided to add the scaling-feature to the existing JTC?

fmauch · 2023-09-18T13:57:34Z

Yes, that was the plan. @destogl told me a couple of times that he planned to do it at some point, so I kept my feed still until now.

destogl · 2023-09-18T17:49:39Z

Yes, I wanted to do this. It will be done in the next 2 weeks, since we have a release of application with this feature. So I will clean this up to be ready for merge.

fmauch · 2023-10-04T19:57:47Z

I've updated the PR with the current state from ur_controllers' scaled JTC. As discussed above, I modified the JTC to accept a scaling interface. There is still some work to be planned, hence converting to draft:

Make the speed scaling interface optional (If not specified, speed scaling of 1 should be assumed)
Make the speed scaling configurable by a service call. (This doesn't necessarily have to be done in this PR, though)
Add documentation about speed scaling
Add tests for speed scaling

codecov · 2023-10-04T20:03:22Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 30.88%. Comparing base (0b43291) to head (7b15431).
Report is 7 commits behind head on master.

❗ Current head 7b15431 differs from pull request most recent head 9851eac. Consider uploading reports for the commit 9851eac to get more accurate results

Additional details and impacted files

@@             Coverage Diff             @@
##           master     #301       +/-   ##
===========================================
- Coverage   71.86%   30.88%   -40.98%     
===========================================
  Files          41        7       -34     
  Lines        3650      832     -2818     
  Branches     1794      505     -1289     
===========================================
- Hits         2623      257     -2366     
+ Misses        707      133      -574     
- Partials      320      442      +122

Flag	Coverage Δ
unittests	`30.88% <ø> (-40.98%)`	⬇️

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

see 43 files with indirect coverage changes

christophfroehlich

I'm late on this topic, and don't have experience with UR.
Where does the info of the scaling come from? Why is this implemented as a variable coming from the hardware?
But you mentioned a service call tbd, so that users can probably set the speed from a GUI and we could add it to rqt_joint_trajectory_controller.

If there isn't just position interface: Shouldn't it scale the velocity and acceleration according to the scaling factor? This would be a more general solution, though not perfect because we cannot scale effort values.

Making this optional is missing for sure (you haven't drafted this PR ;))

joint_trajectory_controller/src/joint_trajectory_controller.cpp

christophfroehlich · 2023-10-09T07:05:41Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+  time_data.time = get_node()->now();
+  time_data.period = rclcpp::Duration::from_nanoseconds(0);
+  time_data.uptime = get_node()->now();
+  time_data_.initRT(time_data);


or

Suggested change

time_data_.initRT(time_data);

time_data_.writeFromNonRT(time_data);

I guess? If this is wrong, I have to change the other buffers in on_activate

The docs state

This method is part of the real-time loop, therefore avoid any reservation of memory and, in general, keep it as short as possible.

so, probably initRT is the correct approach?

possibly? but is a reset() before necessary then?

christophfroehlich · 2023-10-09T07:07:47Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+  }
+  else
+  {
+    RCLCPP_ERROR(


Using a throttled logger or

Suggested change

RCLCPP_ERROR(

RCLCPP_ERROR_ONCE(

I'll leave this unresolved for now. Once the scaling interface is optional it would probably make sense to return ERROR at this point (If scaling is configured to be used, but no interface is found).

fmauch · 2023-10-10T12:50:26Z

Why is this implemented as a variable coming from the hardware? But you mentioned a service call tbd, so that users can probably set the speed from a GUI and we could add it to rqt_joint_trajectory_controller.

On URs this is indeed coming from the hardware. The value used is actually a combination between the "Speed slider" and any safety-related scaling done by the controller. Imagine a straight horizontal arm rotating around its base with a restricted Cartesian velocity setup in the safety configuration. the UR will actually slow down the motion if rotating at full speed would cause a velocity above the limit.

Hence, we read the actual value from the hardware in that case. For robots that don't have this kind of mechanism it might make sense to add this using a service call, though.

christophfroehlich · 2023-10-10T14:26:19Z

On URs this is indeed coming from the hardware. The value used is actually a combination between the "Speed slider" and any safety-related scaling done by the controller. Imagine a straight horizontal arm rotating around its base with a restricted Cartesian velocity setup in the safety configuration. the UR will actually slow down the motion if rotating at full speed would cause a velocity above the limit.

Got it. Do you use a position interface only, or does the UR scale the velocity values internally and you only stretch the trajectory in time?

fmauch · 2023-10-10T20:34:25Z

Got it. Do you use a position interface only, or does the UR scale the velocity values internally and you only stretch the trajectory in time?

Indeed, up until now we only used it with a position interface. With velocities (or even efforts) things get a bit more complicated... I guess I'll have to think about that conceptually a bit more...

christophfroehlich · 2023-10-10T20:44:08Z

Indeed, up until now we only used it with a position interface. With velocities (or even efforts) things get a bit more complicated... I guess I'll have to think about that conceptually a bit more...

I think there is no way to scale efforts properly. And velocity+acceleration would be trivial only assuming the scaling being constant.

mergify · 2023-11-15T19:06:08Z

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

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

fmauch · 2024-04-03T12:52:59Z

@bmagyar is there a reason this got closed? I was hoping to finish this until next week.

firesurfer · 2024-06-27T08:31:18Z

@fmauch Whats the state? You wrote in april you are hoping to finish it within the week.
@bmagyar Same question as in april. Is there a reason this PR was closed?

Btw. this PR might be the right one to follow up on the discussion we had in: UniversalRobots/Universal_Robots_ROS2_Driver#883

Let me know @fmauch if I can help you with upstreaming the scaled JTC. ( I have a certain interest because of #1182 :D )

github-actions bot requested review from bijoua29, bmagyar, destogl, livanov93, rosterloh, Serafadam and VX792 February 23, 2022 16:40

fmauch mentioned this pull request Feb 23, 2022

Use the scaled JTC from (potentially) ros2_controllers UniversalRobots/Universal_Robots_ROS2_Driver#303

Closed

destogl requested changes Mar 1, 2022

View reviewed changes

destogl reviewed Apr 19, 2022

View reviewed changes

fmauch mentioned this pull request Sep 20, 2023

Fix sjtc to correctly aborts on violation of constraints UniversalRobots/Universal_Robots_ROS2_Driver#810

Merged

fmauch force-pushed the scaled_jtc branch from f28b6a1 to 2db6392 Compare October 4, 2023 19:54

christophfroehlich requested changes Oct 9, 2023

View reviewed changes

fmauch marked this pull request as draft October 9, 2023 08:56

christophfroehlich mentioned this pull request Dec 6, 2023

Backports to iron #869

Merged

fmauch and others added 3 commits March 21, 2024 09:10

use reset+initRT due to missing writeFromRT

37aa162

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

Reformat scaling the time period

0904e7c

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)

fmauch force-pushed the scaled_jtc branch from 7b15431 to 59f5865 Compare March 22, 2024 10:52

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

4aea77a

…etting of scaling factor Co-authored-by: Manuel M <[email protected]>

fmauch force-pushed the scaled_jtc branch from 59f5865 to 4aea77a Compare March 22, 2024 10:53

fmauch added 2 commits March 27, 2024 10:12

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

5fc7b41

Improve scaling exchange with hardware

9851eac

fmauch force-pushed the scaled_jtc branch from 7b86724 to 9851eac Compare March 27, 2024 22:06

bmagyar closed this Apr 1, 2024

christophfroehlich mentioned this pull request Jun 26, 2024

[Iron] - JTC produces jump at the end of the trajectory if chained with admittance controller #1182

Closed

firesurfer mentioned this pull request Jul 2, 2024

Ported scaled_jtc #1188

Closed

fmauch mentioned this pull request Jul 3, 2024

Scaled jtc #1191

Open

8 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

scaled jtc #301

scaled jtc #301

fmauch commented Feb 23, 2022 •

edited

Loading

destogl left a comment

destogl Mar 1, 2022

fmauch Mar 3, 2022

destogl Mar 1, 2022

bijoua29 Mar 1, 2022

destogl Mar 1, 2022

bijoua29 Mar 1, 2022

fmauch Mar 3, 2022

destogl left a comment

fmauch commented Apr 19, 2022

bmagyar commented Sep 15, 2023

christophfroehlich commented Sep 15, 2023

fmauch commented Sep 18, 2023

destogl commented Sep 18, 2023

fmauch commented Oct 4, 2023

codecov bot commented Oct 4, 2023 •

edited

Loading

christophfroehlich left a comment •

edited

Loading

christophfroehlich Oct 9, 2023

fmauch Oct 10, 2023

christophfroehlich Oct 10, 2023

christophfroehlich Oct 9, 2023

fmauch Oct 10, 2023

fmauch commented Oct 10, 2023

christophfroehlich commented Oct 10, 2023

fmauch commented Oct 10, 2023

christophfroehlich commented Oct 10, 2023

mergify bot commented Nov 15, 2023

fmauch commented Apr 3, 2024

firesurfer commented Jun 27, 2024

		@@ -0,0 +1,66 @@
		// Copyright 2019, FZI Forschungszentrum Informatik

		#ifndef SCALED_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
		#define SCALED_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_

		#ifndef SCALED_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_
		#define SCALED_CONTROLLERS__SPEED_SCALING_STATE_BROADCASTER_HPP_

	time_data_.initRT(time_data);
	time_data_.writeFromNonRT(time_data);

scaled jtc #301

scaled jtc #301

Conversation

fmauch commented Feb 23, 2022 • edited Loading

destogl left a 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

destogl left a comment

Choose a reason for hiding this comment

fmauch commented Apr 19, 2022

bmagyar commented Sep 15, 2023

christophfroehlich commented Sep 15, 2023

fmauch commented Sep 18, 2023

destogl commented Sep 18, 2023

fmauch commented Oct 4, 2023

codecov bot commented Oct 4, 2023 • edited Loading

Codecov Report

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

fmauch commented Oct 10, 2023

christophfroehlich commented Oct 10, 2023

fmauch commented Oct 10, 2023

christophfroehlich commented Oct 10, 2023

mergify bot commented Nov 15, 2023

fmauch commented Apr 3, 2024

firesurfer commented Jun 27, 2024

fmauch commented Feb 23, 2022 •

edited

Loading

codecov bot commented Oct 4, 2023 •

edited

Loading

christophfroehlich left a comment •

edited

Loading