Acceleration Limited Smoothing Plugin for Servo #2651

pac48 · 2024-01-13T00:17:57Z

Description

This PR adds a new plugin called AccelerationLimitedPlugin under the online_signal_smoothing::SmoothingBaseClass interface. The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes to the servo command topics. The plugin is not meant to act as a reference tracking controller, rather it's designed to be a layer between the hardware and command interface that prevents infeasible motions.

There are three cases considered for acceleration-limiting illustrated in the following figures:

In the figures, the red arrows show the displacement that will occur given the current velocity. The blue line shows the displacement between the current position and the desired position. The black dashed line shows the maximum possible displacements that are within the acceleration limits. The green line shows the acceleration commands that will be used by this acceleration-limiting plugin.

Figure A: The desired position is within the acceleration limits. The next commanded point will be exactly the desired point.
Figure B: The line between the current position and the desired position intersects the acceleration limits, but the reference position is not within the bounds. The next commanded point will be the point on the displacement line that is closest to the reference.
Figure C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while maintaining its direction.

Case B can be solved via linear or quadratic program, hence I added OSQP as a dependency. This approach is compared to a more naive approach (where the required acceleration is scaled to avoid any limits from being violated) on a square tracing servoing motion (Left is this plugin).

The same motions are also shown as functions of time in the following plots. Importantly, the velocity of the left plot goes to zero in order to take the 90-degree turn.

Testing instructions:

Launch demo_ros_api.launch.py from moveit_serov
Run demo_ros_api.launch.py from servo_keyboard_input
Use arrow keys to move the robot. You should see smoother motion compared to without the plugin.

The update to the ros2_control usage may have broken this example. So to test, you will have to revert that commit.

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference

codecov · 2024-01-13T00:59:12Z

Codecov Report

Attention: 5 lines in your changes are missing coverage. Please review.

Comparison is base (e1782bc) 50.92% compared to head (05e0918) 50.74%.

Files	Patch %	Lines
...nline_signal_smoothing/src/acceleration_filter.cpp	97.34%	4 Missing ⚠️
moveit_ros/moveit_servo/src/servo.cpp	83.34%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2651      +/-   ##
==========================================
- Coverage   50.92%   50.74%   -0.17%     
==========================================
  Files         391      392       +1     
  Lines       32590    32549      -41     
==========================================
- Hits        16592    16513      -79     
- Misses      15998    16036      +38

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

...it_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

sjahr · 2024-01-16T10:13:03Z

...it_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

