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);