Fix orientation of subframe offset before applying to motion plans by riv-tnoble · Pull Request #2890 · moveit/moveit

riv-tnoble · 2021-09-29T16:07:07Z

Description

This PR fixes #2879 by reorienting the subframe offset applied to a goal pose in the PTP planner, so that it is applied in the correct direction. It also adds the same offset computation to LIN and CIRC motions, allowing them to work with subframes.

The video below demonstrates the fix by showing that the goal point planned to by pilz PTP is now the same as OMPL.

pilz_subframes_fix.mp4

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference (Not applicable)
Document API changes relevant to the user in the MIGRATION.md notes (Not applicable)
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI (Not applicable)
While waiting for someone to review your request, please help review another open pull request to support the maintainers

…d subframe offset to LIN and CIRC motions

codecov · 2021-09-29T18:28:52Z

Codecov Report

Merging #2890 (a58b15c) into master (a0ee202) will increase coverage by 0.04%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #2890      +/-   ##
==========================================
+ Coverage   61.31%   61.35%   +0.04%     
==========================================
  Files         373      373              
  Lines       31742    31747       +5     
==========================================
+ Hits        19461    19476      +15     
+ Misses      12281    12271      -10

Impacted Files	Coverage Δ
...strial_motion_planner/src/trajectory_functions.cpp	`100.00% <100.00%> (ø)`
...l_motion_planner/src/trajectory_generator_circ.cpp	`100.00% <100.00%> (ø)`
...al_motion_planner/src/trajectory_generator_lin.cpp	`100.00% <100.00%> (ø)`
...al_motion_planner/src/trajectory_generator_ptp.cpp	`98.92% <100.00%> (-0.07%)`	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	`79.23% <0.00%> (+0.30%)`	⬆️
...meterization/work_space/pose_model_state_space.cpp	`83.65% <0.00%> (+0.63%)`	⬆️
...g_scene_interface/src/planning_scene_interface.cpp	`49.72% <0.00%> (+1.16%)`	⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp	`59.46% <0.00%> (+16.22%)`	⬆️

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 a0ee202...a58b15c. Read the comment docs.

simonschmeisser · 2021-09-29T19:31:25Z

Could those calculations be moved to a common location? Are they maybe even useful outside of pilz?

riv-rbush · 2021-09-30T09:17:25Z

@simonschmeisser While they are probably useful, those calculations are a specific fix for this pilz bug and this bug only. I gave @aa-tom the mandate to fix this bug and not add enhancements to the MoveIt! codebase. We'd appreciate some help from the MoveIt! community if your suggestion is desired, otherwise it would be good to merge this before the 1.1.6 MoveIt! release.

riv-rbush · 2021-09-30T09:18:40Z

Also fixes #2884

riv-tnoble · 2021-09-30T10:03:30Z

Agree that it'd be nice to make this code common if possible. The most reasonable place seems like the ResolveConstraintFrames plugin, which calculates the subframe offsets before the plan request reaches the planner. I have a couple of concerns though:

I'm not too familiar with the plugin, so would want to make sure that there isn't a specific reason why the offsets aren't already being oriented using the goal pose there. @rhaschke could you weigh in here? I think you originally developed the ResolveConstraintFrames plugin?
I think making the change in the plugin could have unintended effects in other planners. OMPL was never broken, which suggests to me that there's already some kind of offset computation going on in there. I'm worried that, without a thorough investigation of which planners would need to change in response to putting the offset computation in the plugin, we could end up breaking more plans than we fix. Maybe worth its own issue though?

jschleicher

Thank you very much for your investigation!

Other planners rely on the KinematicConstraint classes, which are probably designed for sampling. This is a fundamental differnce of OMPL - here we just use a single constraint. I would prefer a more general approach, but currently am not aware of a simple solution.

To avoid code-duplication, you could still move the offset part into trajectory_functions.cpp or similar.

Note that for a circle, you probably also have to add the offset to the circ_path_point (interim or center): https://github.com/ros-planning/moveit/blob/049be73b8343d67ffde400fed9ed4c387c6ea9e5/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp#L183

jschleicher · 2021-09-30T15:12:16Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp

+    Eigen::Vector3d offset_in_world;
+    tf2::fromMsg(req.goal_constraints.at(0).position_constraints.at(0).target_point_offset, offset_in_world);
+    Eigen::Quaterniond goal_orientation;
+    tf2::fromMsg(req.goal_constraints.at(0).orientation_constraints.at(0).orientation, goal_orientation);
+    Eigen::Vector3d offset_in_goal = goal_orientation * offset_in_world;
+    p.x -= offset_in_goal[0];
+    p.y -= offset_in_goal[1];
+    p.z -= offset_in_goal[2];


Would a calculation directly with Eigen types give the same results? Would look a bit more readable to me:

Suggested change

Eigen::Vector3d offset_in_world;

tf2::fromMsg(req.goal_constraints.at(0).position_constraints.at(0).target_point_offset, offset_in_world);

Eigen::Quaterniond goal_orientation;

tf2::fromMsg(req.goal_constraints.at(0).orientation_constraints.at(0).orientation, goal_orientation);

Eigen::Vector3d offset_in_goal = goal_orientation * offset_in_world;

p.x -= offset_in_goal[0];

p.y -= offset_in_goal[1];

p.z -= offset_in_goal[2];

geometry_msgs::Pose pose;

pose.position =

req.goal_constraints.at(0).position_constraints.at(0).constraint_region.primitive_poses.at(0).position;

pose.orientation = req.goal_constraints.at(0).orientation_constraints.at(0).orientation;

normalizeQuaternion(pose.orientation);

Eigen::Isometry3d pose_eigen;

tf2::fromMsg(pose, pose_eigen);

Eigen::Isometry3d offset_in_world;

tf2::fromMsg(req.goal_constraints.at(0).position_constraints.at(0).target_point_offset, offset_in_world);

pose_eigen.linear() -= offset_in_world * pose_eigen.rotation();

Thanks for the advice! Will get those tasks done tomorrow:

Move common offset transformation into trajectory_functions.cpp

Apply offset to center for CIRC motions

Tidy up Eigen computations

…path constraints of CIRC motions

riv-tnoble · 2021-10-04T06:45:55Z

@jschleicher @rhaschke @v4hn Any ideas why the build might be failing?

riv-tnoble · 2021-10-04T06:52:50Z

Looks like the Windows build failure is the same as this #2892 (comment)

Unsure why so many tests would be failing provided they aren't all using offsets and aren't checking for specific end poses

v4hn · 2021-10-04T09:01:22Z

Unsure why so many tests would be failing provided they aren't all using offsets and aren't checking for specific end poses

I just triggered re-runs to rule out a system issue, but you probably broke something and will have to look into it. :) Just run the specific failing tests locally to debug.

v4hn · 2021-10-04T09:03:10Z

...pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h

+ * @param constraint
+ * @return
+ */
+Eigen::Isometry3d getConstraintPose(moveit_msgs::Constraints constraint);


Suggested change

Eigen::Isometry3d getConstraintPose(moveit_msgs::Constraints constraint);

Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& constraint);

jschleicher · 2021-10-04T11:03:05Z

@aa-tom You can run the tests locally with more output using for example rostest --text pilz_industrial_motion_planner integrationtest_command_planning_frankaemika_panda.test
Which gives an output of

[ INFO] [1633345214.152003125]: Received new planning service request...
[ INFO] [1633345214.152090320]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead.
[ INFO] [1633345214.152116846]: Generating CIRC trajectory...
[ERROR] [1633345214.152365149]: Exception caught: 'vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)'

so maybe some check for empty constraint lists got lost?

riv-tnoble · 2021-10-04T13:00:36Z

Thanks. Will run the tests locally and take a look this afternoon

riv-tnoble · 2021-10-21T01:20:11Z

Sorry for the delay! I believe I've fixed the tests now (locally, at least). The issue was caused by trying to access the goal's position constraints when applying the offset to the circle interim/center as some tests use goals with no position constraints. I have a couple of questions I was hoping you could answer @jschleicher :

Is is normal for a goal to have no position constraints? My best guess is that the goal can be specified entirely in joint constraints instead?
Assuming the above is true, I'm guessing there's no way we can apply an offset, since it's associated with a position constraint to begin with. Is that correct?

riv-tnoble · 2021-10-21T07:43:05Z

Looks like the Windows build failed. ~~Would it be be possible to rerun the pipeline too?~~
Scratch that, needed to sync with master anyway 😁

…ame-fix-pr

v4hn · 2021-10-21T08:31:43Z

Is is normal for a goal to have no position constraints? My best guess is that the goal can be specified entirely in joint constraints instead?

It's not normal, but it's supported throughout the pipeline and should not break anything.
But yes, in that case you can't (and don't need to) associate a Cartesian offset with the goal.

Looks like the Windows build failed

Yes, please ignore it, RoboStack has trouble again.

jschleicher

Great to see this progress! Approving 👍

v4hn

I do believe you fixed the issue with this patch at your end, but do we actually have a test for your use setup? Not saying I insist on writing one to merge this fix.

Please consider my inline feedback though.

v4hn · 2021-10-22T11:51:16Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

+                                    const geometry_msgs::Quaternion& orientation)
+{
+  geometry_msgs::Pose pose;
+  pose.position = constraint.position_constraints.front().constraint_region.primitive_poses.front().position;


This is the only line where you use constraint and passing the whole constraint object makes the other two parameters redundant in two out of three calls (Is the CIRC call passing path_constraints actually correct?)

Please either pass only a Constraints message *and possibly create one for the CIRC call) or change the first parameter to a Vector3, so you can unpack the Constraints message before the call.

I addressed this feedback in rivelinrobotics#5. Please merge this and provide unit tests. Thanks!

riv-tnoble · 2021-10-22T17:41:58Z

I do believe you fixed the issue with this patch at your end, but do we actually have a test for your use setup? Not saying I insist on writing one to merge this fix.

No automated tests currently, but I'm happy to write them. I'm thinking perhaps a unit test for each planner inspecting the MotionPlanInfo object and an integration test for each verifying the motion succeeds would be sufficient?

This is the only line where you use constraint and passing the whole constraint object makes the other two parameters redundant in two out of three calls (Is the CIRC call passing path_constraints actually correct?)

I believe the CIRC call is correct as we want to apply the target offset to the interim / center point in addition to the goal point, though I've only tested the fix manually using PTP, so an automated test would be good in that regard.

Please either pass only a Constraints message *and possibly create one for the CIRC call) or change the first parameter to a Vector3, so you can unpack the Constraints message before the call.

Shall do. I'll change the parameter to Vector3

rhaschke · 2021-11-02T14:48:52Z

@rivelin-tom, what is your progress on #2890 (comment)? This PR is currently blocking the next Noetic release as your colleague? asked for merging this before: #2890 (comment)

rhaschke · 2021-11-05T00:39:55Z

Ping @rivelin-tom.

Fix moveit#2879 by reorienting the subframe offset applied to a goal pose in the PTP planner, so that it is applied in the correct direction. Also add the same offset computation to LIN and CIRC generators, thus allowing them to work with subframes. Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

simonschmeisser · 2021-11-06T09:07:45Z

@rhaschke should this be closed since you merged it manually?

rhaschke · 2021-11-06T14:46:05Z

Should this be closed since you merged it manually?

No, I'm still waiting for unit tests to be added... Unfortunately, there was no activity from @rivelin-tom since 15 days.
I took the time to fix the code as suggested, merge the bug fix, and thus unblock the release.

riv-rbush · 2021-11-08T08:56:17Z

@rhaschke Apologies for the delay but @rivelin-tom has had to implement some features and enhancements for us. We've not forgotten about it, just had some urgent and critical work to get out the way first.

riv-tnoble · 2021-11-11T11:47:46Z

@rhaschke Thanks a bunch for getting it ready for the release and apologies for the delay. I'm super busy at the moment, so it'll be a little while before I'm able to pick this back up and write the tests. I do intend to write them!

gautz · 2021-11-16T16:26:54Z

Is this replaced by e30c6fa?
Should this PR be further extended to correctly transform the CIRC interim frame when setIKframe() is set to an attached object subframe in MTC ? See the issue described here
.

see also moveit/moveit_task_constructor#191 and moveit/moveit_task_constructor#304

v4hn · 2021-12-15T09:45:06Z

@rivelin-tom Is it still realistic to wait for your tests here? I know you want to do them, but you are obviously busy with other things.

Should this PR be further extended

No, feel free to open separate PRs if you have patches.

riv-tnoble · 2021-12-17T15:24:51Z

@v4hn I think it'd be best to close this off for now to avoid delaying longer. I believe it was already merged with master as part of the 1.1.6 release? Happy for this PR to simply be closed if that's the case

rhaschke · 2021-12-17T15:56:26Z

Yes, the fix was already merged. However, this PR is still open as a reminder that you, @rivelin-tom, promised to provide some unit tests. Of course, you can close here and open a new PR for those tests.

riv-tnoble · 2022-01-04T09:39:42Z

Sorry, was away for Christmas. I will do that, then. Thanks for the help implementing the changes 👍

Partially reverts changes of #2890

Partially reverts changes of moveit#2890

