[Servo] Move `enforcePositionLimits` and `enforceVelocityLimits` to utilities by ibrahiminfinite · Pull Request #2180 · moveit/moveit2

ibrahiminfinite · 2023-05-16T12:11:20Z

Description

This PR moves the functions enforcePositionLimits and enforceVelocityLimits into utilities from servo_calcs.cpp and enforce_limits.cpp respectively.
The dependency of some of the utility functions on rclcpp:Clock has also been removed.

Checklist

Required by CI: Code is auto formatted using clang-format

codecov · 2023-05-16T12:37:17Z

Codecov Report

Patch coverage: 75.81% and project coverage change: -0.03 ⚠️

Comparison is base (15b2331) 50.55% compared to head (d983053) 50.51%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2180      +/-   ##
==========================================
- Coverage   50.55%   50.51%   -0.03%     
==========================================
  Files         387      386       -1     
  Lines       31740    31735       -5     
==========================================
- Hits        16044    16029      -15     
- Misses      15696    15706      +10

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	`71.08% <56.25%> (+0.81%)`	⬆️
moveit_ros/moveit_servo/src/utilities.cpp	`73.02% <82.61%> (-11.25%)`	⬇️

... and 2 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

henningkayser

Is there a reason to not use RobotState::enforceBounds() or the alternatives for position/velocities?

ibrahiminfinite · 2023-05-17T07:07:07Z

Is there a reason to not use RobotState::enforceBounds() or the alternatives for position/velocities?

Probably because of this (from PR #451 )

Previously, the incoming robot state velocity was used to check if the robot would move beyond the joint limit. That was a problem if the robot state did not contain velocity data, i.e. if "copy dynamics" hadn't been turned on.
This PR uses the prospective robot velocity from Servo itself to check if the joint would move beyond the position limit and halt, if so. This velocity should always be available regardless of whether "copy dynamics" is ON.

Maybe @AndyZe can clarify more on this ?

Also @sea-bass , did you have any cases where you needed to use the override_velocity_scaling_factor parameter ?
If that is not useful, we can take that out as well.

sea-bass · 2023-05-17T08:53:40Z

We are currently using override_velocity_scaling_factor actively right now, so please keep it!

moveit_ros/moveit_servo/include/moveit_servo/utilities.h

ibrahiminfinite · 2023-05-18T15:59:55Z

@AndyZe
Can you clarify on this #2180 (review)

AndyZe · 2023-05-18T17:16:47Z

I agree with your comment there

moveit_ros/moveit_servo/include/moveit_servo/utilities.h

moveit_ros/moveit_servo/src/utilities.cpp

ibrahiminfinite · 2023-05-19T14:45:55Z

This PR is a cleanup PR, mostly to keep things cleaner in the current ServoCalcs and also pull out functions that can be reused into utilities so that it could be used in the upcoming refactor.

sea-bass

Just one comment here

moveit_ros/moveit_servo/src/utilities.cpp

moveit_ros/moveit_servo/src/servo_calcs.cpp

sea-bass

Thanks for this! Found a few typos and suggestions.

moveit_ros/moveit_servo/src/servo_calcs.cpp

sea-bass

Almost there! I found a bug in the log message printing.

I think the remaining CI failure might just require updating your branch to main since SonarCloud was just added in a little while earlier.

moveit_ros/moveit_servo/src/servo_calcs.cpp

sea-bass

Awesome!

…pdates

ibrahiminfinite · 2023-05-23T14:45:12Z

@sjahr @sea-bass
I think this can go in now, all tests passing

sjahr · 2023-05-24T07:41:40Z

@AndyZe Do you mind giving a final review?

AndyZe · 2023-05-24T15:30:48Z

Still looks good to me. Let's merge it

ibrahiminfinite force-pushed the cleanup3 branch from 3949857 to 0a917a7 Compare May 16, 2023 15:31

henningkayser reviewed May 16, 2023

View reviewed changes

ibrahiminfinite force-pushed the cleanup3 branch from 0a917a7 to c060d7e Compare May 17, 2023 06:20

AndyZe reviewed May 17, 2023

View reviewed changes

moveit_ros/moveit_servo/include/moveit_servo/utilities.h Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/include/moveit_servo/utilities.h Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/include/moveit_servo/utilities.h Outdated Show resolved Hide resolved

AndyZe reviewed May 19, 2023

View reviewed changes

moveit_ros/moveit_servo/include/moveit_servo/utilities.h Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/utilities.cpp Outdated Show resolved Hide resolved

ibrahiminfinite force-pushed the cleanup3 branch from 0aedf0c to 0a910e6 Compare May 19, 2023 14:43

AndyZe approved these changes May 19, 2023

View reviewed changes

sea-bass suggested changes May 19, 2023

View reviewed changes

moveit_ros/moveit_servo/src/utilities.cpp Outdated Show resolved Hide resolved

sea-bass reviewed May 19, 2023

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Show resolved Hide resolved

ibrahiminfinite requested review from AndyZe and sea-bass May 19, 2023 15:58

sea-bass reviewed May 19, 2023

View reviewed changes

ibrahiminfinite requested a review from sea-bass May 19, 2023 17:01

sea-bass reviewed May 19, 2023

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

ibrahiminfinite force-pushed the cleanup3 branch from 0396433 to b8c49f9 Compare May 19, 2023 19:37

sea-bass suggested changes May 19, 2023

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

moveit_ros/moveit_servo/src/servo_calcs.cpp Outdated Show resolved Hide resolved

ibrahiminfinite force-pushed the cleanup3 branch from bfd95cd to 5976170 Compare May 20, 2023 13:46

sea-bass approved these changes May 20, 2023

View reviewed changes

ibrahiminfinite added 6 commits May 23, 2023 19:15

Move limit enforcing functions to utilities

a537116

Fix comments

28fd114

Make clock const

8e83e92

Remove clock from enforcePositionLimit

87e8946

Remove clock usage from transformTwistToPlanningFrame and applyJointU…

afb2236

…pdates

Remove clock from vvelocityScalingFactorForSingularity

4332e35

ibrahiminfinite added 4 commits May 23, 2023 19:15

Fix tests

b7f5662

Cleanups + clang-tidy

5589c22

Minor cleanups

0c242f2

Log output formatting

d983053

ibrahiminfinite force-pushed the cleanup3 branch from 5976170 to d983053 Compare May 23, 2023 13:45

sjahr approved these changes May 24, 2023

View reviewed changes

AndyZe merged commit 8d319c5 into moveit:main May 24, 2023

ibrahiminfinite deleted the cleanup3 branch June 3, 2023 14:12

Conversation

ibrahiminfinite commented May 16, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Checklist

Uh oh!

codecov bot commented May 16, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

henningkayser left a comment

Choose a reason for hiding this comment

Uh oh!

ibrahiminfinite commented May 17, 2023

Uh oh!

sea-bass commented May 17, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

ibrahiminfinite commented May 18, 2023

Uh oh!

AndyZe commented May 18, 2023

Uh oh!

Uh oh!

Uh oh!

ibrahiminfinite commented May 19, 2023

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

sea-bass left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

sea-bass left a comment

Choose a reason for hiding this comment

Uh oh!

ibrahiminfinite commented May 23, 2023

Uh oh!

sjahr commented May 24, 2023

Uh oh!

AndyZe commented May 24, 2023

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants

ibrahiminfinite commented May 16, 2023 •

edited

Loading

codecov bot commented May 16, 2023 •

edited

Loading

sea-bass commented May 17, 2023 •

edited

Loading

sea-bass left a comment •

edited

Loading