Tranform Pilz goal pose and center point wrt. the planning frame #3522

captain-yoshi · 2023-11-06T15:12:09Z

Description

Should fix #3035 (partially) and moveit/moveit_task_constructor/issues/411 by transforming the goal pose (CIRC, LIN and PTP) and the center point (CIRC) wrt. the planning frame.

Only computes the IK when there is a goal pose constraint. Removed unused frame_id initialisation. Transforms the goal pose for CIRC, LIN and PTP wrt. the planning frame. Transform the center point for CIRC wrt. the planning frame.

Rebased on #3519.

TODO

Fix CIRC
Fix LIN and PTP
Add unit tests
Update moveit_tutorials doc for goals in cartesian space as well as interim/center point -> all frames are resolved with the start pose

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

This post has been edited prior discussion with the maintainers

captain-yoshi · 2023-11-06T16:47:57Z

The current unit test uses the prbt robot from moveit_resources. To demonstrate some of my fixes, I need one more fixed link between the robot base and the prbt_link_1 that is different then the identity matrix.

I can't seem to add/modify links programatically with the robot model. Should I add an alternate prbt_macro.xacro with another link in the moveit_resources ?

codecov · 2023-11-06T17:08:05Z

Codecov Report

Attention: 1 lines in your changes are missing coverage. Please review.

Comparison is base (c404ca2) 61.84% compared to head (8dbcc82) 61.87%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3522      +/-   ##
==========================================
+ Coverage   61.84%   61.87%   +0.03%     
==========================================
  Files         385      385              
  Lines       34091    34107      +16     
==========================================
+ Hits        21079    21099      +20     
+ Misses      13012    13008       -4

Files	Coverage Δ
...al_motion_planner/src/trajectory_generator_lin.cpp	`100.00% <100.00%> (ø)`
...al_motion_planner/src/trajectory_generator_ptp.cpp	`98.95% <100.00%> (+0.12%)`	⬆️
...l_motion_planner/src/trajectory_generator_circ.cpp	`99.03% <94.74%> (-0.97%)`	⬇️

... and 3 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

rhaschke

Thanks for working on this. As you rely on #3519, could you please rebase onto that PR and further minimize the change set? There are a lot of code shifts, which distract from the actual contribution.

The frame_id must be rigidly connected to the root link. There is no sense to use a frame that is not yet known for a goal pose constraint.

I don't agree with this. Of course, the frame needs to be known, but it could be any known frame. For example, why shouldn't it be possible to specify the circular motion w.r.t. the current end-effector pose? Surely, this needs to be transformed into planning frame.

I need one more fixed link between the robot base and the prbt_link_1. But I can't seem to add links programatically with the robot model.

What about adding a sub frame instead and using that one as frame_id?

I changed the status of that PR to draft as this is obviously still work-in-progress.

rhaschke · 2023-11-08T09:34:59Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

+  assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
+  for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
+  {
+    auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) };
+    if (it == req.start_state.joint_state.name.cend())
+    {
+      std::ostringstream os;
+      os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name
+         << "\" in start state of request";
+      throw CircJointMissingInStartState(os.str());
+    }
+    size_t index = it - req.start_state.joint_state.name.cbegin();
+    info.start_joint_position[joint_name] = req.start_state.joint_state.position[index];
+  }


This code block was removed in #3519. To which extend do you still need that?

rhaschke · 2023-11-08T09:38:47Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

