[WIP] Cartesian Tool Path Planning #1946

sea-bass · 2023-02-13T18:53:51Z

Description

This reimplements #377 with the suggestions from review and continues the work to make it a complete feature.

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

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

codecov · 2023-02-13T23:11:37Z

Codecov Report

Base: 50.30% // Head: 50.18% // Decreases project coverage by -0.12% ⚠️

Coverage data is based on head (a1ebdf6) compared to base (bff9600).
Patch coverage: 0.00% of modified lines in pull request are covered.

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

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1946      +/-   ##
==========================================
- Coverage   50.30%   50.18%   -0.12%     
==========================================
  Files         374      374              
  Lines       31358    31438      +80     
==========================================
+ Hits        15772    15773       +1     
- Misses      15586    15665      +79

Impacted Files	Coverage Δ
...de/moveit/ompl_interface/detail/ompl_constraints.h	`0.00% <0.00%> (ø)`
...mpl/ompl_interface/src/detail/ompl_constraints.cpp	`0.00% <0.00%> (ø)`
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	`78.04% <0.00%> (-1.13%)`	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	`45.69% <0.00%> (+0.44%)`	⬆️

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 at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

sea-bass · 2023-02-15T16:00:17Z

The code is now at the point where I can pass in a path constraint, it gets instantiated, and we actually get a plan.

However, the plan right now either fails because of tight bounds or succeeds but doesn't honor the tool path at all.

There are lots of OMPL interface nuances that are still mysterious to me, and I may need some help with this to head in the right direction vs. just doing things with trial and error.

mamoll · 2023-02-15T18:10:33Z

@zkingston do you have time to take a look?

zkingston

Could you elaborate more on the failure case? e.g., when you say the tolerances are too tight and planning fails. I took a quick look through the code.

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

sea-bass · 2023-02-15T20:42:31Z

Could you elaborate more on the failure case? e.g., when you say the tolerances are too tight and planning fails. I took a quick look through the code.

Thanks for looking at this!

I don't know if I have anything super specific rather than generally trying to understand how this all works. Some pointed questions:

What does the bounds_ member correspond to for a constraint like this. Should it be the bounds of the workspace? The bounds for each of the elements in the error function? Something else?
Does implementing function() vs calcError() make a difference? What is the right way of creating something that constrains a search space to be within a polyline like this?
Should the error be a scalar or a vector?

zkingston · 2023-02-15T20:52:15Z

I think MoveIt's box constraint bounds here define the region in which EE positions are considered valid (I am not 100% on this). Since your constraint is a sequence of smaller position constraints (maybe you could think of this as a collection of MoveIt's box / position / equality constraints tiled together so that each error region overlaps with each other...), the bounds here you inherit from BaseConstraint probably don't do what you want, you might need to overload the Jacobian.
calcError is a MoveIt thing, function is the OMPL constraint version. I don't know enough about the MoveIt implementation to say.
Vector should work, but if you can get the gradient for scalar that should be fine as well.

sea-bass · 2023-02-15T21:36:54Z

Interesting... so is there a way in OMPL to do a "ConstraintUnion" rather than ConstraintIntersection to tile these together?

zkingston · 2023-02-15T21:44:00Z

I think an idea similar to what you have currently, but using the NN to store the centers of each constraint region (which tile the path similar to what you have currently), and then evaluating the error / jacobian to the closest constraint may work.

There isn't a way in OMPL to do a "constraint union" like this - I'm not sure if it even is a well-formed concept.

sea-bass · 2023-02-15T23:10:50Z

Thank you for all this input -- these are helpful things to try out!

sea-bass · 2023-02-17T23:03:09Z

I tried going down this path of individual box constraints, and have something that runs with more reusable code... but I keep getting this at the end.

[ERROR] [1676674916.342760349] [ompl]: ./src/ompl/base/src/SpaceInformation.cpp:468 - State samplers generate constraint unsatisfying states.

Interestingly enough, if my path has two points, so it's a single box constraint, everything works perfectly and I can do constrained planning in a straight line. But as soon as I add more points, even if it's just splitting a straight line into two, it fails.

Will dig more when I find more time / energy.

sea-bass · 2023-02-17T23:04:47Z

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

