Remove the margin argument when checking bounds

[SuyashSapkal]

Description

Fixes #2090

Updated moveit_core/robot_model/src/joint_model.cpp at line 121.
changes done as mentioned in the screenshots below

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
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[codecov]

Codecov Report

Patch coverage: 100.00% and project coverage change: +50.90 🎉

Comparison is base (c5d1ec9) 0.00% compared to head (9963376) 50.90%.

❗ Current head 9963376 differs from pull request most recent head 4ce7d70. Consider uploading reports for the commit 4ce7d70 to get more accurate results

Additional details and impacted files

@@            Coverage Diff            @@
##           main    #2097       +/-   ##
=========================================
+ Coverage      0   50.90%   +50.90%     
=========================================
  Files         0      391      +391     
  Lines         0    32126    +32126     
=========================================
+ Hits          0    16350    +16350     
- Misses        0    15776    +15776     

Impacted Files	Coverage Δ
moveit_core/robot_model/src/joint_model.cpp	60.57% <100.00%> (ø)

... and 390 files with indirect coverage changes

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

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

[AndyZe]

Unfortunately the margin argument is used in lots of other places, too. So this PR will need to be bigger in scope. Before diving into the coding, is there a consensus that it is indeed backward? @henningkayser @tylerjw @sjahr

[tylerjw]

I'm confused about what the usefulness of this change is.

[AndyZe]

Don't you think the margin should be subtracted from max_velocity instead of added to it? And also, is this margin actually used by anyone?

I suspect it was a hack back when time parameterization wasn't very good and it occasionally gave output that exceeded vel/accel limits.

[SuyashSapkal]

from the code below:

I am assuming the function should return false
if other_bounds[i].min_velocity_ - margin < values[i] < other_bounds[i].max_velocity_ + margin.

Currently,
it's returning true.

Is that the issue?
or is it the way it should be? that implies no changes required?

[henningkayser]

The reasoning makes sense to me, a safety margin should define a range "before" the limit and not an additional tolerance. Please fix the min_velocity case as well. Also, appropriate documentation of the result seems needed as well.

[AndyZe]

@henningkayser what about removing this margin argument completely? It's used in lots of places throughout the codebase, not just here. I'm convinced it was a hack which allows small overshoots from time parameterization. Which we should not be having any longer.

[henningkayser]

@henningkayser what about removing this margin argument completely? It's used in lots of places throughout the codebase, not just here. I'm convinced it was a hack which allows small overshoots from time parameterization. Which we should not be having any longer.

we might certainly break some things by fixing the margin behavior. If you think the currently implemented use cases are hacks, I would agree with removing support for it.

[SuyashSapkal]

Hi @AndyZe and @henningkayser ,

I did a quick search for satisfiesVelocityBounds( using find command in the whole repo and found it being present in the below files with their lines in which it is being used.

1. moveit_core/robot_model/include/moveit/robot_model/joint_model.h
   311: bool satisfiesVelocityBounds(const double* values, double margin = 0.0) const
   313: return satisfiesVelocityBounds(values, variable_bounds_, margin);
   317: virtual bool satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const;

2. moveit_core/robot_model/src/joint_model.cpp
   117:bool JointModel::satisfiesVelocityBounds(const double* values, const Bounds& other_bounds, double margin) const

3. moveit_core/robot_state/include/moveit/robot_state/robot_state.h
   1552: return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
   1558: bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const
   1560: return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);

4. moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp
   187: if (!joint_model->satisfiesVelocityBounds(&joint_limit.max_velocity))

I am assuming I need to make changes in all these lines for removing margin parameter.
Let me know if I should start doing the changes in these lines.

[AndyZe]

Please remove it everywhere, not just satisfiesVelocityBounds(). Here's a list.

use_of_margin.txt

[SuyashSapkal]

I have made the changes as per the list given by you. Except for these 2 lines

moveit_core/kinematic_constraints/test/test_constraints.cpp --> contains word margin in comments.
moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h --> I was skeptical as it's being used in a function definition void setMargin(btScalar /*margin*/) override

[AndyZe]

@SuyashSapkal can I have write access to your fork of moveit2 please? I'll see what I can do to fix up these CI failures.

[SuyashSapkal]

I have provided you the access. Can you check.
Also, how can I work on the changes so that it passes the CI tests?

[AndyZe]

If you click Details for the failed CI jobs, then expand Run industrial ci -- you'll see this:

/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp:136:40: error: no matching function for call to ‘moveit::core::RobotState::satisfiesBounds(const moveit::core::JointModel*&, const double&)’
136 | if (start_state.satisfiesBounds(jmodel, bounds_dist_))

That particular error is self-explanatory, just remove bounds_dist_ since that argument is gone.

[AndyZe]

@SuyashSapkal I'm sorry but I screwed something up while trying to rebase your PR on main. I think it's because both branches are named main. We need to push your changes back to your branch.

If you can, please do this:

git push --force origin main (Assuming origin is the name of your remote)

[AndyZe]

It would also be wise to create a backup branch of your changes, with a name other than main. Possibly call it remove_margin_arg or something similar.

git checkout -b remove_margin_arg
git push origin remove_margin_arg

[SuyashSapkal]

do you want me to create a backup branch? and push the changes back to my forked main branch?

[AndyZe]

How about push a backup branch with a decent name, then open a new PR with that branch? We can leave this PR closed.

[SuyashSapkal]

remove_margin_arg

ok sure. I have created a branch with name remove_margin_arg will open PR with that