WIP: WMD 2021 Cartesian Planning #377

bostoncleek · 2021-03-10T16:09:49Z

This PR instantiates a BaseConstraint-derived class called, say,
ToolPathConstraint from a CartesianTrajectory.msg that represents a tool path.
The class finely interpolates points along the tool path and inserts these points in a nearest neighbor data structure.
For any random joint state, we can use FK to compute end effector pose and the nearest point on the tool path.

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

mamoll · 2021-03-10T17:11:28Z

Thanks for taking this on, @bostoncleek!

codecov · 2021-03-10T20:20:06Z

Codecov Report

Merging #377 (a26d13d) into main (e97122c) will decrease coverage by 0.92%.
The diff coverage is 0.00%.

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

@@            Coverage Diff             @@
##             main     #377      +/-   ##
==========================================
- Coverage   53.36%   52.45%   -0.91%     
==========================================
  Files         210      213       +3     
  Lines       18814    19136     +322     
==========================================
- Hits        10038    10035       -3     
- Misses       8776     9101     +325

Impacted Files	Coverage Δ
...lude/moveit/ompl_interface/detail/GreedyKCenters.h	`0.00% <0.00%> (ø)`
...de/moveit/ompl_interface/detail/NearestNeighbors.h	`0.00% <0.00%> (ø)`
...oveit/ompl_interface/detail/NearestNeighborsGNAT.h	`0.00% <0.00%> (ø)`
...de/moveit/ompl_interface/detail/ompl_constraints.h	`100.00% <ø> (ø)`
...mpl/ompl_interface/src/detail/ompl_constraints.cpp	`53.69% <ø> (-23.58%)`	⬇️
.../ompl_interface/src/detail/constrained_sampler.cpp	`42.86% <0.00%> (-17.14%)`	⬇️
... and 2 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update e97122c...0cb84e7. Read the comment docs.

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

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

mamoll · 2021-03-12T18:15:08Z

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

+  // TODO: Add path constraint function
+  // Ideal: F(q_actual) - F(q_neared_pose) = 0
+


Use forward kinematics on joint_values to compute EEF pose pose1

Use pose_nn_ to find pose pose2 that is nearest to pose1

Return poseDistance(pose1, pose2)

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

bostoncleek · 2021-03-15T19:19:47Z

@mamoll Thank you this sort of feedback is very beneficial.

gautz · 2021-03-18T14:28:16Z

If I understand well, your goal is to use the constrained planner to plan e.g. a spraying path? The constraints would be defined by discrete trajectory points? Do you plan to use the Cartesian Trajectory proposed interface? https://fzi-robot-interface-proposal.readthedocs.io/en/latest/proposal/index.html#tldr-proposed-interface

I am familiar with ompl_constraints.cpp but not with the classical cartesian planning. I never used the CartesianTrajectory.msg, thus it would be nice if you could explain the idea here..
Where would your input come from? A 3D path generated on a CAD file in a CAM system? In the form of linear, circular and spline segment?

I am working on possibly a similar problem, just want to avoid dual work.

bostoncleek · 2021-03-18T21:27:37Z

@gautz The idea is that the end effector would follow a path (spraying, welding, drawing, etc). The CartesianTrajectory.msg should contain a list of waypoints for the end effector to follow.

As of now the user must specify the waypoints. But as you can imagine the waypoints could come from a CAM system or another path generator. You could even run canny edge detection on an imagine and convert the pixels representing contours to Cartesian space. The robot would then ideally be able to draw the picture.

This PR instantiates a BaseConstraint-derived class called, say, ToolPathConstraint from a CartesianTrajectory.msg that represents a tool path. The class finely interpolates points along the tool path and inserts these points in a nearest neighbor data structure. For any random joint state, we can use FK to compute end effector pose and the nearest point on the tool path.

schornakj · 2021-04-12T14:54:12Z

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

+
+private:
+  // TODO: Why won't NearestNeighborsGNAT allow me to store pose as Eigen Transform


@bostoncleek What kind of error do you get when you try to use an Eigen Transform as the template type?

I was curious about this and it turns out the issue is there is no == operator implemented for these Eigen types:

/opt/ros/humble/include/ompl-1.6/ompl/datastructures/NearestNeighborsGNAT.h:551:102: error: no match for ‘operator==’ (operand types are ‘const Eigen::Transform<double, 3, 1>’ and ‘const Eigen::Transform<double, 3, 1>’) 551 | if (dist < nbh.top().first || (dist < std::numeric_limits<double>::epsilon() && data == key))

