search

moveit / moveit_task_constructor

A hierarchical multi-stage manipulation planner
https://moveit.github.io/moveit_task_constructor
BSD 3-Clause "New" or "Revised" License
176 stars 150 forks source link

Discontinuity in trajectory #485

Closed leander2189 closed 1 year ago

leander2189 commented 1 year ago

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

https://github.com/ros-planning/moveit_task_constructor/assets/95743148/e4637737-23ec-4d62-a96d-76976d26cc19

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

@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 commented 1 year ago

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 commented 1 year ago

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 commented 1 year ago

I noticed several issues:

Abishalini commented 1 year ago

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 - https://github.com/PickNikRobotics/pick_ik/pull/54) and hence we would get partial solutions if the end goal was out of joint limits.

rhaschke commented 1 year ago

I found the underlying culprit in MoveIt2. A fix is proposed in https://github.com/ros-planning/moveit2/pull/2455.

leander2189 commented 1 year ago

Awesome!