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
174 stars 150 forks source link

Trajectory execution fails #478

Open ConstantRoux opened 1 year ago

ConstantRoux commented 1 year ago

Hello,

In order to perform pick and place tasks, I use Moveit2 with Moveit Task Constructor with ROS2 Humble on Ubuntu Jammy. I also use Gazebo Classic 11 as a simulator.

I'm currently able to plan a move using OMPL and Moveit2 and execute it in the simulator with the robot Panda. I am also able to plan the pick and place task having used the following tutorial.

The problem arises when I try to execute the pick and place then planned in the simulator. The task in progress suddenly stops (at the target object approach phase in my case) and the console displays the following error message:

[panda_arm_mtc-1]   1  - ←   5 →   -  3 / pick and place  
[panda_arm_mtc-1]     1  - ←   1 →   -  0 / current  
[panda_arm_mtc-1]     -  0 →   1 →   -  1 / open
[panda_arm_mtc-1]     -  1 →   2 ←   6  - / move to pick
[panda_arm_mtc-1]     6  - ←   6 →   -  6 / pick object
[panda_arm_mtc-1]       6  - ←   6 →   -  0 / grasp pose IK
[panda_arm_mtc-1]        25  - ←  25 →   - 25 / generate grasp pose
[panda_arm_mtc-1]       -  0 →   6 →   -  0 / allow collision (panda_gripper,object)
[panda_arm_mtc-1]       -  0 →   6 →   -  0 / attach object
[panda_arm_mtc-1]       -  0 →   6 →   -  6 / lift object
[panda_arm_mtc-1]     -  6 →   5 ←   3  - / move to place
[panda_arm_mtc-1]     3  - ←   3 →   -  0 / place object
[panda_arm_mtc-1]      20  - ←  20 →   -  6 / place pose IK
[panda_arm_mtc-1]        60  - ←  60 →   - 60 / generate place pose
[panda_arm_mtc-1]       -  6 →  14 →   -  0 / open
[panda_arm_mtc-1]       -  0 →  14 →   -  0 / forbid collision (hand,object)
[panda_arm_mtc-1]       -  0 →  14 →   -  0 / detach object
[panda_arm_mtc-1]       -  0 →   3 →   -  3 / retreat
[panda_arm_mtc-1]     -  0 →   3 →   -  3 / return home
[panda_arm_mtc-1] [ERROR] [1689838426.687436760] [moveit_task_constructor_executor_94734667822192]: Goal was aborted or canceled
[panda_arm_mtc-1] [ERROR] [1689838426.738783208] [panda_arm_mtc]: Task execution failed

I wonder if it could be a configuration problem and can provide more information if needed (source, launch files, logs, etc).

Thanks

leander2189 commented 1 year ago

To me it looks a bit more related to ros2_control than to MTC itself. Is the robot controller correctly loaded and active? Should have more log info in the terminal (unless you are showing only above ERROR level)

ConstantRoux commented 1 year ago

@leander2189 Thank you for your responsiveness.

I think that the controllers are activated because the execution of the task is done in part (opening the gripper, and moving to the area near the object).

I'm attaching the logs of the simulation, the pick and place task, the list of controllers and the list of hardware interfaces.

pick_and_place.log hardware.log control.log sim.log

I may be able to attach the configuration files for ros2 and moveit2.

leander2189 commented 1 year ago

I don't see anything strange in your log files. What I get normally is the following:

[move_group-25] [INFO 1689844393.872963569] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution (goalCallback() at /home/mtorres/ros/bmt/src/moveit_task_constructor/capabilities/src/execute_task_solution_capability.cpp:116)
[move_group-25] [INFO 1689844393.874176193] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.874234687] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.874319501] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.874399547] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.874478361] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.03 (validate() at ./trajectory_execution_manager/src/trajectory_execution_manager.cpp:1004)
[move_group-25] [INFO 1689844393.887434430] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... (executeThread() at ./trajectory_execution_manager/src/trajectory_execution_manager.cpp:1347)
[move_group-25] [INFO 1689844393.887575563] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.887624327] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.887997444] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.888044378] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.902665806] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.902787161] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844393.904017236] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller (sendTrajectory() at ./src/follow_joint_trajectory_controller_handle.cpp:58)
[move_group-25] [INFO 1689844393.906019800] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller started execution (operator()() at ./src/follow_joint_trajectory_controller_handle.cpp:70)
[move_group-25] [INFO 1689844393.906059863] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (operator()() at ./src/follow_joint_trajectory_controller_handle.cpp:74)
[ur_ros2_control_node-4] [INFO 1689844393.905130369] [joint_trajectory_controller]: Received new action goal (goal_received_callback() at ./src/joint_trajectory_controller.cpp:1166)
[ur_ros2_control_node-4] [INFO 1689844393.905210463] [joint_trajectory_controller]: Accepted new action goal (goal_received_callback() at ./src/joint_trajectory_controller.cpp:1181)
[ur_ros2_control_node-4] [INFO 1689844397.735749580] [joint_trajectory_controller]: Goal reached, success! (update() at ./src/joint_trajectory_controller.cpp:341)
[move_group-25] [INFO 1689844397.757054662] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller' successfully finished (controllerDoneCallback() at ./src/follow_joint_trajectory_controller_handle.cpp:224)
[move_group-25] [INFO 1689844397.766706295] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844397.766760690] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844397.773332375] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to joint_trajectory_controller_zstage (sendTrajectory() at ./src/follow_joint_trajectory_controller_handle.cpp:58)
[ur_ros2_control_node-4] [INFO 1689844397.773904009] [joint_trajectory_controller_zstage]: Received new action goal (goal_received_callback() at ./src/joint_trajectory_controller.cpp:1166)
[ur_ros2_control_node-4] [INFO 1689844397.773963740] [joint_trajectory_controller_zstage]: Accepted new action goal (goal_received_callback() at ./src/joint_trajectory_controller.cpp:1181)
[ur_ros2_control_node-4] [INFO 1689844397.774389108] [joint_trajectory_controller_zstage]: Goal reached, success! (update() at ./src/joint_trajectory_controller.cpp:341)
[move_group-25] [INFO 1689844397.776606710] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: joint_trajectory_controller_zstage started execution (operator()() at ./src/follow_joint_trajectory_controller_handle.cpp:70)
[move_group-25] [INFO 1689844397.776647064] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! (operator()() at ./src/follow_joint_trajectory_controller_handle.cpp:74)
[move_group-25] [INFO 1689844397.825637698] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'joint_trajectory_controller_zstage' successfully finished (controllerDoneCallback() at ./src/follow_joint_trajectory_controller_handle.cpp:224)
[move_group-25] [INFO 1689844397.825968640] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844397.826001776] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list (getControllersList() at ./src/moveit_simple_controller_manager.cpp:233)
[move_group-25] [INFO 1689844397.826158181] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ... (executeThread() at ./trajectory_execution_manager/src/trajectory_execution_manager.cpp:1370)

Please check your move_group and ros2_control logs

ConstantRoux commented 1 year ago

After checking, I get the same results (more or less). However, at times, it seems that the trajectory is completed, but is still considered to have failed. So I'm wondering whether there might be some parameters to modify (such as subtask timeout, JointTrajectoryController constraints or planer parameters) that would be responsible for the failure of a trajectory and therefore of the entire task.

I've attached the configuration files here: kinematics.txt ros2_controllers.txt ompl_planning.txt moveit_controllers.txt

Thank you

leander2189 commented 1 year ago

At some times I was getting an error like the one you are mentioning, but after I set this parameter it stopped returning errors

"trajectory_execution.trajectory_duration_monitoring": False,

Inside your moveit_controllers.txt > trajectory_execution

ConstantRoux commented 1 year ago

Thank you that solved my problem. All I had to do was set the trajectory_execution.execution_duration_monitoring parameter to false in the moveit_controllers.yaml. I've also modified the controller parameters to allow a little more control flexibility.

rhaschke commented 1 year ago

trajectory_execution.execution_duration_monitoring=False disables duration monitoring in MoveIt, i.e. MoveIt doesn't care about longer-than-expected execution anymore, but just considers ros_control's feedback.

Looks like the corresponding duration test is too restrictive. As ros_control already handles execution timing, I suggest to set this parameter to False by default. @ros-planning/moveit-maintainers, any comments?

yangming517 commented 4 months ago

Thank you that solved my problem. All I had to do was set the trajectory_execution.execution_duration_monitoring parameter to false in the moveit_controllers.yaml. I've also modified the controller parameters to allow a little more control flexibility.

hello,I have the same problem with you ,however,i add "trajectory_execution.trajectory_duration_monitoring: False" in gripper_moveit_controllers.yaml,it still doesn't work.I want to know what else modifications did you make。Looking forward to your reply。Thanks!!!!

MoveIt uses this configuration for controller management

trajectory_execution: allowed_execution_duration_scaling: 1.2 allowed_goal_duration_margin: 0.5 allowed_start_tolerance: 0.01 trajectory_execution.trajectory_duration_monitoring: False

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager: controller_names:

rhaschke commented 4 months ago

@yangming517, please provide the exact error message(s) you get from the controllers.

yangming517 commented 4 months ago

@rhaschke hello,The following is the information displayed on the terminal ym@ym-PC:~/ws_moveit$ ros2 launch moveit2_tutorials pick_place_demo.launch.py [INFO] [launch]: All log files can be found below /home/ym/.ros/log/2024-04-09-13-51-53-435834-ym-PC-9736 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [mtc_tutorial-1]: process started with pid [9737] [mtc_tutorial-1] [INFO] [1712641918.944142733] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.018954 seconds [mtc_tutorial-1] [INFO] [1712641918.944220381] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [mtc_tutorial-1] [INFO] [1712641919.043267672] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL' [mtc_tutorial-1] [INFO] [1712641919.045664005] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000 [mtc_tutorial-1] [INFO] [1712641919.045673385] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000 [mtc_tutorial-1] [INFO] [1712641919.045676724] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000 [mtc_tutorial-1] [INFO] [1712641919.045693099] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000 [mtc_tutorial-1] [INFO] [1712641919.045702120] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000 [mtc_tutorial-1] [INFO] [1712641919.045705183] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000 [mtc_tutorial-1] [INFO] [1712641919.045712416] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000 [mtc_tutorial-1] [INFO] [1712641919.045715537] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000 [mtc_tutorial-1] [INFO] [1712641919.045724314] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100 [mtc_tutorial-1] [INFO] [1712641919.045732268] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization' [mtc_tutorial-1] [INFO] [1712641919.045736239] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links' [mtc_tutorial-1] [INFO] [1712641919.045738417] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds' [mtc_tutorial-1] [INFO] [1712641919.045740342] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds' [mtc_tutorial-1] [INFO] [1712641919.045742124] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision' [mtc_tutorial-1] [INFO] [1712641919.045743973] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints' [mtc_tutorial-1] [WARN] [1712641919.212281500] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml. [mtc_tutorial-1] [INFO] [1712641919.371672882] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641919.487870442] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'hand' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641919.567272920] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641919.653796285] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641919.688985035] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'hand' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [WARN] [1712641919.757044684] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step. [mtc_tutorial-1] [INFO] [1712641919.862578621] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641919.910716049] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'hand' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641920.038755943] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641920.085337185] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'hand' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [WARN] [1712641920.212818889] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step. [mtc_tutorial-1] [INFO] [1712641920.410669337] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [INFO] [1712641920.460252714] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'hand' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [mtc_tutorial-1] [WARN] [1712641920.530614150] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step. [mtc_tutorial-1] [ERROR] [1712641923.393837668] [moveit_task_constructor_executor_95396015939936]: Goal was aborted or canceled [mtc_tutorial-1] [ERROR] [1712641923.409459629] [mtc_tutorial]: Task execution failed

yangming517 commented 4 months ago