Can probably get around this with a thin wrapper struct or something

gautz · 2021-05-05T18:46:13Z

@bostoncleek any plans to implement a jacobian() with this generic Cartesian path approach? Or should the function() be sufficient for performance?

bostoncleek · 2021-05-05T19:12:16Z

@bostoncleek any plans to implement a jacobian() with this generic Cartesian path approach? Or should the function() be sufficient for performance?

Ompl can compose the derivative numerically. Of course the jacobian will allows provide an increase in performance.

mergify · 2021-11-19T11:06:40Z

This pull request is in conflict. Could you fix it @bostoncleek?

mergify · 2022-09-27T22:34:20Z

This pull request is in conflict. Could you fix it @bostoncleek?

tylerjw · 2022-11-15T16:22:47Z

@sea-bass this one, sorry about that other issue. Could you review this one and advise?

sea-bass

I think having a nearest neighbors based path constraint for planning is worthwhile to pull in.

However, my overarching comments to get this in would be:

Do we really need to copy-paste and modify 3 OMPL files headers here, or can we make the necessary changes to OMPL itself?
A lot of the distance functions are already available in Eigen, and since we're converting anyway we may as well just use Eigen instead of geometry_msgs.
The pose interpolation function does a lot of superfluous conversions from geometry_msgs to Eigen types, but combined with the above point, this could be easily addressed

And then, we want a compelling demo that maybe sets e.g., a circular path and allows users to plan within that constraint.

sea-bass · 2022-11-15T16:44:01Z

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

+                                          double step)
+{
+  // Pose interpolation
+  // https://answers.ros.org/question/299691/interpolation-for-between-two-poses/
+
+  const Eigen::Vector3d t1(pose1.position.x, pose1.position.y, pose1.position.z);
+  const Eigen::Vector3d t2(pose2.position.x, pose2.position.y, pose2.position.z);
+
+  const Eigen::Quaterniond q1(pose1.orientation.w, pose1.orientation.x, pose1.orientation.y, pose1.orientation.z);
+  const Eigen::Quaterniond q2(pose2.orientation.w, pose2.orientation.x, pose2.orientation.y, pose2.orientation.z);
+
+  const Eigen::Vector3d interp_translation = (1.0 - step) * t1 + step * t2;
+  const Eigen::Quaterniond interp_orientation = q1.slerp(step, q2);
+
+  geometry_msgs::msg::Pose interp_pose;
+  interp_pose.position.x = interp_translation.x();
+  interp_pose.position.y = interp_translation.y();
+  interp_pose.position.z = interp_translation.z();
+
+  interp_pose.orientation.w = interp_orientation.w();
+  interp_pose.orientation.x = interp_orientation.x();
+  interp_pose.orientation.y = interp_orientation.y();
+  interp_pose.orientation.z = interp_orientation.z();
+
+  return interp_pose;
+}


Right now we're converting poses to Eigen objects and back for each individual interpolation point which could definitely be made more efficient by at least getting all the interpolated poses between two poses in one shot.

sea-bass · 2022-11-15T16:45:24Z

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

Rename to interpolatePoses or something more "verby"

sea-bass · 2022-11-15T16:51:53Z

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

+{
+  const auto dx = pose1.position.x - pose2.position.x;
+  const auto dy = pose1.position.y - pose2.position.y;
+  const auto dz = pose1.position.z - pose2.position.z;
+  return std::sqrt(dx * dx + dy * dy + dz * dz);
+}
+
+double arcLength(const geometry_msgs::msg::Pose& pose1, const geometry_msgs::msg::Pose& pose2)
+{
+  // Distance between two quaternions
+  // https://math.stackexchange.com/questions/90081/quaternion-distance
+  // https://github.com/ompl/ompl/blob/main/src/ompl/base/spaces/src/SO3StateSpace.cpp#L254-L262
+  // dot product between q1 and q2
+  auto dq = std::fabs(pose1.orientation.w * pose2.orientation.w + pose1.orientation.x * pose2.orientation.x +
+                      pose1.orientation.y * pose2.orientation.y + pose1.orientation.z * pose2.orientation.z);
+
+  if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
+  {
+    return 0.0;
+  }
+
+  return std::acos(dq);
+}


If we can convert to Eigen data types, there already are functions to do this. See e.g. in cartesian_interpolator.cpp:

double rotation_distance = start_quaternion.angularDistance(target_quaternion); double translation_distance = (rotated_target.translation() - start_pose.translation()).norm();

So if we're doing Eigen conversions anyway in this function, we could use these functions "for free" and have the nearest neighbor use Eigen::Isometry3d instead of geometry_msgs::msg::Pose?

sea-bass · 2022-11-15T16:52:58Z

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

+
+/* Author: Mark Moll, Bryant Gipson */
+
+// This file is a slightly modified version of <ompl/datastructures/NearestNeighborsGNAT.h>


So can we instead update OMPL instead of copy-pasting stuff here?

sea-bass · 2022-11-18T14:54:28Z

By request, I'm closing this PR and capturing that we should pick up this work in a new issue : #1740

bostoncleek mentioned this pull request Mar 10, 2021

World MoveIt Day 2021 Tracks #363

Closed

2 tasks

bostoncleek added help wanted Extra attention is needed enhancement New feature or request labels Mar 11, 2021

mamoll reviewed Mar 12, 2021

View reviewed changes

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

mamoll reviewed Mar 12, 2021

View reviewed changes

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

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

schornakj reviewed Apr 12, 2021

View reviewed changes

gautz mentioned this pull request May 20, 2021

Support FollowCartesianTrajectory moveit/moveit#2675

Open

sea-bass reviewed Nov 15, 2022

View reviewed changes

tylerjw assigned sea-bass Nov 17, 2022

sea-bass mentioned this pull request Nov 18, 2022

Finish Tool Path Constraint Implementation #1740

Open

sea-bass closed this Nov 18, 2022

sea-bass mentioned this pull request Feb 13, 2023

[WIP] Cartesian Tool Path Planning #1946

Closed

6 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

WIP: WMD 2021 Cartesian Planning #377

WIP: WMD 2021 Cartesian Planning #377

bostoncleek commented Mar 10, 2021

mamoll commented Mar 10, 2021

codecov bot commented Mar 10, 2021 •

edited

Loading

mamoll Mar 12, 2021

bostoncleek commented Mar 15, 2021

gautz commented Mar 18, 2021

bostoncleek commented Mar 18, 2021

schornakj Apr 12, 2021

sea-bass Feb 13, 2023 •

edited

Loading

gautz commented May 5, 2021

bostoncleek commented May 5, 2021

mergify bot commented Nov 19, 2021

mergify bot commented Sep 27, 2022

tylerjw commented Nov 15, 2022

sea-bass left a comment

sea-bass Nov 15, 2022

sea-bass Nov 15, 2022

sea-bass Nov 15, 2022

sea-bass Nov 15, 2022

sea-bass commented Nov 18, 2022

		// TODO: Add path constraint function
		// Ideal: F(q_actual) - F(q_neared_pose) = 0


		private:
		// TODO: Why won't NearestNeighborsGNAT allow me to store pose as Eigen Transform


		/* Author: Mark Moll, Bryant Gipson */

		// This file is a slightly modified version of <ompl/datastructures/NearestNeighborsGNAT.h>

WIP: WMD 2021 Cartesian Planning #377

WIP: WMD 2021 Cartesian Planning #377

Conversation

bostoncleek commented Mar 10, 2021

Checklist

mamoll commented Mar 10, 2021

codecov bot commented Mar 10, 2021 • edited Loading

Codecov Report

mamoll Mar 12, 2021

Choose a reason for hiding this comment

bostoncleek commented Mar 15, 2021

gautz commented Mar 18, 2021

bostoncleek commented Mar 18, 2021

schornakj Apr 12, 2021

Choose a reason for hiding this comment

sea-bass Feb 13, 2023 • edited Loading

Choose a reason for hiding this comment

gautz commented May 5, 2021

bostoncleek commented May 5, 2021

mergify bot commented Nov 19, 2021

mergify bot commented Sep 27, 2022

tylerjw commented Nov 15, 2022

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass Nov 15, 2022

Choose a reason for hiding this comment

sea-bass Nov 15, 2022

Choose a reason for hiding this comment

sea-bass Nov 15, 2022

Choose a reason for hiding this comment

sea-bass Nov 15, 2022

Choose a reason for hiding this comment

sea-bass commented Nov 18, 2022

codecov bot commented Mar 10, 2021 •

edited

Loading

sea-bass Feb 13, 2023 •

edited

Loading