Discontinuity in trajectory

[leander2189]

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]

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]

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]

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]

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]

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]

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]

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]

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]

@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]

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]

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]

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]

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.