+    const auto link = robot_model_->getLinkModel(frame_id);
+    if (link)
    {
+      // check if robot link is rigidly connected to the root link
+      const auto parent_link = robot_model_->getRigidlyConnectedParentLinkModel(link);


There is also:
https://github.com/ros-planning/moveit/blob/167a62176b69a3afe102a2c9696aaf20225c52e4/moveit_core/robot_state/include/moveit/robot_state/robot_state.h#L1389-L1390

which allows to combine these two cases.

captain-yoshi · 2023-11-08T16:46:02Z

Will rebase and minimize the changes.

I don't agree with this. Of course, the frame needs to be known, but it could be any known frame. For example, why shouldn't it be possible to specify the circular motion w.r.t. the current end-effector pose? Surely, this needs to be transformed into planning frame.

You are right. We should allow every frame_id (robot link, collision object/subframes) that is:

rigidly connected to the root link
wrt. the link_name of the constraint and everything up the chain that is attached to a fixed joint

Can someone plan something with a pure position constraint, e.g. the orientation could change, or is the orientation set to identity when missing ?

What about adding a sub frame instead and using that one as frame_id ?

This would work, but there would be one test missing that checks explicitly if the frame is a link. Maybe I can simplify these checks. Nonetheless I will start testing with sub-frames.

rhaschke · 2023-11-08T20:34:05Z

We should allow every frame_id that is ... rigidly connected to the root link.

There is no need to constrain to frames that are (rigidly) connected to the root link.

Can we plan with a pure position constraint?

Sure. Orientation of the eef is arbitrary in that case.

There would be one test missing that checks explicitly if the frame is a link.

There is no need to test that explicitly. The planning code doesn't differ if the frame_id refers to a link or a sub frame.

rhaschke · 2023-11-08T20:40:34Z

Can we plan with a pure position constraint?

Sure. Orientation of the eef is arbitrary in that case.

You were probably asking that question in the context of the Pilz planner. To be honest, there I don't know.
My answer was for OMPL.

captain-yoshi · 2023-11-08T20:46:41Z

Here is my understanding of valid frames versus constraint link names:

Depending of the type of joint, there could be more valid frames, e.g for the link_name Link TCP one could use the Link3/2 for frame_id and it would be valid if J3 was revolute. I don't think this is necessary...

And considering the case were orientation is not constrained, that means that a link_name with Link TCP and a frame_id of Link TCP would be correct, but a frame_id with Bottle or Bottle/Cap would not be valid because of the IK (only constrained for position).

captain-yoshi · 2023-11-08T21:24:49Z

We can also just transform the goal pose wrt. the planning frame, without checking if the frame is valid (known in advanced). It would be up to the user to enter a valid frame_id.

rhaschke · 2023-11-08T21:27:47Z

I still don't get why you want to constrain possible frame_id's. The frame_id just defines the frame w.r.t. which the start, end, and center point of a CIRC motion are interpreted. As long as the frame is known (and somehow linked to the robot's TF tree), we can use the current pose of that frame to transform those points into the planning frame. From there on, the CIRC path in space is fixed and well-defined w.r.t. the planning frame.

captain-yoshi · 2023-11-09T04:55:25Z

Sorry, I wrongfully though that we had to use the goal pose state for the goal pose constraint (which some frames are not known in advance) instead of the current pose which are known in advance.

rhaschke

I merged #3519. Could you please rebase on master again?
I shortly looked into your changes as well. As far as I understand, the main culprit is that the start pose is computed w.r.t. the robot's model frame (via FK) while the goal pose, computed via getConstraintPose(), is local w.r.t. to frame_id (which often/in the tests? coincides with the former).
It would be great, if you could add tests. Specifying the goal pose w.rt. to any frame != model frame should reveal the issue.

captain-yoshi · 2023-11-10T03:50:11Z

Rebased. Exactly, the tests use the planning frame as the frame_id. Will add tests tomorrow or this weekend.

captain-yoshi · 2023-11-10T22:43:19Z

Had a hard time finding that the prbt_tcp link was actually different for the 2 robot models: default and with the pg70 gripper.

The test code for computing CIRC center point has the same problem, it does not transform the pose wrt. the frame_id. Can I use the same code (fixed in 846f611) to compute the center point in the test or should I use a different method ? Obviously, if there is an error, the test will not be able to catch it.

Just have to fix one final error with my new test: testutils::isGoalReached. I suspect that it does not correctly transforms the goal pose wrt. the planning frame.

EDIT

Confirmed, at least there was a TODO in the testutils !

rhaschke · 2023-11-11T09:50:02Z

Can I use the same code (fixed in 846f611) to compute the center point in the test or should I use a different method?

I think it is fine using the same approach. Your main issue was that start and goal were computed w.r.t. different frames and this will be catched, right?
Is the PR ready for review from your point of view? I will have a look into it later this weekend.

rhaschke · 2023-11-11T23:38:27Z

@captain-yoshi, although I merged this PR already, please don't hesitate to add your unit tests.
I added some more fixes in #3523.

captain-yoshi requested a review from jschleicher as a code owner November 6, 2023 15:12

captain-yoshi force-pushed the fix-pilz-center-point-tf branch from 4a22b71 to 2ef65b5 Compare November 8, 2023 01:31

rhaschke marked this pull request as draft November 8, 2023 09:51

rhaschke reviewed Nov 8, 2023

View reviewed changes

captain-yoshi force-pushed the fix-pilz-center-point-tf branch from 2ef65b5 to 5d5a81b Compare November 9, 2023 19:05

rhaschke reviewed Nov 10, 2023

View reviewed changes

captain-yoshi added 4 commits November 9, 2023 22:26

[fix] only compute ik for cartesian goal constraints

d30354d

[fix] transform center point wrt. the planning frame

846f611

[fix] transform goal pose wrt. the planning frame

733f135

[cleanup] remove unnecessary initialization

8dbcc82

captain-yoshi force-pushed the fix-pilz-center-point-tf branch from 5d5a81b to 8dbcc82 Compare November 10, 2023 03:28

rhaschke marked this pull request as ready for review November 11, 2023 22:57

rhaschke merged commit ea9a0d3 into moveit:master Nov 11, 2023
7 checks passed

captain-yoshi mentioned this pull request Nov 12, 2023

Fix Pilz unit tests: correctly transforms the goal pose and center point wrt. the planning frame #3524

Closed

6 tasks

rhaschke mentioned this pull request Mar 30, 2024

Different frame_id when calculating Pilz CIRC distance between start-center and goal-center moveit/moveit_task_constructor#411

Closed

Danilrivero mentioned this pull request May 28, 2024

PILZ Industrial motion planner is ignoring the goal frame_id passed moveit/moveit2#2857

Closed

rr-tom-noble mentioned this pull request Nov 8, 2024

Enhancement/ports moveit 3522 moveit/moveit2#3070

Merged

6 tasks

This was referenced Nov 8, 2024

Enhancement/ports moveit 3522 (backport #3070) moveit/moveit2#3074

Merged

Enhancement/ports moveit 3522 (backport #3070) moveit/moveit2#3075

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Tranform Pilz goal pose and center point wrt. the planning frame #3522

Tranform Pilz goal pose and center point wrt. the planning frame #3522

captain-yoshi commented Nov 6, 2023 •

edited

Loading

captain-yoshi commented Nov 6, 2023

codecov bot commented Nov 6, 2023 •

edited

Loading

rhaschke left a comment

rhaschke Nov 8, 2023

rhaschke Nov 8, 2023

captain-yoshi commented Nov 8, 2023

rhaschke commented Nov 8, 2023

rhaschke commented Nov 8, 2023

captain-yoshi commented Nov 8, 2023 •

edited

Loading

captain-yoshi commented Nov 8, 2023

rhaschke commented Nov 8, 2023

captain-yoshi commented Nov 9, 2023

rhaschke left a comment

captain-yoshi commented Nov 10, 2023

captain-yoshi commented Nov 10, 2023 •

edited

Loading

rhaschke commented Nov 11, 2023

rhaschke commented Nov 11, 2023

Tranform Pilz goal pose and center point wrt. the planning frame #3522

Tranform Pilz goal pose and center point wrt. the planning frame #3522

Conversation

captain-yoshi commented Nov 6, 2023 • edited Loading

Description

Checklist

captain-yoshi commented Nov 6, 2023

codecov bot commented Nov 6, 2023 • edited Loading

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Nov 8, 2023

Choose a reason for hiding this comment

rhaschke Nov 8, 2023

Choose a reason for hiding this comment

captain-yoshi commented Nov 8, 2023

rhaschke commented Nov 8, 2023

rhaschke commented Nov 8, 2023

captain-yoshi commented Nov 8, 2023 • edited Loading

captain-yoshi commented Nov 8, 2023

rhaschke commented Nov 8, 2023

captain-yoshi commented Nov 9, 2023

rhaschke left a comment

Choose a reason for hiding this comment

captain-yoshi commented Nov 10, 2023

captain-yoshi commented Nov 10, 2023 • edited Loading

rhaschke commented Nov 11, 2023

rhaschke commented Nov 11, 2023

captain-yoshi commented Nov 6, 2023 •

edited

Loading

codecov bot commented Nov 6, 2023 •

edited

Loading

captain-yoshi commented Nov 8, 2023 •

edited

Loading

captain-yoshi commented Nov 10, 2023 •

edited

Loading