End-state deviates from goal state #532

captain-yoshi · 2024-02-21T18:39:36Z

This fixes #531 but may not fix the actual problem, e.g. it may be the planner that silently fails or something else in the framework.

I just validate the last trajectory waypoint with the goal in the ConnectStage. If there is a deviation, the solution is marked as failed and a comment is set. I'm pretty sure you can optimize some of the code ;)

On a side note, I had to make sure that the joints where compared correctly (-pi = pi). Currently, when comparing joint position in the Connect::compatible method, this is not checked. You might reject some valid solutions, e.g the IK solver gives some joint positions in a negative defined angle versus the RobotState who may have a positive defined angle.

Solution 1 et 3 are valid but not 2 and 4.

If not within the threshold, mark solution as a failure and set comment.

codecov · 2024-02-21T18:44:25Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 59.09%. Comparing base (7638e5f) to head (8f8f08b).

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #532      +/-   ##
==========================================
+ Coverage   58.99%   59.09%   +0.11%     
==========================================
  Files          90       90              
  Lines        8497     8504       +7     
==========================================
+ Hits         5012     5025      +13     
+ Misses       3485     3479       -6

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

rhaschke

Reading your solution to #531, I think that this is not related to incompatible start and end states: What you check is that the end-state actually reaches the goal state.
I think, your issue is a duplicate of #485, which was caused by a bug in MoveIt2.
Are you using MoveIt1 or MoveIt2? If you were using MoveIt1, it would be great if you could setup a failing unittest for your issue. Then, I can search for the underlying problem instead of applying cosmetic corrections 😉

rhaschke · 2024-02-23T11:28:37Z

core/src/stages/connect.cpp

+			if (jm->getType() == robot_state::JointModel::REVOLUTE)
+				min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]);
+			else
+				min_distance = positions_last_waypoint[i], positions_goal[i];


I guess you want to compute the joint difference:

Suggested change

min_distance = positions_last_waypoint[i], positions_goal[i];

min_distance = std::abs(positions_last_waypoint[i] - positions_goal[i]);

rhaschke · 2024-02-23T11:29:58Z

core/src/stages/connect.cpp

+			ROS_DEBUG_STREAM_NAMED("Connect",
+			                       "angular deviation: " << min_distance << " between trajectory last waypoint and goal.");
+
+			if (std::abs(min_distance) > 1e-4) {


The magic number 1e-4 should be configurable, e.g. via a stage property.

captain-yoshi · 2024-02-23T15:11:02Z

What you check is that the end-state actually reaches the goal state.

Yes ! Changed the title.

This fixes #531 but may not fix the actual problem, e.g. it may be the planner that silently fails or something else in the framework.

Just found the culprit. Even though LIN Pilz receives a JointConstraint, it will compute the FK to compute the goal pose. Currently the LIN planner only plans from a cartesian goal. Will create an issue in MoveIt.

Not tested but I'm pretty sure I could recreate the problem by changing the demo pick and place and replacing this line with the LIN Pilz planner. Or would you prefer a unit-test in MoveIt ?

For testing this PR, is there a way to fake a planner response that would offset the goal state outside the threshold limits ?

captain-yoshi · 2024-02-23T16:25:49Z

To replicate this error, just use this mtc fork captain-yoshi/moveit_task_constructor/tree/trigger-error-532 and start the pick & place demo. This includes the validation in this PR.

You should see that it's failing and that for some solution the end-state does not match the goal-state.

captain-yoshi · 2024-02-23T18:11:56Z

After reading more on the Pilz LIN planner, it generates linear Cartesian trajectory between goal and start poses.

The solution would be to feed the joint constraint goal instead of redoing an IK.

EDIT
The IK is invoked on each sample of the cartesian path. Because the end goal IK can be different from the one supplied, one cannot use a Pilz LIN in a Connect stage.

Well you can use it, but it will only work for IK's who succeeded (in the ComputeIK stage) on their first attempt because the seed is wrt. the start state ?

rhaschke · 2024-02-24T17:27:02Z

Yes, the Pilz planner performs linear interpolation in Cartesian space. If you want to perform linear interpolation in joint space, there is the JointInterpolationPlanner in MTC available for this.

Not tested but I'm pretty sure I could recreate the problem by changing the demo pick and place and replacing this line with the LIN Pilz planner. Or would you prefer a unit-test in MoveIt ?

So you claim, that the Pilz planner generates incomplete trajectories, which are considered to be valid by MTC?
Please create a unit test in MTC triggering exactly this issue. Thanks.

For testing this PR, is there a way to fake a planner response that would offset the goal state outside the threshold limits?

Why would you like to do so? If a planner behaves like that, this planner is buggy and should get fixed.

rhaschke

I see the need for this PR. Some planners, like Pilz or MTC's Cartesian interpolation, may not reach the exact goal state, although the Cartesian poses match.
See the comment below to address before merging.

rhaschke · 2024-02-24T17:38:49Z

core/src/stages/connect.cpp

+		double min_distance;
+		for (std::size_t i = 0; i < num; ++i) {
+			if (jm->getType() == robot_state::JointModel::REVOLUTE)
+				min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]);


You must not use angles::shortest_angular_distance here, as we need to reach the exact goal state - subsequent stages rely on this fact!

Hum... I'm pretty sure it removed valid solutions... Will investigate more and report back soon.

We cannot accept equality up to multiples to 360°. The goal state needs to be reached exactly. Otherwise subsequent stages will run into trouble because they relied on a different start state.

captain-yoshi · 2024-02-25T02:06:41Z

I see the need for this PR. Some planners, like Pilz or MTC's Cartesian interpolation, may not reach the exact goal state, although the Cartesian poses match.

Exactly ! This is necessary to be able to use such planners, which does not respect the goal joint constraint. Furthermore, it can catch errors in other planners, e.g. #485. Finally, it guards against the move_group/ExecuteTaskSolutionCapability which seems to shoot the hole trajectory without doing any validation against the end state and the start state of sub-trajectories.

rhaschke · 2024-02-25T11:10:01Z

it guards against the move_group/ExecuteTaskSolutionCapability which seems to shoot the hole trajectory without doing any validation against the end state and the start state of sub-trajectories.

The discontinuity doesn't happen between the end-state and start-state of to consecutive trajectories but between the next-to-last and last state of the Connect trajectories. Thus, MoveIt's task execution cannot guard against this discontinuity.

rhaschke · 2024-02-25T11:11:14Z

I pushed a simplifying commit, using RobotState::distance() to compute the joint-space distance between the two states. No need to reimplement the wheel ;-)

rhaschke

I also added a unit test now. For me, this is ready to merge. @captain-yoshi, any objections to my changes?

captain-yoshi · 2024-02-25T14:31:31Z

All good on my end :) Great optimization by the way 🥇

Validate connect stage deviation: end trajectory with goal

c931acf

If not within the threshold, mark solution as a failure and set comment.

captain-yoshi added 2 commits February 22, 2024 09:38

[fix] compute shortest angular distance only on revolute joints

7c317b9

[refac] move validation into it's own method

b42c0b1

rhaschke reviewed Feb 23, 2024

View reviewed changes

captain-yoshi changed the title ~~Validate connect stage deviation from end trajectory to goal~~ End-state deviates from goal state Feb 23, 2024

captain-yoshi added 2 commits February 23, 2024 10:48

[fix] compute joint difference

26e8519

[impl] expose deviation threshold to stage property

0d50ec9

rhaschke requested changes Feb 24, 2024

View reviewed changes

simplify

fc6eacc

rhaschke added 2 commits February 25, 2024 13:46

Add corresponding unit test

7edc2f9

fixup! simplify

8f8f08b

rhaschke approved these changes Feb 25, 2024

View reviewed changes

rhaschke merged commit 5720b83 into moveit:master Feb 25, 2024
8 checks passed

sea-bass mentioned this pull request May 3, 2024

Fix description of max_distance property of Connect stage #564

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

End-state deviates from goal state #532

End-state deviates from goal state #532

captain-yoshi commented Feb 21, 2024

codecov bot commented Feb 21, 2024 •

edited

Loading

rhaschke left a comment

rhaschke Feb 23, 2024

rhaschke Feb 23, 2024

captain-yoshi commented Feb 23, 2024

captain-yoshi commented Feb 23, 2024

captain-yoshi commented Feb 23, 2024 •

edited

Loading

rhaschke commented Feb 24, 2024

rhaschke left a comment •

edited

Loading

rhaschke Feb 24, 2024

captain-yoshi Feb 25, 2024

rhaschke Feb 25, 2024

captain-yoshi commented Feb 25, 2024 •

edited

Loading

rhaschke commented Feb 25, 2024

rhaschke commented Feb 25, 2024

rhaschke left a comment

captain-yoshi commented Feb 25, 2024

	min_distance = positions_last_waypoint[i], positions_goal[i];
	min_distance = std::abs(positions_last_waypoint[i] - positions_goal[i]);

End-state deviates from goal state #532

End-state deviates from goal state #532

Conversation

captain-yoshi commented Feb 21, 2024

codecov bot commented Feb 21, 2024 • edited Loading

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Feb 23, 2024

Choose a reason for hiding this comment

rhaschke Feb 23, 2024

Choose a reason for hiding this comment

captain-yoshi commented Feb 23, 2024

captain-yoshi commented Feb 23, 2024

captain-yoshi commented Feb 23, 2024 • edited Loading

rhaschke commented Feb 24, 2024

rhaschke left a comment • edited Loading

Choose a reason for hiding this comment

rhaschke Feb 24, 2024

Choose a reason for hiding this comment

captain-yoshi Feb 25, 2024

Choose a reason for hiding this comment

rhaschke Feb 25, 2024

Choose a reason for hiding this comment

captain-yoshi commented Feb 25, 2024 • edited Loading

rhaschke commented Feb 25, 2024

rhaschke commented Feb 25, 2024

rhaschke left a comment

Choose a reason for hiding this comment

captain-yoshi commented Feb 25, 2024

codecov bot commented Feb 21, 2024 •

edited

Loading

captain-yoshi commented Feb 23, 2024 •

edited

Loading

rhaschke left a comment •

edited

Loading

captain-yoshi commented Feb 25, 2024 •

edited

Loading