+void ToolPathConstraint::function(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                  Eigen::Ref<Eigen::VectorXd> out) const
+{
+  const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values);
+
+  const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose));
+  // std::cout << "Function, nearest: " << eef_nearest.path_idx_ << std::endl;
+
+  const auto bc = box_constraints_.at(eef_nearest.path_idx_);
+  bc->function(joint_values, out);
+}
+
+void ToolPathConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                  Eigen::Ref<Eigen::MatrixXd> out) const
+{
+  const Eigen::Isometry3d eef_pose = forwardKinematics(joint_values);
+  const auto eef_nearest = pose_nn_.nearest(EigenIsometry3dWrapper(eef_pose));
+  // std::cout << "Jacobian, nearest: " << eef_nearest.path_idx_ << std::endl;
+
+  const auto bc = box_constraints_.at(eef_nearest.path_idx_);
+  bc->jacobian(joint_values, out);
+}


These are the "magic" functions that map the EEF pose to the nearest box constraint for that path segment and compute the standard function/Jacobian for that box constraints.

I think this is close, except that I think the state sampler needs to be configured.

github-actions · 2023-04-04T12:07:40Z

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

tylerjw · 2023-08-23T17:13:15Z

Closing as this seems to be abandoned. Feel free to re-open if you take it back up.

Reimplement Boston's original PR with just Eigen types

43a1dc3

sea-bass mentioned this pull request Feb 13, 2023

Finish Tool Path Constraint Implementation #1740

Open

mamoll reviewed Feb 13, 2023

View reviewed changes

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp Outdated Show resolved Hide resolved

sea-bass added 2 commits February 13, 2023 17:21

Add arc length between quaternions + fix compiler warnings

25dcb7e

Change operator overload implementation

94d6b41

sea-bass mentioned this pull request Feb 14, 2023

Add PathConstraint message moveit/moveit_msgs#152

Draft

sea-bass added 2 commits February 14, 2023 09:12

Add path constraint to OMPL constraint factory

a1ebdf6

Implement other necessary changes to get path constraint to instantiate

8ff64c0

zkingston reviewed Feb 15, 2023

View reviewed changes

Switch to using discrete box constraints (testing)

77f0e1e

sea-bass commented Feb 17, 2023

View reviewed changes

sea-bass added 3 commits February 17, 2023 19:12

Small tweaks

7f0bc15

Revert changes that were not needed

a8b2527

Remove typo

aad0d17

github-actions bot added the stale Inactive issues and PRs are marked as stale and may be closed automatically. label Apr 4, 2023

tylerjw closed this Aug 23, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] Cartesian Tool Path Planning #1946

[WIP] Cartesian Tool Path Planning #1946

sea-bass commented Feb 13, 2023 •

edited

Loading

codecov bot commented Feb 13, 2023 •

edited

Loading

sea-bass commented Feb 15, 2023 •

edited

Loading

mamoll commented Feb 15, 2023

zkingston left a comment

sea-bass commented Feb 15, 2023

zkingston commented Feb 15, 2023

sea-bass commented Feb 15, 2023

zkingston commented Feb 15, 2023

sea-bass commented Feb 15, 2023

sea-bass commented Feb 17, 2023 •

edited

Loading

sea-bass Feb 17, 2023

github-actions bot commented Apr 4, 2023

tylerjw commented Aug 23, 2023

[WIP] Cartesian Tool Path Planning #1946

[WIP] Cartesian Tool Path Planning #1946

Conversation

sea-bass commented Feb 13, 2023 • edited Loading

Description

Checklist

codecov bot commented Feb 13, 2023 • edited Loading

Codecov Report

sea-bass commented Feb 15, 2023 • edited Loading

mamoll commented Feb 15, 2023

zkingston left a comment

Choose a reason for hiding this comment

sea-bass commented Feb 15, 2023

zkingston commented Feb 15, 2023

sea-bass commented Feb 15, 2023

zkingston commented Feb 15, 2023

sea-bass commented Feb 15, 2023

sea-bass commented Feb 17, 2023 • edited Loading

sea-bass Feb 17, 2023

Choose a reason for hiding this comment

github-actions bot commented Apr 4, 2023

tylerjw commented Aug 23, 2023

sea-bass commented Feb 13, 2023 •

edited

Loading

codecov bot commented Feb 13, 2023 •

edited

Loading

sea-bass commented Feb 15, 2023 •

edited

Loading

sea-bass commented Feb 17, 2023 •

edited

Loading