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