-
Notifications
You must be signed in to change notification settings - Fork 318
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Scaled jtc #1191
Draft
fmauch
wants to merge
32
commits into
ros-controls:master
Choose a base branch
from
fmauch:scaled_jtc
base: master
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
+282
−6
Draft
Scaled jtc #1191
Changes from all commits
Commits
Show all changes
32 commits
Select commit
Hold shift + click to select a range
d1fcba0
Add scaling factor to JTC
fmauch 55c3bb0
use reset+initRT due to missing writeFromRT
fmauch 79a8b7c
Reformat scaling the time period
fmauch 600ba71
synchronization of scaling factor with hw optional, add service for s…
fmauch c01b1ad
Improve scaling exchange with hardware
fmauch c003883
Check command interface name when setting speed scaling
fmauch aa63e5d
Update code formatting
fmauch 821c88d
Do not advertise speed scaling service unless only position interface is
fmauch f7d0ae0
WIP: Adding documentation about speed scaling
fmauch 71ccb39
Use a subscriber instead of a service
fmauch 1fb38b8
Do not put the time_data_ into a RealtimeBuffer
fmauch 6890d6e
Fix goal time violated
b5a567d
Merge remote-tracking branch 'origin/master' into scaled_jtc
fmauch 908b539
Use custom message to subscribe speed scaling
fmauch 1e66e44
Add scaling factor to controller state
fmauch 7daf240
Code formatting
fmauch 66abfd9
Allow scaling factors greater than 1
fmauch 45fa036
Add parameter validator for default scaling factor
fmauch 5e9592b
More formatting...
fmauch 42394b9
Remove unnecessary iostream include
fmauch ba5fecb
REVERT_ME: Use dev branch for control_msgs
fmauch 4b2f18c
Write back scaling factor to buffer when updated from hardware
fmauch 76e5cab
Remove additional time_data structure in update method
fmauch b22ce07
Use std::atomic for scaling factor buff
fmauch 7e88744
Use transient_local for the speed scaling topic subscriber
fmauch d2130e0
Added documentation on tolerance effects
fmauch 915bfd3
Added a short explanation of scaling
fmauch 90c6d45
Use a ordering-safe reference for scaling command and state interfaces
fmauch 77e78ce
Merge branch 'master' into scaled_jtc
fmauch 0a3b974
REVERT_ME Use my version of control_msgs
fmauch 09bea48
Use period directly instead of calculating an own period
fmauch 9658347
Merge branch 'master' into scaled_jtc
fmauch File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,105 @@ | ||
Speed scaling | ||
============= | ||
|
||
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 | ||
:math:`{f \cdot \Delta_t}` per control step where :math:`{\Delta_t}` is the controller's cycle time. | ||
|
||
Methods of speed scaling | ||
------------------------ | ||
|
||
Generally, the speed scaling feature has two separate scaling approaches in mind: On-Robot scaling | ||
and On-Controller scaling. They are both conceptually different and to correctly configure speed | ||
scaling it is important to understand the differences. | ||
|
||
On-Robot speed scaling | ||
~~~~~~~~~~~~~~~~~~~~~~ | ||
|
||
This scaling method is intended for robots that provide a scaling feature directly on the robot's | ||
teach pendant and / or through a safety feature. One example of such robots are the `Universal | ||
Robots manipulators <https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver>`_. | ||
|
||
For the scope of this documentation a user-defined scaling and a safety-limited scaling will be | ||
treated the same resulting in a "hardware scaling factor". | ||
|
||
In this setup, the hardware will treat the command sent from the ROS controller (e.g. Reach joint | ||
configuration :math:`{\theta}` within :math:`{\Delta_t}` seconds.). This effectively means that the | ||
robot will only make half of the way towards the target configuration when a scaling factor of 0.5 | ||
is given (neglectling acceleration and deceleration influcences during this time period). | ||
|
||
The following plot shows trajectory execution (for one joint) with a hardware-scaled execution and | ||
a controller that is **not** aware of speed scaling: | ||
|
||
.. image:: traj_without_speed_scaling.png | ||
:alt: Trajectory with a hardware-scaled-down execution with a non-scaled controller | ||
|
||
The graph shows a trajectory with one joint being moved to a target point and back to its starting | ||
point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling | ||
(black line) activates and limits the joint speed (green line). As a result, the target trajectory | ||
(light blue) doesn't get executed by the robot, but instead the pink trajectory is executed. The | ||
vertical distance between the light blue line and the pink line is the path error in each control | ||
cycle. We can see that the path deviation gets above 300 degrees at some point and the target point | ||
at -6 radians never gets reached. | ||
|
||
With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes: | ||
|
||
.. image:: traj_with_speed_scaling.png | ||
:alt: Trajectory with a hardware-scaled-down execution with a scaled controller | ||
|
||
The deviation between trajectory interpolation on the ROS side and actual robot execution stays | ||
minimal and the robot reaches the intermediate setpoint instead of returning "too early" as in the | ||
example above. | ||
|
||
Scaling is done in such a way, that the time in the trajectory is virtually scaled. For example, if | ||
a controller runs with a cycle time of 100 Hz, each control cycle is 10 ms long. A speed scaling of | ||
0.5 means that in each time step the trajectory is moved forward by 5 ms instead. | ||
So, the beginning of the 3rd timestep is 15 ms instead of 30 ms in the trajectory. | ||
|
||
Command sampling is performed as in the unscaled case, with the timestep's start plus the **full** | ||
cycle time of 10 ms. The robot will scale down the motion command by 50% resulting in only half of | ||
the distance being executed, which is why the next control cycle will be started at the current | ||
start plus half of the step time. | ||
|
||
|
||
On-Controller speed scaling | ||
~~~~~~~~~~~~~~~~~~~~~~~~~~~ | ||
|
||
Conceptionally, with this scaling the robot hardware isn't aware of any scaling happening. The JTC | ||
generates commands to be sent to the robot that are already scaled down accordingly, so they can be | ||
directly executed by the robot. | ||
|
||
Since the hardware isn't aware of speed scaling, the speed-scaling related command and state | ||
interfaces should not be specified and the scaling factor will be set through the | ||
``~/speed_scaling_input`` topic directly: | ||
|
||
.. code:: console | ||
|
||
$ ros2 topic pub --qos-durability transient_local --once \ | ||
/joint_trajectory_controller/speed_scaling_input control_msgs/msg/SpeedScalingFactor "{factor: 0.5}" | ||
|
||
|
||
.. note:: | ||
The ``~/speed_scaling_input`` topic uses the QoS durability profile ``transient_local``. This | ||
means you can restart the controller while still having a publisher on that topic active. | ||
|
||
.. note:: | ||
The current implementation only works for position-based interfaces. | ||
|
||
|
||
Effect on tolerances | ||
-------------------- | ||
|
||
When speed scaling is used while executing a trajectory, the tolerances configured for execution | ||
will be scaled, as well. | ||
|
||
Since commands are generated from the scaled trajectory time, **path errors** will also be compared to | ||
the scaled trajectory. However, since the next command is always using the full time step, there | ||
will obviously always be a small error added by the robot only executing a part of the command. | ||
|
||
The **goal time tolerance** also uses the virtual trajectory time. This means that a trajectory | ||
being executed with a constant scaling factor of 0.5 will take twice as long for execution than the | ||
``time_from_start`` value of the last trajectory point specifies. As long as the robot doesn't take | ||
longer than that the goal time tolerance is considered to be met. | ||
|
||
If an application relies on the actual execution time as set in the ``time_from_start`` fields, an | ||
external monitoring has to be wrapped around the trajectory execution action. |
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
And/Or use a different child logger that can be separately silenced