Fix pilz planners to consider and plan Cartesian motions w.r.t. subframes by riv-tnoble · Pull Request #3519 · moveit/moveit

riv-tnoble · 2023-10-30T12:07:36Z

Description

Fixes #3517 by allowing pilz to use subframes in goal constraints.

Updates computePoseIK() to build a RobotState from the PlanningScene rather than the RobotModel, so that subframes are taken into account. Also adds some error handling logic around calls to this function as, currently, if the passed link_name does not exist, the start_pose is set to the identity pose, which leads to a less intuitive IK solver error later on.

This also means the collision checking in the pilz computePoseIK() method is now attached collision object aware.

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

…subframe

rhaschke

Generally, I think the approach is correct. In order to yield interpolation w.r.t. the subframe, the frame needs to be resolved in the Pilz planner and not only in setFromIK.

moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

rhaschke · 2023-10-30T12:21:09Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp

+  // if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
+  // {
+  //   std::ostringstream os;
+  //   os << "No IK solver available for link: \"" << pos_constraint.link_name << "\"";
+  //   throw NoIKSolverAvailable(os.str());
+  // }


If not needed anymore: remove completely. Leave a comment here, why it's not needed anymore.
I guess it is redundant to similar checks elsewhere?

But: Instead of doing the error checking and reporting frequently in downstream trajectory generators, why not do it once here?

Will double check that this is redundant. Do you think it'd make sense to move the transformation into a tip goal here too?

Do you think it'd make sense to move the transformation into a tip goal here too?

Scratch that part given the transformation is no longer necessary (see reply from above comment)

@rhaschke What would be the easiest way of turning this check into one that can detect subframes? In other places, I've been using RobotState::knowsFrameTransform().

The method calls are:

TrajectoryGenerator::generate() -> TrajectoryGenerator::validateRequest() -> TrajectoryGenerator::checkGoalConstraints() -> TrajectoryGenerator::checkCartesianGoalConstraint()

The first of those methods has access to a planning scene, though I'm unsure if there's a cleaner way than passing a state through the method calls (especially since the state itself is arbitrary, it's just the access to the subframes we're interested in)

From my understanding, the check (for existence of the IK (sub)frame and it's usability for IK) didn't fail.
In contrast to RobotState::knowsFrameTransform(), canSetStateFromIK() should also verify that the IK frame is rigidly connected to the IK solver's tip frame (so exactly what you need).

I do get the same error as before uncommenting this when planning with a subframe:

[10:15:49.016004] [/fake/move_group]: No IK solver available for link: "tb_1_1/disc_tip" [10:15:49.016126] [/fake/move_group]: Planner threw an exception (error code: -31): Could not solve request [10:15:49.017372] [/fake/motion_server]: Failed to plan sequence motion foo. Error code: NO_IK_SOLUTION. If problem persists, check toolbit config.

I think the issue the issue is that JointModelGroup has no notion of subframes and only checks for links in canSetStateFromIK, whereas RobotState knows about both and setStateFromIK is designed for that possibility.

To summarise my findings:

The transformation from subframe goal to tip link goal I added to pilz isn't required provided the RobotState used for setStateFromIK() is built from the scene which knows about the subframes, as setStateFromIK() accounts for this case. I've removed this transformation from pilz now.

I'm no longer using the default_planner_request_adapters/ResolveConstraintFrames adapter as this was causing the original issue of planning a linear motion wrt the tip link rather than the subframe.

Therefore, anywhere pilz is validating the link_name of constraints, it's the subframe name that's being used (causing the above failure whenever the validation code is subframe unaware). This appears to be the case whenever using JointModelGroup.

codecov · 2023-10-30T13:03:49Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (984a0ea) 61.81% compared to head (eda07ab) 61.84%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3519      +/-   ##
==========================================
+ Coverage   61.81%   61.84%   +0.04%     
==========================================
  Files         385      385              
  Lines       34121    34091      -30     
==========================================
- Hits        21088    21081       -7     
+ Misses      13033    13010      -23

Files	Coverage Δ
..._industrial_motion_planner/planning_context_base.h	`100.00% <100.00%> (ø)`
...z_industrial_motion_planner/trajectory_generator.h	`100.00% <ø> (ø)`
...ustrial_motion_planner/trajectory_generator_circ.h	`100.00% <ø> (ø)`
...dustrial_motion_planner/trajectory_generator_lin.h	`100.00% <ø> (ø)`
...strial_motion_planner/src/command_list_manager.cpp	`100.00% <100.00%> (ø)`
...anner/src/trajectory_blender_transition_window.cpp	`100.00% <ø> (ø)`
...strial_motion_planner/src/trajectory_functions.cpp	`100.00% <100.00%> (ø)`
...strial_motion_planner/src/trajectory_generator.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%> (ø)`
... and 1 more

... and 7 files with indirect coverage changes

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

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

riv-tnoble · 2023-10-30T13:22:45Z

Simple sanity check of the LIN output:
https://github.com/ros-planning/moveit/assets/86778266/4c341090-64c1-4856-96a2-662006716dbf

Will do the same for CIRC as I believe a decent portion of the code is shared

rhaschke

More feedback below.

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

….com/rr-tom-noble/moveit into bugfix/fix-lin-plans-using-subframes

riv-tnoble · 2023-11-01T10:09:03Z

We're currently looking into an issue using this for our use case, which is as follows:

We plan to rotate a separate planning group before the sequence plan (the current state of that group at the time of the sequence plan is unrotated)
We pass the expected rotation of that other planning group as part of the start state in the plan sequence request
The changes to use a robot state from getCurrentState() now takes collision objects into account (desired)
However, (I think) this also takes a collision object of the unrotated other planning group into account (rather than the rotated start state version)
This causes a collision (which is not an issue, as we expect the other planning group to have already rotated by the time we plan, hence setting the start state as such)

@rr-mark is also looking into this issue

riv-mjohnson · 2023-11-01T10:34:35Z

@rr-mark is also looking into this issue

~~It looks like~~

~~constructing the robot state from the robot model has the correct joint states, but no attached collision objects.~~
~~constructing the robot state from the planning scene has the attached collision objects, but the joint states are the current state, rather than the planning context.~~

~~I'm currently trying to rationalise the two; it would be good to get help from someone who know more about what is supposed to happen here.~~

With a little more digging, what is actually happening is now that computePoseIK has attached collision objects, it is performing collision checks with those objects where it wasn't before.

It looks like info.start_joint_position never contained information about joint groups other than the group currently being planned, and this previously didn't matter because the objects attached to those groups were not being checked.

Now those objects are being checked, it matters that their joints are not properly being updated.

Is computePoseIK supposed to be unaware of attached collision objects, with these checks happening elsewhere? Or is this something which should now be updated? It seems easy enough to fix the info.start_joint_position to contain all joint states, but this might have implications for computation times and for what errors are returned when collisions are detected.

riv-mjohnson · 2023-11-01T11:47:07Z

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

@@ -134,9 +133,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
    info.start_joint_position[joint_name] = req.start_state.joint_state.position[index];
  }


assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); std::map<std::string, double> start_joint_position; for (unsigned i=0; i<req.start_state.joint_state.name.size(); ++i) { start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i]; } for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) { if (start_joint_position.count(joint_name) == 0) { std::ostringstream os; os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name << "\" in start state of request"; throw LinJointMissingInStartState(os.str()); } info.start_joint_position[joint_name] = start_joint_position[joint_name]; } computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); // check goal pose ik before Cartesian motion plan starts std::map<std::string, double> ik_solution; if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, start_joint_position, ik_solution)) { std::ostringstream os; os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose"; throw LinInverseForGoalIncalculable(os.str()); }

This looks like it correctly updates all the joint states, including those not in the current joint group, so that the IK collision detection is correct.

rhaschke · 2023-11-02T08:06:11Z

I'm not sure what (remaining?) problem you are referring to in #3519 (comment) and #3519 (comment).
In general, a planner should use the current state of the PS (PlanningScene::getCurrentState()) for planning and this state should comprise all joint values. However, this state might differ from the real current robot state.

That computePoseIK() wasn't using that state, but a newly created one, was definitely a design bug.

riv-mjohnson · 2023-11-06T08:29:13Z

Your changes work on my end, thank you for your time.

riv-mjohnson · 2023-11-08T17:28:01Z

We're seeing a couple of issues with this PR:

Planning LIN paths is now much slower, as attached collision objects are now being considered in the collision checking (since they are now part of the robot state, where they weren't previously). Adds a MeshCache to _msgsToAttachedBody to prevent duplicate attached collision objects being stored in FCLShapeCache #3518 helps a lot, but the net effect is still a slowdown.
Collisions along the path are now reported as IK failures, and nothing is published to display_contacts. This makes it harder to turn the output into a user-friendly error message.

Is all the extra collision checking working as intended? Or is this redundant with other collision checking after the path has been generated?

rhaschke · 2023-11-08T20:49:11Z

You are right, collision checking is performed twice: (i) during path generation and (ii) as a final trajectory validation step.
As the Pilz planner is not actually planning around obstacles, but simply generating a trajectory, we can skip the collision checking during planning. I will add a corresponding commit.

rhaschke · 2023-11-08T21:33:11Z

Actually, collision checking in generateJointTrajectory() is optional and disabled by default, particularly for all trajectory generators. The only source that was using collision checking was blend().

Can you please verify that the behavior is as intended now?

riv-mjohnson · 2023-11-09T09:29:15Z

Yes, it looks good now; collisions along the path are reported as they were before this PR.

Thanks again.

rhaschke · 2023-11-09T21:48:43Z

Thanks for iterating on this.

rhaschke · 2023-11-10T01:22:06Z

While this PR seems to fix ignored modified start states in unrelated JMGs, the original issue is not yet solved, is it?

When planning a LIN/CIRC motion using a subframe as the link_name in constraints, the motion is linear w.r.t. the end-effector link, rather than the subframe.

I think this can only be resolved by explicitly interpolating between the actual subframe's start and goal pose (i.e. not first back-projecting to the associated robot link) and considering the offset when computing the IK instead. I implemented this for the CartesianInterpolator in #3197. #2890, authored by you guys, should be reverted to a large extent in order to maintain the original goal pose.

Unfortunately, the unit tests don't cover those cases yet - they only consider the final goal pose, but don't check the path in between.

Partially reverts changes of moveit#3519

riv-tnoble · 2023-11-13T12:54:49Z

While this PR seems to fix ignored modified start states in unrelated JMGs, the original issue is not yet solved, is it?

When planning a LIN/CIRC motion using a subframe as the link_name in constraints, the motion is linear w.r.t. the end-effector link, rather than the subframe.

I think this can only be resolved by explicitly interpolating between the actual subframe's start and goal pose (i.e. not first back-projecting to the associated robot link) and considering the offset when computing the IK instead. I implemented this for the CartesianInterpolator in #3197. #2890, authored by you guys, should be reverted to a large extent in order to maintain the original goal pose.

Unfortunately, the unit tests don't cover those cases yet - they only consider the final goal pose, but don't check the path in between.

Hi, and thanks for the feedback and help with this PR.

I think the code does work as intended as passing in the subframe as the link_name and removing the default_planner_request_adapters/ResolveConstraintFrames adapter causes the target point offset to be [0. 0. 0]. The start and goal points are for the subframe, and it's those points we use KDL to build the LIN motion for. It's only when we're sampling points along that path and performing IK that the points are back projected to a robot link (within the robot_state->setFromIK() call I believe).

I do wonder if this makes the point of target_offset_point a little redundant, so I don't mind those changes being reverted. Maybe still useful if you did want to plan using an offset from the subframe itself?

Let me know if you agree with the above

rhaschke · 2023-11-13T12:58:41Z

Sure, if you remove the ResolveConstraintFrames adapter, the target_point_offset becomes zero. However, I was complaining about the case when this offset is non-zero. See #3523 for the fix. Now it works with both ResolveConstraintFrames enabled or disabled.

* Ports moveit/moveit/pull/3519 to ros2 * Applies formatting * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Replaces RobotState with pointer in tests * Fixes tests

* Ports moveit/moveit/pull/3519 to ros2 * Applies formatting * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Replaces RobotState with pointer in tests * Fixes tests (cherry picked from commit 70e1aae) # Conflicts: # moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h # moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp # moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp # moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp # moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp

Co-authored-by: Tom Noble <tom@rivelinrobotics.com>

* Ports moveit/moveit/pull/3519 to ros2 * Applies formatting * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Replaces RobotState with pointer in tests * Fixes tests

Fixes LIN plans using subframes not being linear with respect to the …

0c239a4

…subframe

riv-tnoble requested a review from jschleicher as a code owner October 30, 2023 12:07

rhaschke reviewed Oct 30, 2023

View reviewed changes

Removes error that tends to spam terminal with long request details

993841f

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

rhaschke reviewed Oct 30, 2023

View reviewed changes

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp Outdated Show resolved Hide resolved

riv-tnoble added 6 commits October 30, 2023 13:33

Removes call to RobotState::setToDefaultValues()

9f8f6bb

Replaces PlanningScene with RobotState as computePoseIK parameter

c7c1b5e

Removes redundant code

b37d5eb

Undo removal of spacing

365ec90

Applies formatting

cf1b18b

Begins fixing unit tests

a7bed71

rhaschke mentioned this pull request Oct 30, 2023

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

Closed

rhaschke marked this pull request as draft October 31, 2023 07:17

rhaschke reviewed Oct 31, 2023

View reviewed changes

riv-tnoble and others added 5 commits October 31, 2023 10:47

Removes robot_state.setToDefaultValues() line

f360d1a

Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>

Restructures computePoseIK to minimise changes

883d9b9

Removes whitespace

f76284a

Deduplicates computeLinkFK error handling

cf6e955

Merge branch 'bugfix/fix-lin-plans-using-subframes' of https://github…

1e83879

….com/rr-tom-noble/moveit into bugfix/fix-lin-plans-using-subframes

riv-tnoble requested a review from rhaschke October 31, 2023 11:43

riv-tnoble changed the title ~~Fixes LIN plans using subframes not being linear with respect to the …~~ WIP Fixes LIN plans using subframes not being linear with respect to the … Nov 1, 2023

riv-mjohnson added 2 commits November 1, 2023 09:40

Minor tidying.

0a0de04

Undoes bad change to comment.

5410070

riv-mjohnson reviewed Nov 1, 2023

View reviewed changes

More cleanup

03417c9

rhaschke approved these changes Nov 3, 2023

View reviewed changes

rhaschke added the awaits 2nd review one maintainer approved this request label Nov 3, 2023

captain-yoshi mentioned this pull request Nov 6, 2023

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

Merged

10 tasks

rhaschke added 2 commits November 8, 2023 21:55

Simplify isStateColliding()

ff4f000

Disable collision checking for blend trajectory generation

eda07ab

rhaschke merged commit c404ca2 into moveit:master Nov 9, 2023

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

f3f1e1a

Partially reverts changes of moveit#3519

rhaschke mentioned this pull request Nov 11, 2023

Fix Pilz planner: consider link_name offset during IK computation #3523

Merged

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

riv-mjohnson mentioned this pull request Feb 13, 2024

Sequence planning in PILZ does not keep attached collision object poses constant. #3569

Closed

Danilrivero mentioned this pull request May 28, 2024

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

Closed

riv-tnoble added a commit to riv-tnoble/moveit2 that referenced this pull request Nov 2, 2024

Ports moveit/moveit/pull/3519 to ros2

87e637a

riv-tnoble mentioned this pull request Nov 2, 2024

Ports moveit/moveit/pull/3519 to ros2 moveit/moveit2#3055

Merged

6 tasks

This was referenced Nov 5, 2024

Ports moveit/moveit/pull/3519 to ros2 (backport #3055) moveit/moveit2#3061

Merged

Ports moveit/moveit/pull/3519 to ros2 (backport #3055) moveit/moveit2#3062

Closed

sjahr pushed a commit to moveit/moveit2 that referenced this pull request Nov 8, 2024

Ports moveit/moveit/pull/3519 to ros2 (backport #3055) (#3061)

ad0431b

Co-authored-by: Tom Noble <tom@rivelinrobotics.com>

riv-mjohnson mentioned this pull request Feb 11, 2025

PILZ LIN planner calculates first IK using start state containing wrong joint values for joints outside the current joint model group. moveit/moveit2#3336

Closed

		@@ -134,9 +133,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
		info.start_joint_position[joint_name] = req.start_state.joint_state.position[index];
		}

Conversation

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

Uh oh!

Description

Checklist

Uh oh!

rhaschke left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

rhaschke Oct 30, 2023

Choose a reason for hiding this comment

Uh oh!

riv-tnoble Oct 30, 2023

Choose a reason for hiding this comment

Uh oh!

riv-tnoble Oct 31, 2023

Choose a reason for hiding this comment

Uh oh!

rhaschke Oct 31, 2023

Choose a reason for hiding this comment

Uh oh!

riv-tnoble Oct 31, 2023 • 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 Oct 31, 2023 • 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!

codecov bot commented Oct 30, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Uh oh!

riv-tnoble commented Oct 30, 2023

Uh oh!

rhaschke left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

riv-tnoble commented Nov 1, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson commented Nov 1, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-mjohnson Nov 1, 2023 • 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-mjohnson Nov 1, 2023 • 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!

rhaschke commented Nov 2, 2023

Uh oh!

riv-mjohnson commented Nov 6, 2023

Uh oh!

riv-mjohnson commented Nov 8, 2023

Uh oh!

rhaschke commented Nov 8, 2023

Uh oh!

rhaschke commented Nov 8, 2023

Uh oh!

riv-mjohnson commented Nov 9, 2023

Uh oh!

rhaschke commented Nov 9, 2023

Uh oh!

rhaschke commented Nov 10, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

riv-tnoble commented Nov 13, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

rhaschke commented Nov 13, 2023

riv-tnoble commented Oct 30, 2023 •

edited

Loading

riv-tnoble Oct 31, 2023 •

edited

Loading

riv-tnoble Oct 31, 2023 •

edited

Loading

codecov bot commented Oct 30, 2023 •

edited

Loading

riv-tnoble commented Nov 1, 2023 •

edited

Loading

riv-mjohnson commented Nov 1, 2023 •

edited

Loading

riv-mjohnson Nov 1, 2023 •

edited

Loading

riv-mjohnson Nov 1, 2023 •

edited

Loading

rhaschke commented Nov 10, 2023 •

edited

Loading

riv-tnoble commented Nov 13, 2023 •

edited

Loading