@rhaschke Here is some information displayed by another terminal which run "ros2 launch moveit2_tutorials mtc_demo.launch.py" [move_group-4] You can start planning now! [move_group-4] [INFO] [ros2 run controller_manager spawner panda_hand_controller-7]: process has finished cleanly [pid 9699] [INFO] [ros2 run controller_manager spawner joint_state_broadcaster-8]: process has finished cleanly [pid 9701] [INFO] [ros2 run controller_manager spawner panda_arm_controller-6]: process has finished cleanly [pid 9697] [rviz2-1] [INFO] [1712641919.553484907] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0361462 seconds [rviz2-1] [INFO] [1712641919.553552907] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [rviz2-1] [INFO] [1712641920.198942618] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.00113381 seconds [rviz2-1] [INFO] [1712641920.198964012] [moveit_robot_model.robot_model]: Loading robot model 'panda'... [rviz2-1] [WARN] [1712641920.207527303] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [rviz2-1] [INFO] [1712641920.252549742] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor [rviz2-1] [INFO] [1712641920.253503625] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene' [move_group-4] [WARN] [1712641920.538221047] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '3' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538308988] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '4' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538343328] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '6' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538375662] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '10' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538482519] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '12' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538559703] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '13' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538590742] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '17' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538646799] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '20' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [WARN] [1712641920.538726919] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '21' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection. [move_group-4] [INFO] [1712641920.538732970] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution [move_group-4] [INFO] [1712641920.538770983] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538782176] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538818749] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538824031] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538844233] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538848846] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538859374] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538863149] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538877652] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538881958] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538900711] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538905318] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538924583] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538929001] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538959699] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538965213] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538983338] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.538987689] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.539015958] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [move_group-4] [INFO] [1712641920.540547496] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... [move_group-4] [INFO] [1712641920.540577572] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.540588330] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [ERROR] [1712641920.540635804] [moveit_robot_state.conversions]: Found empty JointState message [move_group-4] [INFO] [1712641920.540810055] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.540820711] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [ros2_control_node-5] [INFO] [1712641920.541006659] [panda_hand_controller]: Received & accepted new action goal [move_group-4] [INFO] [1712641920.591584373] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.591604813] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641920.591668424] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to panda_arm_controller [move_group-4] [INFO] [1712641920.591929701] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution [move_group-4] [INFO] [1712641920.591942446] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! [ros2_control_node-5] [INFO] [1712641920.591838234] [panda_arm_controller]: Received new action goal [ros2_control_node-5] [INFO] [1712641920.591870594] [panda_arm_controller]: Accepted new action goal [ros2_control_node-5] [INFO] [1712641922.639949518] [panda_arm_controller]: Goal reached, success! [move_group-4] [INFO] [1712641922.642187493] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'panda_arm_controller' successfully finished [move_group-4] [ERROR] [1712641922.642231713] [moveit_robot_state.conversions]: Found empty JointState message [move_group-4] [INFO] [1712641922.642415475] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641922.642427431] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641922.642487974] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to panda_arm_controller [ros2_control_node-5] [INFO] [1712641922.642643030] [panda_arm_controller]: Received new action goal [ros2_control_node-5] [INFO] [1712641922.642665677] [panda_arm_controller]: Accepted new action goal [move_group-4] [INFO] [1712641922.642802330] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution [move_group-4] [INFO] [1712641922.642811339] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted! [ros2_control_node-5] [INFO] [1712641923.299994305] [panda_arm_controller]: Goal reached, success! [move_group-4] [INFO] [1712641923.342939026] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'panda_arm_controller' successfully finished [move_group-4] [ERROR] [1712641923.342980379] [moveit_robot_state.conversions]: Found empty JointState message [move_group-4] [INFO] [1712641923.343052104] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641923.343062714] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [ERROR] [1712641923.343081854] [moveit_robot_state.conversions]: Found empty JointState message [move_group-4] [INFO] [1712641923.343118846] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641923.343125123] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641923.343209705] [moveit_ros.plan_execution]: Trajectory component '7/16' is invalid [move_group-4] [INFO] [1712641923.343223688] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored. [move_group-4] [INFO] [1712641923.343227025] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored) [move_group-4] [INFO] [1712641923.343244263] [moveit_ros.plan_execution]: Upcoming trajectory component '7/16' is invalid [move_group-4] [INFO] [1712641923.343249780] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1712641923.343254322] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list [ros2_control_node-5] [INFO] [1712641923.343350495] [panda_hand_controller]: Received & accepted new action goal [move_group-4] [INFO] [1712641923.350640864] [moveit_ros.plan_execution]: Trajectory component '7/16' is invalid [move_group-4] [INFO] [1712641923.350657463] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored. [move_group-4] [INFO] [1712641923.350661130] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored) [move_group-4] [INFO] [1712641923.350664597] [moveit_ros.plan_execution]: Trajectory component '7/16' is invalid after scene update [move_group-4] [INFO] [1712641923.350667122] [moveit_ros.plan_execution]: Stopping execution because the path to execute became invalid(probably the environment changed) [move_group-4] [INFO] [1712641923.350675029] [moveit.simple_controller_manager.gripper_controller_handle]: Cancelling execution for panda_hand_controller [ros2_control_node-5] [INFO] [1712641923.350788890] [panda_hand_controller]: Got request to cancel goal [ros2_control_node-5] [INFO] [1712641923.350817611] [panda_hand_controller]: Canceling active action goal because cancel callback received. [move_group-4] [INFO] [1712641923.350903528] [moveit_ros.trajectory_execution_manager]: Stopped trajectory execution. [move_group-4] [INFO] [1712641923.393619067] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status PREEMPTED ...

rhaschke commented 4 months ago

The second log shows that 6/16 trajectory components are executed successfully. Thus, trajectory execution works in principle. The 7th component is invalid due to a collision:

[move_group-4] [INFO] [1712641923.343209705] [moveit_ros.plan_execution]: Trajectory component '7/16' is invalid
[move_group-4] [INFO] [1712641923.343223688] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'object' (type 'Object') and 'panda_leftfinger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.

May I ask, which version of MoveIt and MTC you are using?

yangming517 commented 4 months ago

@rhaschke I'm so sorry to bother you, but I realized that I missed this step https://github.com/ros-planning/moveit_task_constructor/commit/b40e932da113fddc434f2cd503bee4ec2b9a45a4 .Thank you very much