ompl planer in MTC fails to execute.

[LordAbraham]

Hi,

I have weird problem in MTC ROS2 Humble branch. Problem is created only while executing ompl path, on pilz everything works as it should. I tried setting TimeOptimalTrajectoryGeneration manually in properties of planer to match my moveits config. In rviz and by moveit api ompl works fine, problem is only in MTC.

my code:

 (...)
    // pilz planner
    pilz_planner = std::make_shared<solvers::PipelinePlanner>(node_, "pilz");
    pilz_planner->setProperty("goal_joint_tolerance", 1e-5);
    pilz_planner->setPlannerId("PTP");
    pilz_planner->setMaxAccelerationScalingFactor(acc);
    pilz_planner->setMaxVelocityScalingFactor(vel);

    // ompl planner
    ompl_planner = std::make_shared<solvers::PipelinePlanner>(node_, "ompl");
    ompl_planner->setProperty("goal_joint_tolerance", 1e-4);
    ompl_planner->setPlannerId(ompl_planner_);
    ompl_planner->setMaxAccelerationScalingFactor(acc);
    ompl_planner->setMaxVelocityScalingFactor(vel);

      (...)
            {
                auto current_state = std::make_unique<stages::CurrentState>("current state");
                task_->add(std::move(current_state));
            }
           {
            auto fallbacks_conectors = std::make_unique<Fallbacks>("Fallbacks to ready");
            fallbacks_conectors->properties().configureInitFrom(Stage::PARENT);
            {
                auto stage = std::make_unique<stages::MoveTo>("move to ready ompl", ompl_planner);
                stage->setGroup(group_);
                stage->setGoal("ready");
                stage->restrictDirection(stages::MoveTo::FORWARD);
                fallbacks_conectors->add(std::move(stage));
            }
            {
                auto stage = std::make_unique<stages::MoveTo>("move to ready pilz", pilz_planner);
                stage->setGroup(group_);
                stage->setGoal("ready");
                stage->restrictDirection(stages::MoveTo::FORWARD);
                fallbacks_conectors->add(std::move(stage));
            }

            task_->add(std::move(fallbacks_conectors));
        }  (...) 

While executing plan i get this error:

[move_group-2] [INFO] [1717170120.129316713] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution

[move_group-2] [INFO] [1717170120.159596949] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list

[move_group-2] [INFO] [1717170120.159778197] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to iiwa_arm_controller

[ros2_control_node-1] [INFO] [1717170120.159907682] [iiwa_arm_controller]: Received new action goal

[ros2_control_node-1] [ERROR] [1717170120.159947347] [iiwa_arm_controller]: Time between points 0 and 1 is not strictly increasing, it is 0.000000 and 0.000000 respectively

[move_group-2] [INFO] [1717170120.160058743] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: iiwa_arm_controller started execution

[move_group-2] [WARN] [1717170120.160072314] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected

[move_group-2] [ERROR] [1717170120.160099559] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server

[move_group-2] [ERROR] [1717170120.160110414] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller iiwa_arm_controller

[move_group-2] [INFO] [1717170120.160114068] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...

[pick_place_demo-4] [ERROR] [1717170120.163506721] [moveit_task_constructor_executor_94306724871968]: Goal was aborted or canceled

[rhaschke]

According to the error message, the controller complains about non-increasing time between waypoints 0 and 1. This is a problem of the timing generation in MoveIt. Not an MTC issue.

[LordAbraham]

Hi, thanks for response. I know it's because of problem with ompl timings, but this problem for me is exclusive for MTC. Nevertheless, I will try to find answer on ompl thread.

[rhaschke]

I don't think this is OMPL-related. Only the time parameterization is failing.

[robotics-qc]

@LordAbraham I had a very similar issue and I am not sure if it s relevant to you, but when you create your ros node make sure to override parameters from defaults

     rclcpp::NodeOptions options;
    options.automatically_declare_parameters_from_overrides(true); //Not having this caused my planner to give zero time to all trajectory points - so cartesian planners work but OMPL plannners do not.

    std::shared_ptr<CustomMtcPipeline> mtc_node = std::make_shared<CustomMtcPipeline>(options);