…3523) * Revert consideration of link offset in start/goal poses (introduced in #2890) * Consider link offset in IK computation instead

Fix orientation of subframe offset before applying to PTP motions. Ad…

1685d95

…d subframe offset to LIN and CIRC motions

riv-tnoble requested a review from jschleicher as a code owner September 29, 2021 16:07

jschleicher reviewed Sep 30, 2021

View reviewed changes

Pull offset computation into common helper function. Apply offset to …

70a7f8c

…path constraints of CIRC motions

riv-tnoble requested review from rhaschke and v4hn as code owners October 1, 2021 14:36

Tom Noble added 5 commits October 1, 2021 16:29

Apply formatting

a1381ba

Apply formatting

1ff01e7

Apply formatting

18d22ca

Remove .idea directory

1c87f75

Remove redundant directories

ea23029

v4hn reviewed Oct 4, 2021

View reviewed changes

Tom Noble added 7 commits October 4, 2021 14:21

Change getConstraintPose() to use const reference

876366d

Comment transformation of CIRC path point while debugging

977ff89

Fix formatting

ea9b897

Modify CIRC planner to transform path constraint correctly

347a59e

Fix typos

7c4a2db

Apply formatting

095a0c0

Handle case where goal has no position constraints

40e7179

riv-tnoble requested a review from jschleicher October 21, 2021 01:14

Merge remote-tracking branch 'upstream/master' into bugfix/pilz-subfr…

a58b15c

…ame-fix-pr

riv-tnoble requested a review from v4hn October 21, 2021 14:22

jschleicher approved these changes Oct 22, 2021

View reviewed changes

v4hn reviewed Oct 22, 2021

View reviewed changes

v4hn mentioned this pull request Oct 22, 2021

Noetic release 1.1.6 #2886

Closed

riv-tnoble changed the title ~~Fix orientation of subframe offset before applying to PTP motions. Ad…~~ Fix orientation of subframe offset before applying to motion plans Oct 25, 2021

rhaschke mentioned this pull request Nov 6, 2021

Cleanup + Simplify rivelinrobotics/rivelin_moveit#5

Closed

gautz mentioned this pull request Nov 16, 2021

Path constraints not working with attached body #2250

Closed

riv-tnoble closed this Jan 4, 2022

riv-tnoble deleted the bugfix/pilz-subframe-fix-pr branch November 9, 2023 08:11

rhaschke mentioned this pull request Nov 10, 2023

Fix pilz planners to consider and plan Cartesian motions w.r.t. subframes #3519

Merged

6 tasks

rhaschke added a commit that referenced this pull request Nov 11, 2023

Revert consideration of link offset in start/goal poses

740a582

Partially reverts changes of #2890

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Nov 11, 2023

Revert consideration of link offset in start/goal poses

dc51807

Partially reverts changes of moveit#2890

rhaschke added a commit that referenced this pull request Nov 12, 2023

Fix Pilz planner: consider link_name's offset during IK computation (#…

942abef

…3523) * Revert consideration of link offset in start/goal poses (introduced in #2890) * Consider link offset in IK computation instead

	Eigen::Isometry3d getConstraintPose(moveit_msgs::Constraints constraint);
	Eigen::Isometry3d getConstraintPose(const moveit_msgs::Constraints& constraint);

Conversation

riv-tnoble commented Sep 29, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Checklist

Uh oh!

codecov bot commented Sep 29, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

simonschmeisser commented Sep 29, 2021

Uh oh!

riv-rbush commented Sep 30, 2021

Uh oh!

riv-rbush commented Sep 30, 2021

Uh oh!

riv-tnoble commented Sep 30, 2021

Uh oh!

jschleicher 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!

jschleicher Sep 30, 2021

Choose a reason for hiding this comment

Uh oh!

riv-tnoble Sep 30, 2021 • 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!

riv-tnoble commented Oct 4, 2021

Uh oh!

riv-tnoble commented Oct 4, 2021

Uh oh!

v4hn commented Oct 4, 2021 via email

Uh oh!

v4hn Oct 4, 2021

Choose a reason for hiding this comment

Uh oh!

riv-tnoble Oct 4, 2021

Choose a reason for hiding this comment

Uh oh!

jschleicher commented Oct 4, 2021

Uh oh!

riv-tnoble commented Oct 4, 2021

Uh oh!

riv-tnoble commented Oct 21, 2021

Uh oh!

riv-tnoble commented Oct 21, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

v4hn commented Oct 21, 2021

Uh oh!

jschleicher left a comment

Choose a reason for hiding this comment

Uh oh!

v4hn left a comment

Choose a reason for hiding this comment

Uh oh!

v4hn Oct 22, 2021

Choose a reason for hiding this comment

Uh oh!

rhaschke Nov 6, 2021

Choose a reason for hiding this comment

Uh oh!

riv-tnoble commented Oct 22, 2021 • edited by rhaschke Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rhaschke commented Nov 2, 2021

Uh oh!

rhaschke commented Nov 5, 2021

Uh oh!

simonschmeisser commented Nov 6, 2021

Uh oh!

rhaschke commented Nov 6, 2021

Uh oh!

riv-rbush commented Nov 8, 2021

Uh oh!

riv-tnoble commented Nov 11, 2021

Uh oh!

gautz commented Nov 16, 2021

riv-tnoble commented Sep 29, 2021 •

edited

Loading

codecov bot commented Sep 29, 2021 •

edited

Loading

jschleicher left a comment •

edited

Loading

riv-tnoble Sep 30, 2021 •

edited

Loading

riv-tnoble commented Oct 21, 2021 •

edited

Loading

riv-tnoble commented Oct 22, 2021 •

edited by rhaschke

Loading