diff --git a/xarm_moveit_config/launch/_robot_moveit_common.launch.py b/xarm_moveit_config/launch/_robot_moveit_common.launch.py
index b4033c8f..116a645c 100644
--- a/xarm_moveit_config/launch/_robot_moveit_common.launch.py
+++ b/xarm_moveit_config/launch/_robot_moveit_common.launch.py
@@ -60,6 +60,7 @@ def launch_setup(context, *args, **kwargs):
     kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='')
 
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
+    add_mtc = LaunchConfiguration('add_mtc', default=False)
 
     moveit_config_package_name = 'xarm_moveit_config'
     xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context) if robot_type.perform(context) in ('xarm', 'lite') else '')
@@ -237,21 +238,26 @@ def launch_setup(context, *args, **kwargs):
     #     'ros.filtered_cloud_topic': 'filtered_cloud',
     # }
 
+    # MTC ExecuteTaskSolution capability (requires ros-${ROS_DISTRO}-moveit-task-constructor-capabilities)
+    move_group_params = [
+        robot_description_parameters,
+        ompl_planning_pipeline_config,
+        trajectory_execution,
+        plan_execution,
+        moveit_controllers,
+        planning_scene_monitor_parameters,
+        # sensor_manager_parameters,
+        {'use_sim_time': use_sim_time},
+    ]
+    if add_mtc.perform(context) in ('True', 'true'):
+        move_group_params.append({'capabilities': 'move_group/ExecuteTaskSolutionCapability'})
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package='moveit_ros_move_group',
         executable='move_group',
         output='screen',
-        parameters=[
-            robot_description_parameters,
-            ompl_planning_pipeline_config,
-            trajectory_execution,
-            plan_execution,
-            moveit_controllers,
-            planning_scene_monitor_parameters,
-            # sensor_manager_parameters,
-            {'use_sim_time': use_sim_time},
-        ],
+        parameters=move_group_params,
     )
 
     # rviz with moveit configuration
diff --git a/xarm_moveit_config/launch/_robot_moveit_common2.launch.py b/xarm_moveit_config/launch/_robot_moveit_common2.launch.py
index fdc32553..efae4e91 100644
--- a/xarm_moveit_config/launch/_robot_moveit_common2.launch.py
+++ b/xarm_moveit_config/launch/_robot_moveit_common2.launch.py
@@ -30,20 +30,26 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
     moveit_config_dump = LaunchConfiguration('moveit_config_dump')
     rviz_config = LaunchConfiguration('rviz_config', default='')
+    add_mtc = LaunchConfiguration('add_mtc', default=False)
     
     moveit_config_dump = moveit_config_dump.perform(context)
     moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader)
     moveit_config_package_name = 'xarm_moveit_config'
 
+    # MTC ExecuteTaskSolution capability (requires ros-${ROS_DISTRO}-moveit-task-constructor-capabilities)
+    move_group_params = [
+        moveit_config_dict,
+        {'use_sim_time': use_sim_time},
+    ]
+    if add_mtc.perform(context) in ('True', 'true'):
+        move_group_params.append({'capabilities': 'move_group/ExecuteTaskSolutionCapability'})
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package='moveit_ros_move_group',
         executable='move_group',
         output='screen',
-        parameters=[
-            moveit_config_dict,
-            {'use_sim_time': use_sim_time},
-        ],
+        parameters=move_group_params,
     )
 
     # rviz with moveit configuration
diff --git a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py
index 8782ff21..46af1bb8 100644
--- a/xarm_moveit_config/launch/_robot_moveit_fake.launch.py
+++ b/xarm_moveit_config/launch/_robot_moveit_fake.launch.py
@@ -54,6 +54,7 @@ def launch_setup(context, *args, **kwargs):
     geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
 
     no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+    add_mtc = LaunchConfiguration('add_mtc', default=False)
     ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
 
     ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware'
@@ -106,7 +107,7 @@ def launch_setup(context, *args, **kwargs):
         geometry_mesh_tcp_xyz=geometry_mesh_tcp_xyz,
         geometry_mesh_tcp_rpy=geometry_mesh_tcp_rpy,
     ).to_moveit_configs()
-    
+
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
@@ -126,6 +127,7 @@ def launch_setup(context, *args, **kwargs):
             'attach_xyz': attach_xyz,
             'attach_rpy': attach_rpy,
             'no_gui_ctrl': no_gui_ctrl,
+            'add_mtc': add_mtc,
             'use_sim_time': 'false',
             'moveit_config_dump': yaml.dump(moveit_config.to_dict()),
         }.items(),
diff --git a/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py b/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py
index 61304972..98a40437 100644
--- a/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py
+++ b/xarm_moveit_config/launch/xarm6_moveit_fake.launch.py
@@ -15,6 +15,7 @@
 
 def generate_launch_description():
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    add_mtc = LaunchConfiguration('add_mtc', default='false')
 
     # robot moveit fake launch
     # xarm_moveit_config/launch/_robot_moveit_fake.launch.py
@@ -25,6 +26,7 @@ def generate_launch_description():
             'robot_type': 'xarm',
             'hw_ns': hw_ns,
             'no_gui_ctrl': 'false',
+            'add_mtc': add_mtc,
         }.items(),
     )