+ *********************************************************************/
+
+/* Author: Paul Gesel
+   Description: applies smoothing by limiting the acceleration between consecutive commands


Maybe add more details to description

If you feel particularly ambitious, the images in your PR ASCIIfied would be great here. Or otherwise, to add it to a README in this folder.

Added ASCII diagrams

sjahr · 2024-01-16T09:48:13Z

...it_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

+
+#include <cstddef>
+
+// Auto-generated


Is this comment referring to moveit_acceleration_parameters.hpp? If yes, maybe it can be removed since we're using generate_parameters all over the moveit2 codebase

Good point. I am removing it.

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

sjahr · 2024-01-16T09:51:13Z

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

+ *********************************************************************/
+
+/* Author: Paul Gesel
+   Description: applies smoothing by limiting the acceleration between consecutive commands


Suggested change

Description: applies smoothing by limiting the acceleration between consecutive commands

Usually, we only have a description in the header

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

sjahr · 2024-01-16T10:17:55Z

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

+    osqp_update_bounds(work_, data_->l.data(), data_->u.data());
+  };
+
+  size_t num_constraints = num_joints_ + 1;


Add more comments to describe the computation steps

moveit_ros/moveit_servo/launch/demo_pose.launch.py

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

Co-authored-by: Sebastian Jahr <sebastian.jahr@tuta.io>

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

sea-bass

Looks awesome!

My only real comment besides small stuff is that the variable naming and amount of comments in the "meat" of the OSQP code could use some attention.

Oh yeah, also one of the unit tests seems to be failing: https://github.com/ros-planning/moveit2/actions/runs/7562781804/job/20593931009?pr=2651#step:12:9070

moveit_core/CMakeLists.txt

moveit_core/online_signal_smoothing/CMakeLists.txt

sea-bass · 2024-01-18T02:53:36Z

...it_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

+ *********************************************************************/
+
+/* Author: Paul Gesel
+   Description: applies smoothing by limiting the acceleration between consecutive commands


If you feel particularly ambitious, the images in your PR ASCIIfied would be great here. Or otherwise, to add it to a README in this folder.

...it_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h

moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp

moveit_ros/moveit_servo/launch/demo_ros_api.launch.py

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

sea-bass

I think it's almost there!

moveit_core/online_signal_smoothing/CMakeLists.txt

moveit_ros/moveit_servo/launch/demo_ros_api.launch.py

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

sea-bass

LGTM -- but let's wait until @sjahr is also good.

sea-bass · 2024-01-18T22:32:32Z

Aw man! There are some clang-tidy checks remaining: https://github.com/ros-planning/moveit2/actions/runs/7576392893/job/20635103938?pr=2651#step:12:9525

Clang tidy

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 marked this pull request as draft January 13, 2024 00:18

sjahr mentioned this pull request Jan 16, 2024

Update servo parameters - studio_ws PickNikRobotics/moveit_studio_ur_ws#173

Merged

1 task

sjahr reviewed Jan 16, 2024

View reviewed changes

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from d351f44 to aaf3af4 Compare January 17, 2024 18:45

pac48 and others added 10 commits January 17, 2024 11:46

add plugin

6c2f702

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

add move_group_name parameter to filter

952cbcb

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

add osqp dependency

da4d7be

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>

osqp working

5ef3e44

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>

setup second optmization problem

bb09293

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

slow down math needs work

c31f4cf

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

fixed slow down pprblem!

108f555

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

clean up and simplify logic

3f4ef69

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

update launch files

434b605

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

fix unsigned compare

9bd096c

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from aaf3af4 to aa77647 Compare January 17, 2024 18:46

pac48 marked this pull request as ready for review January 17, 2024 18:46

add tests

d28596c

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from aa77647 to d28596c Compare January 17, 2024 19:42

pac48 and others added 3 commits January 17, 2024 13:35

Update moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

3fe0e4a

Co-authored-by: Sebastian Jahr <sebastian.jahr@tuta.io>

address PR feedback

6bd765a

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

revert ros2 control fix

231f02a

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from 225f1e0 to 231f02a Compare January 17, 2024 21:09

pac48 added 3 commits January 17, 2024 14:14

fix compile issue in test and change order of operations in servo

705c229

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

remove explicit parameter check

adb42c7

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

add math description

d0cae38

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 requested a review from sjahr January 17, 2024 22:54

sea-bass reviewed Jan 18, 2024

View reviewed changes

pac48 added 2 commits January 18, 2024 08:38

add ASCII diagrams

ce379bf

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

add README

6484cd9

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

rename parameters and fix cmake format

7b4c6e3

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from ea7ce4c to 7b4c6e3 Compare January 18, 2024 15:58

pac48 and others added 6 commits January 18, 2024 11:50

fix test

adbadf7

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

move_group_name rename planning_group_name

3dbae8f

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

Update moveit_core/online_signal_smoothing/src/acceleration_filter.cpp

7de0c9d

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

add const and fix spelling

3875cf2

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

change parameter name

48ff657

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

update README

8b80d6e

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 requested a review from sea-bass January 18, 2024 20:30

Merge branch 'main' into pr-add-acceleration-based-smoothing

6641364

sea-bass reviewed Jan 18, 2024

View reviewed changes

moveit_core/online_signal_smoothing/CMakeLists.txt Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/launch/demo_ros_api.launch.py Outdated Show resolved Hide resolved

pac48 and others added 2 commits January 18, 2024 14:50

Update moveit_ros/moveit_servo/launch/demo_ros_api.launch.py

23902c1

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>

add pluginlib to cmake

2d64d39

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

sea-bass previously approved these changes Jan 18, 2024

View reviewed changes

pac48 force-pushed the pr-add-acceleration-based-smoothing branch 2 times, most recently from be703cd to 87bd0a8 Compare January 18, 2024 23:32

rename matrices and add override

05e0918

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

pac48 force-pushed the pr-add-acceleration-based-smoothing branch from 87bd0a8 to 05e0918 Compare January 18, 2024 23:47

sea-bass approved these changes Jan 19, 2024

View reviewed changes

sjahr approved these changes Jan 19, 2024

View reviewed changes

sjahr merged commit cba873a into moveit:main Jan 19, 2024
12 checks passed

AndyZe mentioned this pull request Aug 7, 2024

Servo cannot reach the desired control rate #2955

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Acceleration Limited Smoothing Plugin for Servo #2651

Acceleration Limited Smoothing Plugin for Servo #2651

pac48 commented Jan 13, 2024 •

edited

Loading

codecov bot commented Jan 13, 2024 •

edited

Loading

sjahr Jan 16, 2024

sea-bass Jan 18, 2024

pac48 Jan 18, 2024

sjahr Jan 16, 2024

pac48 Jan 17, 2024 •

edited

Loading

sjahr Jan 16, 2024

pac48 Jan 17, 2024

sjahr Jan 16, 2024

sea-bass left a comment •

edited

Loading

sea-bass Jan 18, 2024

sea-bass left a comment

sea-bass left a comment

sea-bass commented Jan 18, 2024

Acceleration Limited Smoothing Plugin for Servo #2651

Acceleration Limited Smoothing Plugin for Servo #2651

Conversation

pac48 commented Jan 13, 2024 • edited Loading

Description

Testing instructions:

Checklist

codecov bot commented Jan 13, 2024 • edited Loading

Codecov Report

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

pac48 Jan 17, 2024 • 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

sea-bass left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass commented Jan 18, 2024

pac48 commented Jan 13, 2024 •

edited

Loading

codecov bot commented Jan 13, 2024 •

edited

Loading

pac48 Jan 17, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading