Discontinuity in trajectory #485

leander2189 · 2023-09-12T09:05:29Z

I am building a complex movment comprised of several stages. In one of them, a connecting stage using RRT sampling planner, a discontinuty appears, and the movement is taken as correct

move_fail.mp4

I suspect that the issue might not be with MTC itself, but rather with the sampling planner configuration.

Can you point me in which direction I should look to debug this issue?

That stage is build using this:

// Sampling planner
sampling_planner_ = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(node_);
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);

auto stage = std::make_unique<mtc::stages::Connect>(
            name, mtc::stages::Connect::GroupPlannerVector{{"complete_chain", sampling_planner_}});

stage->setTimeout(timeout);
stage->properties().configureInitFrom(Stage::PARENT);

I am also getting some logs that I suspect may be related, but don't know which config parameters I should tweak

[operation_solver-14] [ERROR 1694508450.076982062] [moveit.ros_planning.planning_pipeline]: Computed path is not valid. Invalid states at index locations: [ 51 ] out of 59. Explanations follow in command line. Contacts are published on /display_contacts (generatePlan() at ./planning_pipeline/src/planning_pipeline.cpp:329)
[operation_solver-14] [INFO 1694508450.077185331] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'work_object' (type 'Object') and 'ee_hilok' (type 'Robot attached'), which constitutes a collision. Contact information is not stored. (collisionCallback() at ./collision_detection_fcl/src/collision_common.cpp:319)
[operation_solver-14] [INFO 1694508450.077203612] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored) (collisionCallback() at ./collision_detection_fcl/src/collision_common.cpp:350)
[operation_solver-14] [ERROR 1694508450.077629306] [moveit.ros_planning.planning_pipeline]: Completed listing of explanations for invalid states. (generatePlan() at ./planning_pipeline/src/planning_pipeline.cpp:357)
[operation_solver-14] [INFO 1694508450.186159185] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [INFO 1694508458.290763315] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [INFO 1694508461.125787735] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [INFO 1694508462.561658228] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [WARN 1694508467.562497303] [moveit.ompl_planning.model_based_planning_context]: Solution is approximate (logPlannerStatus() at ./ompl_interface/src/model_based_planning_context.cpp:998)
[operation_solver-14] [INFO 1694508468.242949434] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [WARN 1694508492.011518945] [ompl]: ./src/ompl/geometric/src/PathSimplifier.cpp:711 - Solution path may slightly touch on an invalid region of the state space (log() at ./ompl_interface/src/ompl_planner_manager.cpp:68)
[operation_solver-14] [INFO 1694508492.049325049] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [WARN 1694508497.052255456] [moveit.ompl_planning.model_based_planning_context]: Solution is approximate (logPlannerStatus() at ./ompl_interface/src/model_based_planning_context.cpp:998)
[operation_solver-14] [INFO 1694508497.378308211] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'complete_chain' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. (useConfig() at ./ompl_interface/src/model_based_planning_context.cpp:406)
[operation_solver-14] [WARN 1694508547.380569701] [moveit.ompl_planning.model_based_planning_context]: Timed out (logPlannerStatus() at ./ompl_interface/src/model_based_planning_context.cpp:994)
[operation_solver-14] [INFO 1694508547.543847603] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem (solve() at ./ompl_interface/src/model_based_planning_context.cpp:791)

leander2189 · 2023-09-12T11:28:42Z

After some more testing, I am pretty sure that the "Time out" error is what is causing this issue. I've increased the timeout setting in the stage and the number of solutions with the issue went down dramatically.

Any clue how a not fully computed stage is being added to the solution?

rhaschke · 2023-09-18T12:39:48Z

Thanks for reporting this issue. So far I was not able to reproduce the issue (timed-out solutions are not considered on my side). Do you mind providing a minimal example reproducing your issue? Which versions of MTC and MoveIt are you exactly working with?

leander2189 · 2023-09-21T09:49:29Z

I am using humble binaries for MoveIt, and humble branch for MTC.
This code is part of a much bigger project, so it's going to take me a few days to extract the relevant parts and come up with a minimal example that reproduces the issue. Hopefully I will have it ready sometime next week. Thanks!

DaniGarciaLopez · 2023-10-02T17:21:07Z

Hi @rhaschke and @leander2189,

I encountered a similar issue in April/May of this year, but I didn't have the time to investigate the root cause back then. After reading this issue, I tried extending the timeout, which has helped to significantly reduce incorrect solutions, although occasional discontinuous paths still pop up.

I was utilizing PRM* initially, and I also tried RRT with the same results. Same configuration: moveit from binaries and MTC from humble branch.

LeroyR · 2023-10-04T08:38:32Z

It could be that the starting position is invalid. This combined with default planning adapters like:

      default_planner_request_adapters/FixStartStateBounds
      default_planner_request_adapters/FixStartStateCollision
      default_planner_request_adapters/FixStartStatePathConstraints

results in ompl just using some other position instead of the given start pose -> Discontinuity in trajectory

e.x.: the solution of some state is not collision checked. You could remove the FixStart* adapters or filter collisions with a PredicateFilter stage.

auto applicability_filter = std::make_unique<stages::PredicateFilter>("filter collision", std::move(stage));
applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
    if (s.end()->scene()->isStateColliding())
    {
        collision_detection::CollisionResult::ContactMap contacts;
        s.end()->scene()->getCollidingPairs(contacts);
        comment = "collision";
        for(auto c : contacts) {
            comment += " between '" + c.first.first + "' and '" + c.first.second + "'";
        }
        return false;
    }
    return true;
});

leander2189 · 2023-10-04T08:46:29Z

It could be that the starting position is invalid.

If the starting position is invalid, wouldn't every solution be the same?

The situation that we are seeing is that only one of the solutions proposed by MTC is discontinous and the others are usually ok.

In fact, further testing has led us to discover that the "jump" is between the last point of the trajectory and the goal position

LeroyR · 2023-10-04T10:13:30Z

If the starting position is invalid, wouldn't every solution be the same?

Normally (with the given code) the connect stage solves once for each pair e.g.
Initial(1 states)->connect<-PoseGenerator(5 states)
results in 5 solutions for connect

The situation that we are seeing is that only one of the solutions proposed by MTC is discontinous and the others are usually ok.

In fact, further testing has led us to discover that the "jump" is between the last point of the trajectory and the goal position

If you are sure that the error is with the planner you could maybe try to extract the subsolutions until you reach the state which is used as start for ompl and the given goal to reproduce where ompl produces a discontinuity

I am not seeing your complete task but given the image section (complex task) i assume the error lies somewhere else. If you can provide some example we could try to work it out.

leander2189 · 2023-10-04T10:17:11Z

If you are sure that the error is with the planner you could maybe try to extract the subsolutions until you reach the state which is used as start for ompl and the given goal to reproduce where ompl produces a discontinuity

Yeah the error lies somewhere in the Connect stage. This is a good idea, we already implemented some solution checking to automatically detect the error, so I think we can save that state somehow to improve reproducibility.

If you can provide some example we could try to work it out.

I am working on it. My code is tightly coupled with a lot of other stuff (it's part of a much bigger project), so it's taking me quite long to extract the relevant code into a standalone executable.

rhaschke · 2023-10-04T17:20:28Z

@leander2189, instead of extracting your code into a standalone executable, try to reproduce the issue with moveit_resources_panda_moveit_config. Take one of the examples in demo/src as a starting point.

leander2189 · 2023-10-09T09:05:53Z

Hi,

I pushed here: https://github.com/leander2189/dtc_test a project with a motion planning that sometimes reproduces the error. I've been playing with stage timeouts but the test not always causes the discontinuity.

In case that helps debugging, I got the log and a screenshot of an execution where the discontinuity happens. I've been diving through the log file but there are too many lines and I can't find anything in there. Any help on how to locate the stage that is failing would be appreciated.

debug_data.zip

Solution #111 with a total cost of 59.7775, and the stage that fails is "move_to_fastener_registration" #8 with a partial cost of 8.3445.

Screencast.from.10-09-2023.10.19.50.AM.webm

rhaschke · 2023-10-12T12:34:53Z

Thanks for providing the example. I'm available to reproduce the issue (after reducing the connect-stage timeout to 0.1s) and will have a look now.

rhaschke · 2023-10-13T00:57:57Z

I noticed several issues:

Traversing the sequence of sub-solutions in SerialContainer sometimes violates the assertion:
https://github.com/ros-planning/moveit_task_constructor/blob/564804bd7bb9e320b304b8c4880d96c6ce63b915/core/src/container.cpp#L491
I don't yet understand why, though. Replicating the task setup as a unit test didn't yet reproduce that issue.
The broken solutions in your case stem from approximate solutions, which are added as valid solutions.
Your debug_data.txt has four of those approximate solutions, all of them stemming from a timed out planning attempt (planning time is just above the timeout of 2s):
```
[dtc_test-6] [DEBUG] [1696838058.399928362] [ompl]: ./src/ompl/geometric/src/SimpleSetup.cpp:145 - Solution found in 2.000133 seconds
[dtc_test-6] [WARN] [1696838058.399932681] [moveit.ompl_planning.model_based_planning_context]: Solution is approximate
```
MoveIt seems to turn an approximate solution into a valid one after (successful) application of planning request adapters. Need to investigate that tomorrow in more depth.
A new unit test introduced in [WIP] Debug/Fix #485 #495 exhibits duplicated full solution paths.

Abishalini · 2023-10-13T14:55:04Z

I was encountering this problem and added this check in the Connect stage to catch the discontinuities. MoveIt is returning a success with partial solutions. I haven't had the chance to dig into MoveIt to identify the root cause yet.

Here is what I added to the Connect stage in the compute function

// Check if the end goal and the last waypoint are close. The trajectory should be marked as failure otherwise.
		if (success) {
			const auto distance = end->getCurrentState().distance(trajectory->getLastWayPoint());
			const auto& goal_tolerance = props.get<float>("goal_tolerance");
			if (distance > goal_tolerance) {
				RCLCPP_INFO_STREAM(LOGGER, "Stage Name : " << name_);
				RCLCPP_INFO_STREAM(LOGGER, "The trajectory given by the plan function does not satisfy the goal state. "
				                               << "Marking it as a failure");
				success = false;
			}
		}

One of the reasons for discontinuity came from using pick_ik as our IK solver. The joint limits were being mixed up (Ref - PickNikRobotics/pick_ik#54) and hence we would get partial solutions if the end goal was out of joint limits.

rhaschke · 2023-10-16T18:16:59Z

I found the underlying culprit in MoveIt2. A fix is proposed in moveit/moveit2#2455.

leander2189 · 2023-10-17T09:58:06Z

Awesome!

[WIP] Debug/Fix #485

rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this issue Oct 13, 2023

Unit test for moveit#485

5b6287a

rhaschke mentioned this issue Oct 13, 2023

[WIP] Debug/Fix #485 #495

Merged

2 tasks

rhaschke mentioned this issue Oct 16, 2023

Map ompl's APPROXIMATE_SOLUTION -> TIMED_OUT / PLANNING_FAILED moveit/moveit2#2455

Merged

rhaschke closed this as completed in moveit/moveit2#2455 Oct 17, 2023

rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this issue Oct 19, 2023

Unit test for moveit#485

212cb6d

rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this issue Oct 20, 2023

Unit tests for moveit#485

68de335

rhaschke added a commit to ubi-agni/moveit_task_constructor that referenced this issue Oct 20, 2023

Unit tests for moveit#485

08b102d

v4hn added a commit that referenced this issue Feb 16, 2024

Merge pull request #495 from ubi-agni/debug-#485

7638e5f

[WIP] Debug/Fix #485

rhaschke mentioned this issue Feb 23, 2024

End-state deviates from goal state #532

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Discontinuity in trajectory #485

Discontinuity in trajectory #485

leander2189 commented Sep 12, 2023

leander2189 commented Sep 12, 2023

rhaschke commented Sep 18, 2023

leander2189 commented Sep 21, 2023

DaniGarciaLopez commented Oct 2, 2023

LeroyR commented Oct 4, 2023 •

edited

Loading

leander2189 commented Oct 4, 2023 •

edited

Loading

LeroyR commented Oct 4, 2023 •

edited

Loading

leander2189 commented Oct 4, 2023 •

edited

Loading

rhaschke commented Oct 4, 2023

leander2189 commented Oct 9, 2023

rhaschke commented Oct 12, 2023

rhaschke commented Oct 13, 2023

Abishalini commented Oct 13, 2023

rhaschke commented Oct 16, 2023

leander2189 commented Oct 17, 2023

Discontinuity in trajectory #485

Discontinuity in trajectory #485

Comments

leander2189 commented Sep 12, 2023

leander2189 commented Sep 12, 2023

rhaschke commented Sep 18, 2023

leander2189 commented Sep 21, 2023

DaniGarciaLopez commented Oct 2, 2023

LeroyR commented Oct 4, 2023 • edited Loading

leander2189 commented Oct 4, 2023 • edited Loading

LeroyR commented Oct 4, 2023 • edited Loading

leander2189 commented Oct 4, 2023 • edited Loading

rhaschke commented Oct 4, 2023

leander2189 commented Oct 9, 2023

rhaschke commented Oct 12, 2023

rhaschke commented Oct 13, 2023

Abishalini commented Oct 13, 2023

rhaschke commented Oct 16, 2023

leander2189 commented Oct 17, 2023

LeroyR commented Oct 4, 2023 •

edited

Loading

leander2189 commented Oct 4, 2023 •

edited

Loading

LeroyR commented Oct 4, 2023 •

edited

Loading

leander2189 commented Oct 4, 2023 •

edited

Loading