Found MTC plan is not executed but rejected by joint_trajectory_controller: Time between points 0 and 1 is not strictly increasing

[kirienkomaxym]

Hi MoveIt developers, thank you for your awesome work!

I'm encountering an unusual and undocumented issue with tasks created by the MoveIt Task Constructor (MTC) for pick-and-place operations. The issue is as follows: while MTC successfully plans the pick-and-place task, an error occurs upon execution. The scaled_joint_trajectory_controller reports the following:

[ur_ros2_control_node-1] [ERROR] [1730205942.890204145] [scaled_joint_trajectory_controller]: Time between points 0 and 1 is not strictly increasing, it is 0.000000 and 0.000000 respectively

Additionally, the MTC node produces these messages:

[move_group-1] [INFO] [1730206740.960265483] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: scaled_joint_trajectory_controller started execution
[move_group-1] [WARN] [1730206740.960283408] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-1] [ERROR] [1730206740.960266909] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-1] [ERROR] [1730206740.960312030] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller scaled_joint_trajectory_controller
[move_group-1] [INFO] [1730206740.960320506] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[pick_place_demo-4] [ERROR] [1730206740.969351652] [moveit_task_constructor_executor_110907598154160]: Goal was aborted or canceled
[pick_place_demo-4] [ERROR] [1730206740.972013312] [mercurio_moveit_task_constructor]: Task execution failed and returned: 99999
[pick_place_demo-4] [ERROR] [1730206740.972029861] [pick_place_service_node]: Failed to execute PickPlaceTask

Upon investigation, I found that the issue might be related to a missing default_planner_request_adapters/AddTimeOptimalParameterization. However, this adapter is included in my launch file configuration:

ompl_planning_pipeline_config = {
    "move_group": {
        "planning_plugin": "ompl_interface/OMPLPlanner",
        "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
        "start_state_max_bounds_error": 0.1,
    }
}
ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

Interestingly, if I perform motion planning directly in RViz2 by dragging the robot pose and using "Plan & Execute," it succeeds, and the motion reflects correctly in my URSim robot emulator.

Switching from scaled_joint_trajectory_controller to joint_trajectory_controller did not resolve the issue. My MoveIt settings were generated with Moveit Setup Assistant, and my .xacro file I put into Moveit Setup Assistant is created by combining UR robot URDF.xacro from ur_description with the Robotiq gripper description .xacro from robotiq_description.

I've provided a repository containing all relevant data, including full log files and launch commands.

My package versions are as follows:

ROS2 distro: Humble
ur_robot_driver: 2.2.15
moveit2: 2.2.5
moveit_task_constructor_core: 0.1.3

Thank you for any advice!

[rhaschke]

I have seen similar issue reports, e.g. #578. However, I was never able to reproduce the issue. Your linked repository doesn't work out-of-the-box as well as it has many undeclared dependencies.
So far, I'm not sure if this is simply a node configuration problem, a problem with the time parameterization plugin, or a generic MTC issue. It would be great if you could reproduce your issue with the standard demos included with MTC.

[kirienkomaxym]

@rhaschke The advise given in the PR #576 is really working. Im sorry for disturbing you. Thank you very much!

[rhaschke]

I guess this is a typo and you are referring to #578 (comment)?
If so, this simply means that parameters (like the planning adapter definition for the pipeline) didn't actually show up in the node.
I guess we can close this?