From 03110c7da82e9fe62f39cb5164ee66705f6c5e41 Mon Sep 17 00:00:00 2001
From: Vinman <vinman.cub@gmail.com>
Date: Mon, 18 Sep 2023 17:20:52 +0800
Subject: [PATCH] [fix] Organize the configuration files of the
 `xarm_controller`

---
 .../launch/realMove_exec.launch               |  10 +-
 .../xarm5_vacuum_gripper_moveit_gazebo.launch |   4 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../xarm6_vacuum_gripper_moveit_gazebo.launch |   4 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../xarm7_vacuum_gripper_moveit_gazebo.launch |   4 +-
 .../launch/lite6_moveit_gazebo.launch         |   4 +-
 .../launch/realMove_exec.launch               |   9 +-
 ...azebo_controllers_with_gripper_action.yaml |  18 ++
 ...azebo_controllers_with_gripper_action.yaml |  17 ++
 ...azebo_controllers_with_gripper_action.yaml |  18 ++
 ...azebo_controllers_with_gripper_action.yaml |  19 ++
 .../launch/__move_group.launch                |  25 ++-
 ..._moveit_controller_manager_fake.launch.xml |  12 +-
 ...oveit_controller_manager_gazebo.launch.xml |  12 +-
 ...moveit_controller_manager_robot.launch.xml |  11 +-
 .../launch/__moveit_rviz.launch               |  12 +-
 .../launch/__moveit_rviz_common.launch        |  11 +-
 .../launch/__trajectory_execution.launch.xml  |  12 +-
 .../launch/_dual_robot_moveit_fake.launch     |   2 +-
 .../launch/_dual_robot_moveit_gazebo.launch   |  14 +-
 .../launch/_dual_robot_moveit_realmove.launch |  18 +-
 .../launch/_robot_moveit_fake.launch          |   7 +-
 .../launch/_robot_moveit_gazebo.launch        |  14 +-
 .../launch/_robot_moveit_realmove.launch      |  12 +-
 .../launch/dual_lite6_moveit_realmove.launch  |   2 +-
 .../launch/dual_uf850_moveit_realmove.launch  |   2 +-
 .../launch/dual_xarm5_moveit_realmove.launch  |   2 +-
 .../launch/dual_xarm6_moveit_realmove.launch  |   2 +-
 .../launch/dual_xarm7_moveit_realmove.launch  |   2 +-
 .../launch/lite6_moveit_realmove.launch       |   2 +-
 .../launch/uf850_moveit_realmove.launch       |   2 +-
 .../launch/xarm5_moveit_realmove.launch       |   2 +-
 .../launch/xarm6_moveit_realmove.launch       |   2 +-
 .../launch/xarm7_moveit_realmove.launch       |   2 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm5_gripper_moveit_gazebo.launch |   9 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm5_moveit_gazebo.launch         |   8 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm6_gripper_moveit_gazebo.launch |   9 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm6_moveit_gazebo.launch         |   8 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm7_gripper_moveit_gazebo.launch |  11 +-
 .../launch/realMove_exec.launch               |   9 +-
 .../launch/xarm7_moveit_gazebo.launch         |   8 +-
 .../dual_lite6_position_controllers.yaml      |  68 ------
 .../config/dual_lite6_traj_controller.yaml    | 130 ------------
 .../dual_uf850_position_controllers.yaml      |  68 ------
 .../config/dual_uf850_traj_controller.yaml    | 130 ------------
 .../dual_xarm5_position_controllers.yaml      |  60 ------
 .../config/dual_xarm5_traj_controller.yaml    | 118 -----------
 .../dual_xarm6_position_controllers.yaml      |  61 ------
 .../config/dual_xarm6_traj_controller.yaml    | 120 -----------
 .../dual_xarm7_position_controllers.yaml      |  76 -------
 .../config/dual_xarm7_traj_controller.yaml    | 144 -------------
 .../dual_gripper_controllers.yaml}            |  29 ++-
 .../dual_gripper_gazebo_ros_control.yaml      |   0
 .../gripper_controllers.yaml}                 |  15 +-
 .../gripper_gazebo_ros_control.yaml           |   0
 .../config/lite6/dual_lite6_controllers.yaml  | 174 ++++++++++++++++
 .../lite6_controllers.yaml}                   |  41 ++--
 .../config/lite6_position_controllers.yaml    |  43 ----
 .../config/uf850/dual_uf850_controllers.yaml  | 174 ++++++++++++++++
 .../uf850_controllers.yaml}                   |  41 ++--
 .../config/uf850_position_controllers.yaml    |  43 ----
 .../config/xarm5/dual_xarm5_controllers.yaml  | 154 ++++++++++++++
 .../xarm5_controllers.yaml}                   |  37 ++--
 .../config/xarm5_position_controllers.yaml    |  39 ----
 .../config/xarm6/dual_xarm6_controllers.yaml  | 174 ++++++++++++++++
 .../xarm6_controllers.yaml}                   |  41 ++--
 .../config/xarm6_position_controllers.yaml    |  43 ----
 .../config/xarm7/dual_xarm7_controllers.yaml  | 194 ++++++++++++++++++
 .../xarm7_controllers.yaml}                   |  47 +++--
 .../config/xarm7_effort_controllers.yaml      |  67 ------
 .../config/xarm7_position_controllers.yaml    |  47 -----
 .../config/xarm_gripper_controller.yaml       |  13 --
 xarm_controller/launch/_robot_control.launch  |  21 +-
 .../launch/dual_xarm6_control.launch          |  15 +-
 xarm_controller/launch/lite6_control.launch   |  15 +-
 xarm_controller/launch/uf850_control.launch   |  15 +-
 xarm_controller/launch/xarm5_control.launch   |  17 +-
 xarm_controller/launch/xarm6_control.launch   |  15 +-
 xarm_controller/launch/xarm7_control.launch   |  18 +-
 .../launch/_dual_robot_beside_table.launch    |   2 +-
 xarm_gazebo/launch/_robot_beside_table.launch |   4 +-
 xarm_gazebo/launch/xarm5_beside_table.launch  |   2 +-
 xarm_gazebo/launch/xarm6_beside_table.launch  |   2 +-
 xarm_gazebo/launch/xarm7_beside_table.launch  |   2 +-
 xarm_gazebo/launch/xarm_camera_scene.launch   |  10 +-
 xarm_planner/launch/robot_planner_fake.launch |   2 +-
 .../launch/robot_planner_realmove.launch      |   2 +-
 93 files changed, 1378 insertions(+), 1569 deletions(-)
 create mode 100644 uf_robot_moveit_config/config/uf850/gazebo_controllers_with_gripper_action.yaml
 create mode 100644 uf_robot_moveit_config/config/xarm5/gazebo_controllers_with_gripper_action.yaml
 create mode 100644 uf_robot_moveit_config/config/xarm6/gazebo_controllers_with_gripper_action.yaml
 create mode 100644 uf_robot_moveit_config/config/xarm7/gazebo_controllers_with_gripper_action.yaml
 delete mode 100755 xarm_controller/config/dual_lite6_position_controllers.yaml
 delete mode 100755 xarm_controller/config/dual_lite6_traj_controller.yaml
 delete mode 100755 xarm_controller/config/dual_uf850_position_controllers.yaml
 delete mode 100755 xarm_controller/config/dual_uf850_traj_controller.yaml
 delete mode 100644 xarm_controller/config/dual_xarm5_position_controllers.yaml
 delete mode 100644 xarm_controller/config/dual_xarm5_traj_controller.yaml
 delete mode 100755 xarm_controller/config/dual_xarm6_position_controllers.yaml
 delete mode 100755 xarm_controller/config/dual_xarm6_traj_controller.yaml
 delete mode 100755 xarm_controller/config/dual_xarm7_position_controllers.yaml
 delete mode 100755 xarm_controller/config/dual_xarm7_traj_controller.yaml
 rename xarm_controller/config/{dual_gripper_traj_controller.yaml => gripper/dual_gripper_controllers.yaml} (78%)
 rename xarm_controller/config/{ => gripper}/dual_gripper_gazebo_ros_control.yaml (100%)
 rename xarm_controller/config/{gripper_traj_controller.yaml => gripper/gripper_controllers.yaml} (79%)
 rename xarm_controller/config/{ => gripper}/gripper_gazebo_ros_control.yaml (100%)
 create mode 100644 xarm_controller/config/lite6/dual_lite6_controllers.yaml
 rename xarm_controller/config/{lite6_traj_controller.yaml => lite6/lite6_controllers.yaml} (63%)
 mode change 100755 => 100644
 delete mode 100755 xarm_controller/config/lite6_position_controllers.yaml
 create mode 100644 xarm_controller/config/uf850/dual_uf850_controllers.yaml
 rename xarm_controller/config/{uf850_traj_controller.yaml => uf850/uf850_controllers.yaml} (63%)
 mode change 100755 => 100644
 delete mode 100755 xarm_controller/config/uf850_position_controllers.yaml
 create mode 100644 xarm_controller/config/xarm5/dual_xarm5_controllers.yaml
 rename xarm_controller/config/{xarm5_traj_controller.yaml => xarm5/xarm5_controllers.yaml} (64%)
 delete mode 100644 xarm_controller/config/xarm5_position_controllers.yaml
 create mode 100644 xarm_controller/config/xarm6/dual_xarm6_controllers.yaml
 rename xarm_controller/config/{xarm6_traj_controller.yaml => xarm6/xarm6_controllers.yaml} (63%)
 mode change 100755 => 100644
 delete mode 100755 xarm_controller/config/xarm6_position_controllers.yaml
 create mode 100644 xarm_controller/config/xarm7/dual_xarm7_controllers.yaml
 rename xarm_controller/config/{xarm7_traj_controller.yaml => xarm7/xarm7_controllers.yaml} (63%)
 mode change 100755 => 100644
 delete mode 100755 xarm_controller/config/xarm7_effort_controllers.yaml
 delete mode 100755 xarm_controller/config/xarm7_position_controllers.yaml
 delete mode 100644 xarm_controller/config/xarm_gripper_controller.yaml

diff --git a/examples/xarm5_vacuum_gripper_moveit_config/launch/realMove_exec.launch b/examples/xarm5_vacuum_gripper_moveit_config/launch/realMove_exec.launch
index 2f10976e..1b8e0de2 100644
--- a/examples/xarm5_vacuum_gripper_moveit_config/launch/realMove_exec.launch
+++ b/examples/xarm5_vacuum_gripper_moveit_config/launch/realMove_exec.launch
@@ -35,8 +35,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm5_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm5/xarm5_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -44,11 +44,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- from="/follow_joint_trajectory" should not remove "/" -->
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm5_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <!-- common part -->
   <remap from="/tf" to="$(arg ext_ns)/tf" />
diff --git a/examples/xarm5_vacuum_gripper_moveit_config/launch/xarm5_vacuum_gripper_moveit_gazebo.launch b/examples/xarm5_vacuum_gripper_moveit_config/launch/xarm5_vacuum_gripper_moveit_gazebo.launch
index 56418c0d..224c6480 100644
--- a/examples/xarm5_vacuum_gripper_moveit_config/launch/xarm5_vacuum_gripper_moveit_gazebo.launch
+++ b/examples/xarm5_vacuum_gripper_moveit_config/launch/xarm5_vacuum_gripper_moveit_gazebo.launch
@@ -7,7 +7,9 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/xarm5_traj_controller/follow_joint_trajectory"/>
+  <arg name="velocity_control" default="false"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm5_vacuum_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/examples/xarm6_vacuum_gripper_moveit_config/launch/realMove_exec.launch b/examples/xarm6_vacuum_gripper_moveit_config/launch/realMove_exec.launch
index 5c31fefe..10ab1095 100644
--- a/examples/xarm6_vacuum_gripper_moveit_config/launch/realMove_exec.launch
+++ b/examples/xarm6_vacuum_gripper_moveit_config/launch/realMove_exec.launch
@@ -35,8 +35,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm6_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm6/xarm6_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -44,10 +44,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm6_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <!-- common part -->
   <remap from="/tf" to="$(arg ext_ns)/tf" />
diff --git a/examples/xarm6_vacuum_gripper_moveit_config/launch/xarm6_vacuum_gripper_moveit_gazebo.launch b/examples/xarm6_vacuum_gripper_moveit_config/launch/xarm6_vacuum_gripper_moveit_gazebo.launch
index 75f500b2..5107b0f1 100644
--- a/examples/xarm6_vacuum_gripper_moveit_config/launch/xarm6_vacuum_gripper_moveit_gazebo.launch
+++ b/examples/xarm6_vacuum_gripper_moveit_config/launch/xarm6_vacuum_gripper_moveit_gazebo.launch
@@ -7,7 +7,9 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/xarm6_traj_controller/follow_joint_trajectory"/>
+  <arg name="velocity_control" default="false"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm6_vacuum_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/examples/xarm7_vacuum_gripper_moveit_config/launch/realMove_exec.launch b/examples/xarm7_vacuum_gripper_moveit_config/launch/realMove_exec.launch
index db4a2fcf..d6acd5ac 100644
--- a/examples/xarm7_vacuum_gripper_moveit_config/launch/realMove_exec.launch
+++ b/examples/xarm7_vacuum_gripper_moveit_config/launch/realMove_exec.launch
@@ -35,8 +35,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm7_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm7/xarm7_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -44,10 +44,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm7_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <!-- common part -->
   <remap from="/tf" to="$(arg ext_ns)/tf" />
diff --git a/examples/xarm7_vacuum_gripper_moveit_config/launch/xarm7_vacuum_gripper_moveit_gazebo.launch b/examples/xarm7_vacuum_gripper_moveit_config/launch/xarm7_vacuum_gripper_moveit_gazebo.launch
index 26f2526a..c360fb16 100644
--- a/examples/xarm7_vacuum_gripper_moveit_config/launch/xarm7_vacuum_gripper_moveit_gazebo.launch
+++ b/examples/xarm7_vacuum_gripper_moveit_config/launch/xarm7_vacuum_gripper_moveit_gazebo.launch
@@ -7,7 +7,9 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/xarm7_traj_controller/follow_joint_trajectory"/>
+  <arg name="velocity_control" default="false"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm7_vacuum_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/lite6_moveit_config/launch/lite6_moveit_gazebo.launch b/lite6_moveit_config/launch/lite6_moveit_gazebo.launch
index 03861b6e..2cab0268 100755
--- a/lite6_moveit_config/launch/lite6_moveit_gazebo.launch
+++ b/lite6_moveit_config/launch/lite6_moveit_gazebo.launch
@@ -22,8 +22,8 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg name="uf_traj_controller" value="$(eval 'lite6_traj_controller_velocity' if arg('velocity_control') else 'lite6_traj_controller')" />
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg uf_traj_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'lite6_velo_traj_controller' if arg('velocity_control') else 'lite6_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find lite6_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/lite6_moveit_config/launch/realMove_exec.launch b/lite6_moveit_config/launch/realMove_exec.launch
index a92a9504..cb4dbda7 100755
--- a/lite6_moveit_config/launch/realMove_exec.launch
+++ b/lite6_moveit_config/launch/realMove_exec.launch
@@ -47,8 +47,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/lite6_traj_controller.yaml" command="load" ns="$(arg uf_hw_ns)"/>
-  <arg name="uf_traj_controller" value="$(eval 'lite6_traj_controller_velocity' if arg('velocity_control') else 'lite6_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/lite6/lite6_controllers.yaml" command="load" ns="$(arg uf_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'lite6_velo_traj_controller' if arg('velocity_control') else 'lite6_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -56,10 +56,9 @@
     respawn="false"
     output="screen"
     ns="$(arg uf_hw_ns)"
-    args="$(arg uf_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/lite6_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg uf_hw_ns)/$(arg uf_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg uf_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
 
   <!-- common part -->
   <remap from="/tf" to="$(arg ext_ns)/tf" />
diff --git a/uf_robot_moveit_config/config/uf850/gazebo_controllers_with_gripper_action.yaml b/uf_robot_moveit_config/config/uf850/gazebo_controllers_with_gripper_action.yaml
new file mode 100644
index 00000000..fb360db0
--- /dev/null
+++ b/uf_robot_moveit_config/config/uf850/gazebo_controllers_with_gripper_action.yaml
@@ -0,0 +1,18 @@
+controller_list:
+  - name: ""
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+
+  - name: gripper_controller
+    action_ns: gripper_action
+    type: GripperCommand
+    joints:
+     - drive_joint
+    allow_failure: true
diff --git a/uf_robot_moveit_config/config/xarm5/gazebo_controllers_with_gripper_action.yaml b/uf_robot_moveit_config/config/xarm5/gazebo_controllers_with_gripper_action.yaml
new file mode 100644
index 00000000..82373e56
--- /dev/null
+++ b/uf_robot_moveit_config/config/xarm5/gazebo_controllers_with_gripper_action.yaml
@@ -0,0 +1,17 @@
+controller_list:
+  - name: ""
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+
+  - name: gripper_controller
+    action_ns: gripper_action
+    type: GripperCommand
+    joints:
+     - drive_joint
+    allow_failure: true
diff --git a/uf_robot_moveit_config/config/xarm6/gazebo_controllers_with_gripper_action.yaml b/uf_robot_moveit_config/config/xarm6/gazebo_controllers_with_gripper_action.yaml
new file mode 100644
index 00000000..fb360db0
--- /dev/null
+++ b/uf_robot_moveit_config/config/xarm6/gazebo_controllers_with_gripper_action.yaml
@@ -0,0 +1,18 @@
+controller_list:
+  - name: ""
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+
+  - name: gripper_controller
+    action_ns: gripper_action
+    type: GripperCommand
+    joints:
+     - drive_joint
+    allow_failure: true
diff --git a/uf_robot_moveit_config/config/xarm7/gazebo_controllers_with_gripper_action.yaml b/uf_robot_moveit_config/config/xarm7/gazebo_controllers_with_gripper_action.yaml
new file mode 100644
index 00000000..80ff346e
--- /dev/null
+++ b/uf_robot_moveit_config/config/xarm7/gazebo_controllers_with_gripper_action.yaml
@@ -0,0 +1,19 @@
+controller_list:
+  - name: ""
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    joints:
+      - joint1
+      - joint2
+      - joint3
+      - joint4
+      - joint5
+      - joint6
+      - joint7
+
+  - name: gripper_controller
+    action_ns: gripper_action
+    type: GripperCommand
+    joints:
+     - drive_joint
+    allow_failure: true
diff --git a/uf_robot_moveit_config/launch/__move_group.launch b/uf_robot_moveit_config/launch/__move_group.launch
index e87a6b44..d7e842d2 100755
--- a/uf_robot_moveit_config/launch/__move_group.launch
+++ b/uf_robot_moveit_config/launch/__move_group.launch
@@ -13,6 +13,7 @@
 
   <arg name="sim_gazebo" default="false"/>
   <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false"/>
 
   <arg name="sensors_3d" default="false" />
   <arg name="dof" default="7" />
@@ -26,21 +27,39 @@
   <arg name="jiggle_fraction" default="0.05" />
   <arg name="publish_monitored_planning_scene" default="true"/>
 
+  <arg name="pipeline" default="ompl" />
+
   <!-- Planning Functionality -->
   <include ns="move_group" file="$(find uf_robot_moveit_config)/launch/__planning_pipeline.launch.xml">
-    <arg name="pipeline" value="ompl" />
+    <arg name="pipeline" value="$(arg pipeline)" />
     <arg name="robot_name" value="$(arg robot_name)"/>
   </include>
 
+  <arg name="gripper_controller" if="$(eval arg('robot_type') == 'lite')" value="false"/>
+  <arg name="gripper_controller" unless="$(eval arg('robot_type') == 'lite')" value="$(arg add_gripper)"/>
+  
   <!-- Trajectory Execution Functionality -->
   <include ns="move_group" file="$(find uf_robot_moveit_config)/launch/__trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
     <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
     <arg name="moveit_controller_manager" value="gazebo" if="$(eval not arg('fake_execution') and arg('sim_gazebo'))"/>
     <arg name="moveit_controller_manager" value="robot" if="$(eval not arg('fake_execution') and not arg('sim_gazebo'))"/>
-    <arg name="add_gripper" value="$(arg add_gripper)"/>
+    <!-- <arg name="add_gripper" value="$(arg add_gripper)"/>
+    <arg name="gripper_action" value="$(arg gripper_action)"/>
     <arg name="robot_type" value="$(arg robot_type)"/>
-    <arg name="robot_name" value="$(arg robot_name)"/>
+    <arg name="robot_name" value="$(arg robot_name)"/> -->
+    
+    <!-- fake -->
+    <arg if="$(eval arg('fake_execution') and arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers_with_gripper.yaml"/>
+    <arg if="$(eval arg('fake_execution') and not arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers.yaml"/>
+    <!-- gazebo -->
+    <arg if="$(eval not arg('fake_execution') and arg('sim_gazebo') and arg('gripper_action') and arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/gazebo_controllers_with_gripper_action.yaml"/>
+    <arg if="$(eval not arg('fake_execution') and arg('sim_gazebo') and not arg('gripper_action') and arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/gazebo_controllers_with_gripper.yaml"/>
+    <arg if="$(eval not arg('fake_execution') and arg('sim_gazebo') and not arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/>
+    <!-- real -->
+    <arg if="$(eval not arg('fake_execution') and not arg('sim_gazebo') and arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers_with_gripper.yaml"/>
+    <arg if="$(eval not arg('fake_execution') and not arg('sim_gazebo') and not arg('gripper_controller'))" name="controllers_file" value="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/>
+
   </include>
 
   <!-- Sensors Functionality -->
diff --git a/uf_robot_moveit_config/launch/__moveit_controller_manager_fake.launch.xml b/uf_robot_moveit_config/launch/__moveit_controller_manager_fake.launch.xml
index 538bc640..cc5d2086 100755
--- a/uf_robot_moveit_config/launch/__moveit_controller_manager_fake.launch.xml
+++ b/uf_robot_moveit_config/launch/__moveit_controller_manager_fake.launch.xml
@@ -1,12 +1,14 @@
 <launch>
-  <arg name="add_gripper" default="false"/>
-  <arg name="robot_name" default="xarm7"/>
+  <!-- <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false"/>
+  <arg name="robot_name" default="xarm7"/> -->
+  <arg name="controllers_file" default=""/>
 
   <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam if="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers_with_gripper.yaml"/>
-  <rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers.yaml"/>
-
+  <!-- <rosparam if="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers_with_gripper.yaml"/>
+  <rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/fake_controllers.yaml"/> -->
+  <rosparam file="$(arg controllers_file)"/>
 </launch>
diff --git a/uf_robot_moveit_config/launch/__moveit_controller_manager_gazebo.launch.xml b/uf_robot_moveit_config/launch/__moveit_controller_manager_gazebo.launch.xml
index 1007be4f..1a3e6d43 100755
--- a/uf_robot_moveit_config/launch/__moveit_controller_manager_gazebo.launch.xml
+++ b/uf_robot_moveit_config/launch/__moveit_controller_manager_gazebo.launch.xml
@@ -1,12 +1,16 @@
 <launch>
-	<arg name="add_gripper" default="false"/>
-  <arg name="robot_name" default="xarm7"/>
+	<!-- <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false"/>
+  <arg name="robot_name" default="xarm7"/> -->
+  <arg name="controllers_file" default=""/>
 
 	<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
   <!-- Disable the exec duration monitoring  -->
   <param name="trajectory_execution/execution_duration_monitoring" value="false"/>
   <!-- The rest of the params are specific to this plugin -->
-	<rosparam if="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/gazebo_controllers_with_gripper.yaml"/>
-	<rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/>
+	<!-- <rosparam if="$(eval arg('add_gripper') and arg('gripper_action'))" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/gazebo_controllers_with_gripper_action.yaml"/>
+	<rosparam if="$(eval arg('add_gripper') and not arg('gripper_action'))" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/gazebo_controllers_with_gripper.yaml"/>
+	<rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/> -->
+  <rosparam file="$(arg controllers_file)"/>
 </launch>
diff --git a/uf_robot_moveit_config/launch/__moveit_controller_manager_robot.launch.xml b/uf_robot_moveit_config/launch/__moveit_controller_manager_robot.launch.xml
index 5bed1288..c335b68d 100755
--- a/uf_robot_moveit_config/launch/__moveit_controller_manager_robot.launch.xml
+++ b/uf_robot_moveit_config/launch/__moveit_controller_manager_robot.launch.xml
@@ -1,13 +1,16 @@
 <launch>
-	<arg name="add_gripper" default="false"/>
-  <arg name="robot_name" default="xarm7"/>
+	<!-- <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false"/>
+  <arg name="robot_name" default="xarm7"/> -->
+  <arg name="controllers_file" default=""/>
 
 	<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
   <!-- Disable the exec duration monitoring  -->
   <param name="trajectory_execution/execution_duration_monitoring" value="false"/>
   <!-- The rest of the params are specific to this plugin -->
-	<rosparam if="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers_with_gripper.yaml"/>
-	<rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/>
+	<!-- <rosparam if="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers_with_gripper.yaml"/>
+	<rosparam unless="$(arg add_gripper)" file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/controllers.yaml"/> -->
+	<rosparam file="$(arg controllers_file)"/>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/__moveit_rviz.launch b/uf_robot_moveit_config/launch/__moveit_rviz.launch
index 53addabe..eb9b2eb0 100755
--- a/uf_robot_moveit_config/launch/__moveit_rviz.launch
+++ b/uf_robot_moveit_config/launch/__moveit_rviz.launch
@@ -5,10 +5,14 @@
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
-  <arg name="no_gui_ctrl" default="false" />
-  <arg if="$(arg no_gui_ctrl)" name="command_args" value="-d $(find xarm_planner)/launch/planner.rviz" />
-  <!-- <arg unless="$(arg no_gui_ctrl)" name="command_args" value="-d $(find uf_robot_moveit_config)/launch/moveit.rviz" /> -->
-  <arg unless="$(arg no_gui_ctrl)" name="command_args" value="-d $(find uf_robot_moveit_config)/config/$(arg robot_name)/moveit.rviz" />
+  <arg name="rviz_config" default="" />
+
+  <arg name="no_gui_plan" default="false" />
+  <arg if="$(arg no_gui_plan)" name="command_args" value="-d $(find xarm_planner)/launch/planner.rviz" />
+  <!-- <arg unless="$(arg no_gui_plan)" name="command_args" value="-d $(find uf_robot_moveit_config)/launch/moveit.rviz" /> -->
+  <!-- <arg unless="$(arg no_gui_plan)" name="command_args" value="-d $(find uf_robot_moveit_config)/config/$(arg robot_name)/moveit.rviz" /> -->
+  <arg if="$(eval not no_gui_plan and rviz_config!='')" name="command_args" value="-d $(arg rviz_config)" />
+  <arg if="$(eval not no_gui_plan and rviz_config=='')" name="command_args" value="-d $(find uf_robot_moveit_config)/config/$(arg robot_name)/moveit.rviz" />
 
   <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
 	args="$(arg command_args)" output="screen">
diff --git a/uf_robot_moveit_config/launch/__moveit_rviz_common.launch b/uf_robot_moveit_config/launch/__moveit_rviz_common.launch
index cd0cc75e..c4210fd4 100755
--- a/uf_robot_moveit_config/launch/__moveit_rviz_common.launch
+++ b/uf_robot_moveit_config/launch/__moveit_rviz_common.launch
@@ -20,8 +20,8 @@
   <arg name="fake_execution" default="false" />
   <!-- option to publish tf topic or not -->
   <arg name="pub_tf" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
-  <arg name="NO_GUI_CTRL" default="false"/>
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <arg name="no_gui_plan" default="false"/>
   <!-- option to show rviz or not -->
   <arg name="show_rviz" default="true" />
   <arg name="limited" default="true"/>
@@ -41,6 +41,7 @@
 
   <arg name="sim_gazebo" default="false"/>
   <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false" />
   <arg name="add_vacuum_gripper" default="false"/>
   <arg name="add_other_geometry" default="false"/>
   <arg name="geometry_type" default="box"/>
@@ -55,6 +56,8 @@
   <arg name="geometry_mesh_tcp_xyz" default="'0 0 0'"/>
   <arg name="geometry_mesh_tcp_rpy" default="'0 0 0'"/>
 
+  <arg name="pipeline" default="ompl" />
+
   <arg name="dual" default="false" />
 
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
@@ -116,15 +119,17 @@
     <arg name="debug" value="$(arg debug)"/>
     <arg name="sim_gazebo" value="$(arg sim_gazebo)"/>
     <arg name="add_gripper" value="$(arg add_gripper)"/>
+    <arg name="gripper_action" value="$(arg gripper_action)"/>
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="dof" value="$(arg robot_dof)"/>
     <arg name="robot_type" value="$(arg robot_type)"/>
     <arg name="robot_name" value="$(arg robot_name)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
   <include if="$(arg show_rviz)" file="$(find uf_robot_moveit_config)/launch/__moveit_rviz.launch">
-    <arg name="no_gui_ctrl" value="$(arg NO_GUI_CTRL)"/>
+    <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
     <arg name="debug" value="$(arg debug)"/>
     <arg name="robot_name" value="$(arg robot_name)"/>
   </include>
diff --git a/uf_robot_moveit_config/launch/__trajectory_execution.launch.xml b/uf_robot_moveit_config/launch/__trajectory_execution.launch.xml
index e3468b00..1aa2e26e 100755
--- a/uf_robot_moveit_config/launch/__trajectory_execution.launch.xml
+++ b/uf_robot_moveit_config/launch/__trajectory_execution.launch.xml
@@ -1,7 +1,9 @@
 <launch>
-  <arg name="add_gripper" default="false"/>
+  <!-- <arg name="add_gripper" default="false"/>
+  <arg name="gripper_action" default="false"/>
   <arg name="robot_type" default="xarm"/>
-  <arg name="robot_name" default="xarm7"/>
+  <arg name="robot_name" default="xarm7"/> -->
+  <arg name="controllers_file" default=""/>
 
   <!-- This file makes it easy to include the settings for trajectory execution  -->
 
@@ -19,9 +21,11 @@
   <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
   <arg name="moveit_controller_manager" default="fake" />
   <include file="$(find uf_robot_moveit_config)/launch/__moveit_controller_manager_$(arg moveit_controller_manager).launch.xml">
-    <arg name="add_gripper" if="$(eval arg('robot_type') == 'lite')" value="false"/>
+    <!-- <arg name="add_gripper" if="$(eval arg('robot_type') == 'lite')" value="false"/>
     <arg name="add_gripper" unless="$(eval arg('robot_type') == 'lite')" value="$(arg add_gripper)"/>
-    <arg name="robot_name" value="$(arg robot_name)"/>
+    <arg name="gripper_action" value="$(arg gripper_action)"/>
+    <arg name="robot_name" value="$(arg robot_name)"/> -->
+    <arg name="controllers_file" value="$(arg controllers_file)"/>
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch
index 9912b5f3..847fdcac 100755
--- a/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch
@@ -40,7 +40,7 @@
     <arg name="dual" value="true" />
     <!-- whether to use fake_execution controller to drive the motion -->
     <arg name="fake_execution" value="true" />
-    <arg name="NO_GUI_CTRL" value="$(arg no_gui_plan)"/>
+    <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
     
     <arg name="dof" value="$(arg dof)" />
     <arg name="robot_type" value="$(arg robot_type)" />
diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch
index ab0e6f61..fcff8f8e 100755
--- a/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch
@@ -36,13 +36,13 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg if="$(eval arg('robot_type') == 'uf850')" name="robot_traj_controller" value="$(eval arg('robot_type') + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
-  <arg unless="$(eval arg('robot_type') == 'uf850')" name="robot_traj_controller" value="$(eval str(arg('robot_type')) + str(arg('dof')) + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
-  <remap from="/L_$(arg robot_name)_controller/follow_joint_trajectory" to="/$(arg hw_ns)/L_$(arg robot_traj_controller)/follow_joint_trajectory"/>
-  <remap from="/R_$(arg robot_name)_controller/follow_joint_trajectory" to="/$(arg hw_ns)/R_$(arg robot_traj_controller)/follow_joint_trajectory"/>
-  <arg name="gripper_traj_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="L_gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/L_$(arg gripper_traj_controller)/follow_joint_trajectory"/>
-  <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="R_gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/R_$(arg gripper_traj_controller)/follow_joint_trajectory"/>
+  <arg if="$(eval arg('robot_type') == 'uf850')" name="robot_controller_name" value="$(eval arg('robot_type') + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
+  <arg unless="$(eval arg('robot_type') == 'uf850')" name="robot_controller_name" value="$(eval str(arg('robot_type')) + str(arg('dof')) + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
+  <remap from="/L_$(arg robot_name)_controller/follow_joint_trajectory" to="/$(arg hw_ns)/L_$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="/R_$(arg robot_name)_controller/follow_joint_trajectory" to="/$(arg hw_ns)/R_$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="L_gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/L_$(arg gripper_controller_name)/follow_joint_trajectory"/>
+  <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="R_gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/R_$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find uf_robot_moveit_config)/launch/__moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg hw_ns)/joint_states]" />
diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch
index ec676e6e..3857a571 100755
--- a/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch
@@ -39,7 +39,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
@@ -84,9 +84,9 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/dual_$(arg robot_name)_traj_controller.yaml" command="load" ns="L_$(arg hw_ns)"/>
-  <rosparam file="$(find xarm_controller)/config/dual_$(arg robot_name)_traj_controller.yaml" command="load" ns="R_$(arg hw_ns)"/>
-  <arg name="robot_traj_controller" value="$(eval arg('robot_name') + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
+  <rosparam file="$(find xarm_controller)/config/$(arg robot_name)/dual_$(arg robot_name)_controllers.yaml" command="load" ns="L_$(arg hw_ns)"/>
+  <rosparam file="$(find xarm_controller)/config/$(arg robot_name)/dual_$(arg robot_name)_controllers.yaml" command="load" ns="R_$(arg hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval arg('robot_name') + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -94,7 +94,7 @@
     respawn="false"
     output="screen"
     ns="L_$(arg hw_ns)"
-    args="L_$(arg robot_traj_controller) joint_state_controller"/>
+    args="L_$(arg robot_controller_name) joint_state_controller"/>
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -102,10 +102,10 @@
     respawn="false"
     output="screen"
     ns="R_$(arg hw_ns)"
-    args="R_$(arg robot_traj_controller) joint_state_controller"/>
+    args="R_$(arg robot_controller_name) joint_state_controller"/>
   
-  <remap from="/L_$(arg robot_name)_controller/follow_joint_trajectory" to="/L_$(arg hw_ns)/L_$(arg robot_traj_controller)/follow_joint_trajectory"/>
-  <remap from="/R_$(arg robot_name)_controller/follow_joint_trajectory" to="/R_$(arg hw_ns)/R_$(arg robot_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/L_$(arg robot_name)_controller/follow_joint_trajectory" to="/L_$(arg hw_ns)/L_$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="/R_$(arg robot_name)_controller/follow_joint_trajectory" to="/R_$(arg hw_ns)/R_$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="L_xarm_gripper/gripper_action" to="/L_$(arg hw_ns)/xarm_gripper/gripper_action"/>
   <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="R_xarm_gripper/gripper_action" to="/R_$(arg hw_ns)/xarm_gripper/gripper_action"/>
 
@@ -119,7 +119,7 @@
     <arg name="jnt_stat_source" value="[L_$(arg hw_ns)/joint_states, R_$(arg hw_ns)/joint_states]" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
     <arg name="show_rviz" value="$(arg show_rviz)" />
-    <arg name="NO_GUI_CTRL" value="$(arg no_gui_plan)"/>
+    <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
   
     <arg name="dof" value="$(arg robot_dof)" />
diff --git a/uf_robot_moveit_config/launch/_robot_moveit_fake.launch b/uf_robot_moveit_config/launch/_robot_moveit_fake.launch
index 264e72cd..4c68998a 100755
--- a/uf_robot_moveit_config/launch/_robot_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/_robot_moveit_fake.launch
@@ -32,6 +32,9 @@
   <!-- only use in xarm_planner -->
   <arg name="no_gui_plan" default="false" />
 
+  <arg name="pipeline" default="ompl" />
+  <arg name="show_rviz" default="true" />
+
   <remap from="$(arg ext_ns)/source_list" to="/source_list" />
   <include file="$(find uf_robot_moveit_config)/launch/__moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[$(arg ext_ns)/move_group/fake_controller_joint_states]" />
@@ -39,7 +42,7 @@
     <arg name="load_move_group" value="true" />
     <!-- whether to use fake_execution controller to drive the motion -->
     <arg name="fake_execution" value="true" />
-    <arg name="NO_GUI_CTRL" value="$(arg no_gui_plan)"/>
+    <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
     
     <arg name="dof" value="$(arg dof)" />
     <arg name="robot_type" value="$(arg robot_type)" />
@@ -70,6 +73,8 @@
 
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="show_rviz" value="$(arg show_rviz)"/>
     
   </include>
 
diff --git a/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch b/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch
index 04a401bb..d4dd7819 100755
--- a/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch
@@ -14,6 +14,7 @@
   <arg name="add_realsense_d435i" default="false" />
   <arg name="add_d435i_links" default="false" />
   <arg name="add_gripper" default="false" />
+  <arg name="gripper_action" default="false" />
   <arg name="add_vacuum_gripper" default="false" />
   <arg name="add_other_geometry" default="false"/>
   <arg name="geometry_type" default="box"/>
@@ -33,11 +34,12 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg if="$(eval arg('robot_type') == 'uf850')" name="robot_traj_controller" value="$(eval arg('robot_type') + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
-  <arg unless="$(eval arg('robot_type') == 'uf850')" name="robot_traj_controller" value="$(eval str(arg('robot_type')) + str(arg('dof')) + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
-  <remap from="/follow_joint_trajectory" to="/$(arg hw_ns)/$(arg robot_traj_controller)/follow_joint_trajectory"/>
-  <arg name="gripper_traj_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/$(arg gripper_traj_controller)/follow_joint_trajectory"/>
+  <arg if="$(eval arg('robot_type') == 'uf850')" name="robot_controller_name" value="$(eval arg('robot_type') + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
+  <arg unless="$(eval arg('robot_type') == 'uf850')" name="robot_controller_name" value="$(eval str(arg('robot_type')) + str(arg('dof')) + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
+  <remap from="/follow_joint_trajectory" to="/$(arg hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_action_controller' if arg('gripper_action') else 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap if="$(eval arg('add_gripper') and not arg('gripper_action'))" from="gripper_controller/follow_joint_trajectory" to="/$(arg hw_ns)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
+  <remap if="$(eval arg('add_gripper') and arg('gripper_action'))" from="gripper_controller/gripper_action" to="/$(arg hw_ns)/$(arg gripper_controller_name)/gripper_cmd"/>
 
   <include file="$(find uf_robot_moveit_config)/launch/__moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg hw_ns)/joint_states]" />
@@ -59,6 +61,7 @@
     <arg name="add_realsense_d435i" value="$(arg add_realsense_d435i)"/>
     <arg name="add_d435i_links" value="$(arg add_d435i_links)"/>
     <arg name="add_gripper" value="$(arg add_gripper)"/>
+    <arg name="gripper_action" value="$(arg gripper_action)" />
     <arg name="add_vacuum_gripper" value="$(arg add_vacuum_gripper)"/>
     <arg name="add_other_geometry" value="$(arg add_other_geometry)"/>
     <arg name="geometry_type" value="$(arg geometry_type)"/>
@@ -94,6 +97,7 @@
     <arg name="add_realsense_d435i" value="$(arg add_realsense_d435i)"/>
     <arg name="add_d435i_links" value="$(arg add_d435i_links)"/>
     <arg name="add_gripper" value="$(arg add_gripper)"/>
+    <arg name="gripper_action" value="$(arg gripper_action)"/>
     <arg name="add_vacuum_gripper" value="$(arg add_vacuum_gripper)"/>
     <arg name="add_other_geometry" value="$(arg add_other_geometry)"/>
     <arg name="geometry_type" value="$(arg geometry_type)"/>
diff --git a/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch b/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch
index 50e390a6..d738f527 100755
--- a/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch
@@ -38,7 +38,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <arg name="robot_name" value="$(eval arg('robot_type') if arg('robot_type') == 'uf850' else arg('robot_type') + str(arg('dof')))" />
@@ -64,8 +64,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/$(arg robot_name)_traj_controller.yaml" command="load" ns="$(arg hw_ns)"/>
-  <arg name="robot_traj_controller" value="$(eval arg('robot_name') + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
+  <rosparam file="$(find xarm_controller)/config/$(arg robot_name)/$(arg robot_name)_controllers.yaml" command="load" ns="$(arg hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval arg('robot_name') + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -73,9 +73,9 @@
     respawn="false"
     output="screen"
     ns="$(arg hw_ns)"
-    args="$(arg robot_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <remap from="/follow_joint_trajectory" to="$(arg hw_ns)/$(arg robot_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" from="xarm_gripper/gripper_action" to="$(arg hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
@@ -87,7 +87,7 @@
     <arg name="jnt_stat_source" value="[$(arg hw_ns)/joint_states]" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
     <arg name="show_rviz" value="$(arg show_rviz)" />
-    <arg name="NO_GUI_CTRL" value="$(arg no_gui_plan)"/>
+    <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
   
     <arg name="dof" value="$(arg dof)" />
diff --git a/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch
index cf055933..4d39b10d 100755
--- a/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch
@@ -37,7 +37,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch
index 1b486318..5fe90fa5 100755
--- a/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch
@@ -37,7 +37,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch
index 69c4ce31..a1ff4381 100755
--- a/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch
@@ -37,7 +37,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch
index c72cb790..d03876f7 100755
--- a/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch
@@ -37,7 +37,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch
index beb0ee12..e1bfb5e6 100755
--- a/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch
@@ -37,7 +37,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch b/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
index 8ad692f4..e7d762a5 100755
--- a/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
@@ -36,7 +36,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch b/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
index 698da672..fbaa5350 100755
--- a/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
@@ -36,7 +36,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
index 3e02b5bd..a0954dbc 100755
--- a/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
@@ -36,7 +36,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
index de63b3f8..add11160 100755
--- a/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
@@ -36,7 +36,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_robot_moveit_realmove.launch">
diff --git a/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
index 8f464367..d8642615 100755
--- a/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
@@ -36,7 +36,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
   <include file="$(find uf_robot_moveit_config)/launch/_robot_moveit_realmove.launch">
diff --git a/xarm5_gripper_moveit_config/launch/realMove_exec.launch b/xarm5_gripper_moveit_config/launch/realMove_exec.launch
index f744a7bb..adfd4525 100644
--- a/xarm5_gripper_moveit_config/launch/realMove_exec.launch
+++ b/xarm5_gripper_moveit_config/launch/realMove_exec.launch
@@ -39,8 +39,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm5_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm5/xarm5_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -48,10 +48,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="arm_controller/follow_joint_trajectory" to="xarm/xarm5_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm5_gripper_moveit_config/launch/xarm5_gripper_moveit_gazebo.launch b/xarm5_gripper_moveit_config/launch/xarm5_gripper_moveit_gazebo.launch
index cf93e69f..1167b6a7 100644
--- a/xarm5_gripper_moveit_config/launch/xarm5_gripper_moveit_gazebo.launch
+++ b/xarm5_gripper_moveit_config/launch/xarm5_gripper_moveit_gazebo.launch
@@ -7,11 +7,10 @@
   <arg name="robot_sn" default="" />
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <!-- <remap from="/follow_joint_trajectory" to="/xarm/xarm5_traj_controller/follow_joint_trajectory"/> -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm5_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm5_moveit_config/launch/realMove_exec.launch b/xarm5_moveit_config/launch/realMove_exec.launch
index 796213f9..1e189108 100755
--- a/xarm5_moveit_config/launch/realMove_exec.launch
+++ b/xarm5_moveit_config/launch/realMove_exec.launch
@@ -51,8 +51,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm5_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm5/xarm5_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -60,11 +60,10 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
   <!-- from="/follow_joint_trajectory" should not remove "/" -->
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm5_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap if="$(arg add_gripper)" from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm5_moveit_config/launch/xarm5_moveit_gazebo.launch b/xarm5_moveit_config/launch/xarm5_moveit_gazebo.launch
index ebf4239d..ec8ed04e 100755
--- a/xarm5_moveit_config/launch/xarm5_moveit_gazebo.launch
+++ b/xarm5_moveit_config/launch/xarm5_moveit_gazebo.launch
@@ -24,10 +24,10 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm5_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm6_gripper_moveit_config/launch/realMove_exec.launch b/xarm6_gripper_moveit_config/launch/realMove_exec.launch
index 08c5754c..223b3b2a 100755
--- a/xarm6_gripper_moveit_config/launch/realMove_exec.launch
+++ b/xarm6_gripper_moveit_config/launch/realMove_exec.launch
@@ -39,8 +39,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm6_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm6/xarm6_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -48,10 +48,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="arm_controller/follow_joint_trajectory" to="xarm/xarm6_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm6_gripper_moveit_config/launch/xarm6_gripper_moveit_gazebo.launch b/xarm6_gripper_moveit_config/launch/xarm6_gripper_moveit_gazebo.launch
index 243afbae..feb5a659 100755
--- a/xarm6_gripper_moveit_config/launch/xarm6_gripper_moveit_gazebo.launch
+++ b/xarm6_gripper_moveit_config/launch/xarm6_gripper_moveit_gazebo.launch
@@ -7,11 +7,10 @@
   <arg name="robot_sn" default="" />
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <!-- <remap from="/follow_joint_trajectory" to="/xarm/xarm6_traj_controller/follow_joint_trajectory"/> -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm6_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm6_moveit_config/launch/realMove_exec.launch b/xarm6_moveit_config/launch/realMove_exec.launch
index cef3b2b2..da2a20a8 100755
--- a/xarm6_moveit_config/launch/realMove_exec.launch
+++ b/xarm6_moveit_config/launch/realMove_exec.launch
@@ -51,8 +51,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm6_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm6/xarm6_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -60,10 +60,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm6_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap if="$(arg add_gripper)" from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch b/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch
index 3fe47f47..eabc445b 100755
--- a/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch
+++ b/xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch
@@ -24,10 +24,10 @@
 
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm6_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm7_gripper_moveit_config/launch/realMove_exec.launch b/xarm7_gripper_moveit_config/launch/realMove_exec.launch
index 2fdd5f1f..ca4aaddb 100644
--- a/xarm7_gripper_moveit_config/launch/realMove_exec.launch
+++ b/xarm7_gripper_moveit_config/launch/realMove_exec.launch
@@ -39,8 +39,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm7_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm7/xarm7_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -48,10 +48,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="arm_controller/follow_joint_trajectory" to="xarm/xarm7_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="arm_controller/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm7_gripper_moveit_config/launch/xarm7_gripper_moveit_gazebo.launch b/xarm7_gripper_moveit_config/launch/xarm7_gripper_moveit_gazebo.launch
index 3da8442e..38f9c414 100644
--- a/xarm7_gripper_moveit_config/launch/xarm7_gripper_moveit_gazebo.launch
+++ b/xarm7_gripper_moveit_config/launch/xarm7_gripper_moveit_gazebo.launch
@@ -7,13 +7,10 @@
   <arg name="robot_sn" default="" />
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <!-- <remap from="/follow_joint_trajectory" to="/xarm/xarm7_traj_controller/follow_joint_trajectory"/> -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
-
-
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm7_gripper_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm7_moveit_config/launch/realMove_exec.launch b/xarm7_moveit_config/launch/realMove_exec.launch
index fae26065..c6c4717f 100755
--- a/xarm7_moveit_config/launch/realMove_exec.launch
+++ b/xarm7_moveit_config/launch/realMove_exec.launch
@@ -51,8 +51,8 @@
   </include>
 
   <!-- load the joint trajectory controller specified in yaml -->
-  <rosparam file="$(find xarm_controller)/config/xarm7_traj_controller.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm7/xarm7_controllers.yaml" command="load" ns="$(arg xarm_hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
   <node
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -60,10 +60,9 @@
     respawn="false"
     output="screen"
     ns="$(arg xarm_hw_ns)"
-    args="$(arg xarm_traj_controller) joint_state_controller"/>
+    args="$(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- <remap from="/follow_joint_trajectory" to="xarm/xarm7_traj_controller/follow_joint_trajectory"/> -->
-  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
+  <remap from="/follow_joint_trajectory" to="$(arg xarm_hw_ns)/$(arg robot_controller_name)/follow_joint_trajectory"/>
   <remap if="$(arg add_gripper)" from="xarm_gripper/gripper_action" to="$(arg xarm_hw_ns)/xarm_gripper/gripper_action"/>
 
   <!-- common part -->
diff --git a/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch b/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch
index 479cb3c3..5af5b6cc 100755
--- a/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch
+++ b/xarm7_moveit_config/launch/xarm7_moveit_gazebo.launch
@@ -24,10 +24,10 @@
   
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
-  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <remap from="/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap if="$(arg add_gripper)" from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <include file="$(find xarm7_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/$(arg namespace)/joint_states]" />
diff --git a/xarm_controller/config/dual_lite6_position_controllers.yaml b/xarm_controller/config/dual_lite6_position_controllers.yaml
deleted file mode 100755
index 66224327..00000000
--- a/xarm_controller/config/dual_lite6_position_controllers.yaml
+++ /dev/null
@@ -1,68 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-L_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-L_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-L_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-L_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-L_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-L_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-R_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-R_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-R_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-R_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-R_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-R_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# # No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint2: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint3: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint4: {p: 450.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint5: {p: 300.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint6: {p: 150.0, i: 5.0, d: 10, i_clamp: 1}
-
diff --git a/xarm_controller/config/dual_lite6_traj_controller.yaml b/xarm_controller/config/dual_lite6_traj_controller.yaml
deleted file mode 100755
index dfe7a8e2..00000000
--- a/xarm_controller/config/dual_lite6_traj_controller.yaml
+++ /dev/null
@@ -1,130 +0,0 @@
-# xarm:
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-  
-L_lite6_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-L_lite6_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  gains:
-    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    L_joint1: 0.25
-    L_joint2: 0.25
-    L_joint3: 0.25
-    L_joint4: 0.25
-    L_joint5: 0.25
-    L_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_lite6_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-lite6_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  gains:
-    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    R_joint1: 0.25
-    R_joint2: 0.25
-    R_joint3: 0.25
-    R_joint4: 0.25
-    R_joint5: 0.25
-    R_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/dual_uf850_position_controllers.yaml b/xarm_controller/config/dual_uf850_position_controllers.yaml
deleted file mode 100755
index 66224327..00000000
--- a/xarm_controller/config/dual_uf850_position_controllers.yaml
+++ /dev/null
@@ -1,68 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-L_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-L_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-L_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-L_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-L_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-L_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-R_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-R_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-R_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-R_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-R_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-R_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# # No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint2: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint3: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint4: {p: 450.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint5: {p: 300.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint6: {p: 150.0, i: 5.0, d: 10, i_clamp: 1}
-
diff --git a/xarm_controller/config/dual_uf850_traj_controller.yaml b/xarm_controller/config/dual_uf850_traj_controller.yaml
deleted file mode 100755
index b908b798..00000000
--- a/xarm_controller/config/dual_uf850_traj_controller.yaml
+++ /dev/null
@@ -1,130 +0,0 @@
-# xarm:
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-  
-L_uf850_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-L_uf850_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  gains:
-    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    L_joint1: 0.25
-    L_joint2: 0.25
-    L_joint3: 0.25
-    L_joint4: 0.25
-    L_joint5: 0.25
-    L_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_uf850_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_uf850_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  gains:
-    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    R_joint1: 0.25
-    R_joint2: 0.25
-    R_joint3: 0.25
-    R_joint4: 0.25
-    R_joint5: 0.25
-    R_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/dual_xarm5_position_controllers.yaml b/xarm_controller/config/dual_xarm5_position_controllers.yaml
deleted file mode 100644
index 606557d5..00000000
--- a/xarm_controller/config/dual_xarm5_position_controllers.yaml
+++ /dev/null
@@ -1,60 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-L_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-L_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-L_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-L_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-L_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-R_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-R_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-R_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-R_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-R_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 500.0, i: 3.0, d: 1.0}
-#     joint5: {p: 500.0, i: 1.0, d: 1.0}
-
-
diff --git a/xarm_controller/config/dual_xarm5_traj_controller.yaml b/xarm_controller/config/dual_xarm5_traj_controller.yaml
deleted file mode 100644
index 7cb18f88..00000000
--- a/xarm_controller/config/dual_xarm5_traj_controller.yaml
+++ /dev/null
@@ -1,118 +0,0 @@
-# xarm:
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-  
-L_xarm5_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-L_xarm5_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-  gains:
-    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    L_joint1: 0.25
-    L_joint2: 0.25
-    L_joint3: 0.25
-    L_joint4: 0.25
-    L_joint5: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm5_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm5_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-  gains:
-    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    R_joint1: 0.25
-    R_joint2: 0.25
-    R_joint3: 0.25
-    R_joint4: 0.25
-    R_joint5: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 500.0, i: 3.0, d: 1.0}
-#     joint5: {p: 500.0, i: 1.0, d: 1.0}
-
diff --git a/xarm_controller/config/dual_xarm6_position_controllers.yaml b/xarm_controller/config/dual_xarm6_position_controllers.yaml
deleted file mode 100755
index 0deb1e5a..00000000
--- a/xarm_controller/config/dual_xarm6_position_controllers.yaml
+++ /dev/null
@@ -1,61 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-L_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-L_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-L_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-L_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-L_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-L_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: L_joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-R_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-R_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-R_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-R_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-R_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-R_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: R_joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-
-
diff --git a/xarm_controller/config/dual_xarm6_traj_controller.yaml b/xarm_controller/config/dual_xarm6_traj_controller.yaml
deleted file mode 100755
index 1bfd1a35..00000000
--- a/xarm_controller/config/dual_xarm6_traj_controller.yaml
+++ /dev/null
@@ -1,120 +0,0 @@
-# xarm:
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-  
-L_xarm6_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-L_xarm6_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-  gains:
-    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    L_joint1: 0.25
-    L_joint2: 0.25
-    L_joint3: 0.25
-    L_joint4: 0.25
-    L_joint5: 0.25
-    L_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm6_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm6_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-  gains:
-    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    R_joint1: 0.25
-    R_joint2: 0.25
-    R_joint3: 0.25
-    R_joint4: 0.25
-    R_joint5: 0.25
-    R_joint6: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
diff --git a/xarm_controller/config/dual_xarm7_position_controllers.yaml b/xarm_controller/config/dual_xarm7_position_controllers.yaml
deleted file mode 100755
index 8ec2c099..00000000
--- a/xarm_controller/config/dual_xarm7_position_controllers.yaml
+++ /dev/null
@@ -1,76 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-L_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-L_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-L_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-L_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-L_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-L_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-L_joint7_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint7
-  pid: {p: 300.0, i: 0.05, d: 1.0}
-
-R_joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-R_joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-R_joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-R_joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-R_joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-R_joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-R_joint7_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint7
-  pid: {p: 300.0, i: 0.05, d: 1.0}
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
-#     joint7: {p: 300.0, i: 0.05, d: 1.0}
-
diff --git a/xarm_controller/config/dual_xarm7_traj_controller.yaml b/xarm_controller/config/dual_xarm7_traj_controller.yaml
deleted file mode 100755
index bac02c25..00000000
--- a/xarm_controller/config/dual_xarm7_traj_controller.yaml
+++ /dev/null
@@ -1,144 +0,0 @@
-# xarm:
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-  
-L_xarm7_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-    - L_joint7
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-    L_joint7: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-L_xarm7_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - L_joint1
-    - L_joint2
-    - L_joint3
-    - L_joint4
-    - L_joint5
-    - L_joint6
-    - L_joint7
-  gains:
-    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    L_joint7: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    L_joint1: 0.25
-    L_joint2: 0.25
-    L_joint3: 0.25
-    L_joint4: 0.25
-    L_joint5: 0.25
-    L_joint6: 0.25
-    L_joint7: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    L_joint1: {trajectory: 1, goal: 0.01}
-    L_joint2: {trajectory: 1, goal: 0.01}
-    L_joint3: {trajectory: 1, goal: 0.01}
-    L_joint4: {trajectory: 1, goal: 0.01}
-    L_joint5: {trajectory: 1, goal: 0.01}
-    L_joint6: {trajectory: 1, goal: 0.01}
-    L_joint7: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm7_traj_controller:
-  type: position_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-    - R_joint7
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.05
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-    R_joint7: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-R_xarm7_traj_controller_velocity:
-  type: velocity_controllers/JointTrajectoryController
-  joints:
-    - R_joint1
-    - R_joint2
-    - R_joint3
-    - R_joint4
-    - R_joint5
-    - R_joint6
-    - R_joint7
-  gains:
-    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-    R_joint7: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
-  velocity_ff:
-    R_joint1: 0.25
-    R_joint2: 0.25
-    R_joint3: 0.25
-    R_joint4: 0.25
-    R_joint5: 0.25
-    R_joint6: 0.25
-    R_joint7: 0.25
-  constraints:
-    goal_time: 0.5
-    stopped_velocity_tolerance: 0.01
-    R_joint1: {trajectory: 1, goal: 0.01}
-    R_joint2: {trajectory: 1, goal: 0.01}
-    R_joint3: {trajectory: 1, goal: 0.01}
-    R_joint4: {trajectory: 1, goal: 0.01}
-    R_joint5: {trajectory: 1, goal: 0.01}
-    R_joint6: {trajectory: 1, goal: 0.01}
-    R_joint7: {trajectory: 1, goal: 0.01}
-  stop_trajectory_duration: 0.2
-  state_publish_rate:  25
-  action_monitor_rate: 10
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
-#     joint7: {p: 300.0, i: 0.05, d: 1.0}
-
diff --git a/xarm_controller/config/dual_gripper_traj_controller.yaml b/xarm_controller/config/gripper/dual_gripper_controllers.yaml
similarity index 78%
rename from xarm_controller/config/dual_gripper_traj_controller.yaml
rename to xarm_controller/config/gripper/dual_gripper_controllers.yaml
index d704824b..ab6908ce 100644
--- a/xarm_controller/config/dual_gripper_traj_controller.yaml
+++ b/xarm_controller/config/gripper/dual_gripper_controllers.yaml
@@ -1,5 +1,5 @@
-# xarm:
-L_gripper_trajectory_controller:
+# default controller 
+L_gripper_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints: 
     - L_drive_joint
@@ -8,7 +8,7 @@ L_gripper_trajectory_controller:
     stopped_velocity_tolerance: 0.05
     L_drive_joint: {trajectory: 1, goal: 0.01}
 
-L_gripper_trajectory_controller_velocity:
+L_gripper_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints: 
     - L_drive_joint
@@ -19,7 +19,17 @@ L_gripper_trajectory_controller_velocity:
     stopped_velocity_tolerance: 0.05
     L_drive_joint: {trajectory: 1, goal: 0.01}
 
-R_gripper_trajectory_controller:
+L_gripper_action_controller:
+  type: position_controllers/GripperActionController
+  joint: L_drive_joint
+  action_monitor_rate: 20
+  goal_tolerance: 0.005
+  max_effort: 100
+  stall_velocity_threshold: 0.01
+  stall_timeout: 0.5
+
+# default controller 
+R_gripper_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints: 
     - R_drive_joint
@@ -28,7 +38,7 @@ R_gripper_trajectory_controller:
     stopped_velocity_tolerance: 0.05
     R_drive_joint: {trajectory: 1, goal: 0.01}
 
-R_gripper_trajectory_controller_velocity:
+R_gripper_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints: 
     - R_drive_joint
@@ -39,6 +49,15 @@ R_gripper_trajectory_controller_velocity:
     stopped_velocity_tolerance: 0.05
     R_drive_joint: {trajectory: 1, goal: 0.01}
 
+R_gripper_action_controller:
+  type: position_controllers/GripperActionController
+  joint: R_drive_joint
+  action_monitor_rate: 20
+  goal_tolerance: 0.005
+  max_effort: 100
+  stall_velocity_threshold: 0.01
+  stall_timeout: 0.5
+
 # No Pid gains specified error fix
 gazebo_ros_control:
   pid_gains:
diff --git a/xarm_controller/config/dual_gripper_gazebo_ros_control.yaml b/xarm_controller/config/gripper/dual_gripper_gazebo_ros_control.yaml
similarity index 100%
rename from xarm_controller/config/dual_gripper_gazebo_ros_control.yaml
rename to xarm_controller/config/gripper/dual_gripper_gazebo_ros_control.yaml
diff --git a/xarm_controller/config/gripper_traj_controller.yaml b/xarm_controller/config/gripper/gripper_controllers.yaml
similarity index 79%
rename from xarm_controller/config/gripper_traj_controller.yaml
rename to xarm_controller/config/gripper/gripper_controllers.yaml
index bf528dda..a10623c7 100644
--- a/xarm_controller/config/gripper_traj_controller.yaml
+++ b/xarm_controller/config/gripper/gripper_controllers.yaml
@@ -1,5 +1,5 @@
-# xarm:
-gripper_trajectory_controller:
+# real_move default controller
+gripper_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints: 
     - drive_joint
@@ -8,7 +8,7 @@ gripper_trajectory_controller:
     stopped_velocity_tolerance: 0.05
     drive_joint: {trajectory: 1, goal: 0.01}
 
-gripper_trajectory_controller_velocity:
+gripper_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints: 
     - drive_joint
@@ -19,6 +19,15 @@ gripper_trajectory_controller_velocity:
     stopped_velocity_tolerance: 0.05
     drive_joint: {trajectory: 1, goal: 0.01}
 
+gripper_action_controller:
+  type: position_controllers/GripperActionController
+  joint: drive_joint
+  action_monitor_rate: 20
+  goal_tolerance: 0.005
+  max_effort: 100
+  stall_velocity_threshold: 0.01
+  stall_timeout: 0.5
+
 # No Pid gains specified error fix
 gazebo_ros_control:
   pid_gains:
diff --git a/xarm_controller/config/gripper_gazebo_ros_control.yaml b/xarm_controller/config/gripper/gripper_gazebo_ros_control.yaml
similarity index 100%
rename from xarm_controller/config/gripper_gazebo_ros_control.yaml
rename to xarm_controller/config/gripper/gripper_gazebo_ros_control.yaml
diff --git a/xarm_controller/config/lite6/dual_lite6_controllers.yaml b/xarm_controller/config/lite6/dual_lite6_controllers.yaml
new file mode 100644
index 00000000..15a5a4c1
--- /dev/null
+++ b/xarm_controller/config/lite6/dual_lite6_controllers.yaml
@@ -0,0 +1,174 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# default controller 
+L_lite6_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      L_joint1: {trajectory: 1, goal: 0.01}
+      L_joint2: {trajectory: 1, goal: 0.01}
+      L_joint3: {trajectory: 1, goal: 0.01}
+      L_joint4: {trajectory: 1, goal: 0.01}
+      L_joint5: {trajectory: 1, goal: 0.01}
+      L_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+L_lite6_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  gains:
+    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    L_joint1: 0.25
+    L_joint2: 0.25
+    L_joint3: 0.25
+    L_joint4: 0.25
+    L_joint5: 0.25
+    L_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    L_joint1: {trajectory: 1, goal: 0.01}
+    L_joint2: {trajectory: 1, goal: 0.01}
+    L_joint3: {trajectory: 1, goal: 0.01}
+    L_joint4: {trajectory: 1, goal: 0.01}
+    L_joint5: {trajectory: 1, goal: 0.01}
+    L_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+L_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+L_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+L_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+L_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+L_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+L_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+
+# default controller 
+R_lite6_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      R_joint1: {trajectory: 1, goal: 0.01}
+      R_joint2: {trajectory: 1, goal: 0.01}
+      R_joint3: {trajectory: 1, goal: 0.01}
+      R_joint4: {trajectory: 1, goal: 0.01}
+      R_joint5: {trajectory: 1, goal: 0.01}
+      R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+R_lite6_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  gains:
+    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    R_joint1: 0.25
+    R_joint2: 0.25
+    R_joint3: 0.25
+    R_joint4: 0.25
+    R_joint5: 0.25
+    R_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    R_joint1: {trajectory: 1, goal: 0.01}
+    R_joint2: {trajectory: 1, goal: 0.01}
+    R_joint3: {trajectory: 1, goal: 0.01}
+    R_joint4: {trajectory: 1, goal: 0.01}
+    R_joint5: {trajectory: 1, goal: 0.01}
+    R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+R_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+R_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+R_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+R_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+R_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+R_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/lite6_traj_controller.yaml b/xarm_controller/config/lite6/lite6_controllers.yaml
old mode 100755
new mode 100644
similarity index 63%
rename from xarm_controller/config/lite6_traj_controller.yaml
rename to xarm_controller/config/lite6/lite6_controllers.yaml
index c87cefdd..92d08af3
--- a/xarm_controller/config/lite6_traj_controller.yaml
+++ b/xarm_controller/config/lite6/lite6_controllers.yaml
@@ -1,8 +1,9 @@
-# xarm:
+# Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
-  
+
+# default controller  
 lite6_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints:
@@ -25,7 +26,7 @@ lite6_traj_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-lite6_traj_controller_velocity:
+lite6_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints:
       - joint1
@@ -61,12 +62,28 @@ lite6_traj_controller_velocity:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
+# Position Controllers ---------------------------------------
+joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/lite6_position_controllers.yaml b/xarm_controller/config/lite6_position_controllers.yaml
deleted file mode 100755
index a2992b6b..00000000
--- a/xarm_controller/config/lite6_position_controllers.yaml
+++ /dev/null
@@ -1,43 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# # No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint2: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint3: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint4: {p: 450.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint5: {p: 300.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint6: {p: 150.0, i: 5.0, d: 10, i_clamp: 1}
-
diff --git a/xarm_controller/config/uf850/dual_uf850_controllers.yaml b/xarm_controller/config/uf850/dual_uf850_controllers.yaml
new file mode 100644
index 00000000..7ad66db7
--- /dev/null
+++ b/xarm_controller/config/uf850/dual_uf850_controllers.yaml
@@ -0,0 +1,174 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# default controller   
+L_uf850_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      L_joint1: {trajectory: 1, goal: 0.01}
+      L_joint2: {trajectory: 1, goal: 0.01}
+      L_joint3: {trajectory: 1, goal: 0.01}
+      L_joint4: {trajectory: 1, goal: 0.01}
+      L_joint5: {trajectory: 1, goal: 0.01}
+      L_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+L_uf850_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  gains:
+    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    L_joint1: 0.25
+    L_joint2: 0.25
+    L_joint3: 0.25
+    L_joint4: 0.25
+    L_joint5: 0.25
+    L_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    L_joint1: {trajectory: 1, goal: 0.01}
+    L_joint2: {trajectory: 1, goal: 0.01}
+    L_joint3: {trajectory: 1, goal: 0.01}
+    L_joint4: {trajectory: 1, goal: 0.01}
+    L_joint5: {trajectory: 1, goal: 0.01}
+    L_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+L_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+L_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+L_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+L_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+L_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+L_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+
+# default controller 
+R_uf850_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      R_joint1: {trajectory: 1, goal: 0.01}
+      R_joint2: {trajectory: 1, goal: 0.01}
+      R_joint3: {trajectory: 1, goal: 0.01}
+      R_joint4: {trajectory: 1, goal: 0.01}
+      R_joint5: {trajectory: 1, goal: 0.01}
+      R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+R_uf850_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  gains:
+    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    R_joint1: 0.25
+    R_joint2: 0.25
+    R_joint3: 0.25
+    R_joint4: 0.25
+    R_joint5: 0.25
+    R_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    R_joint1: {trajectory: 1, goal: 0.01}
+    R_joint2: {trajectory: 1, goal: 0.01}
+    R_joint3: {trajectory: 1, goal: 0.01}
+    R_joint4: {trajectory: 1, goal: 0.01}
+    R_joint5: {trajectory: 1, goal: 0.01}
+    R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+R_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+R_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+R_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+R_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+R_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+R_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
\ No newline at end of file
diff --git a/xarm_controller/config/uf850_traj_controller.yaml b/xarm_controller/config/uf850/uf850_controllers.yaml
old mode 100755
new mode 100644
similarity index 63%
rename from xarm_controller/config/uf850_traj_controller.yaml
rename to xarm_controller/config/uf850/uf850_controllers.yaml
index af22a914..b64edb4f
--- a/xarm_controller/config/uf850_traj_controller.yaml
+++ b/xarm_controller/config/uf850/uf850_controllers.yaml
@@ -1,8 +1,9 @@
-# xarm:
+# Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
-  
+
+# default controller   
 uf850_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints:
@@ -25,7 +26,7 @@ uf850_traj_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-uf850_traj_controller_velocity:
+uf850_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints:
       - joint1
@@ -61,12 +62,28 @@ uf850_traj_controller_velocity:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
+# Position Controllers ---------------------------------------
+joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/uf850_position_controllers.yaml b/xarm_controller/config/uf850_position_controllers.yaml
deleted file mode 100755
index a2992b6b..00000000
--- a/xarm_controller/config/uf850_position_controllers.yaml
+++ /dev/null
@@ -1,43 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# # No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint2: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint3: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint4: {p: 450.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint5: {p: 300.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint6: {p: 150.0, i: 5.0, d: 10, i_clamp: 1}
-
diff --git a/xarm_controller/config/xarm5/dual_xarm5_controllers.yaml b/xarm_controller/config/xarm5/dual_xarm5_controllers.yaml
new file mode 100644
index 00000000..34933c9f
--- /dev/null
+++ b/xarm_controller/config/xarm5/dual_xarm5_controllers.yaml
@@ -0,0 +1,154 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# default controller   
+L_xarm5_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      L_joint1: {trajectory: 1, goal: 0.01}
+      L_joint2: {trajectory: 1, goal: 0.01}
+      L_joint3: {trajectory: 1, goal: 0.01}
+      L_joint4: {trajectory: 1, goal: 0.01}
+      L_joint5: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+L_xarm5_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+  gains:
+    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    L_joint1: 0.25
+    L_joint2: 0.25
+    L_joint3: 0.25
+    L_joint4: 0.25
+    L_joint5: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    L_joint1: {trajectory: 1, goal: 0.01}
+    L_joint2: {trajectory: 1, goal: 0.01}
+    L_joint3: {trajectory: 1, goal: 0.01}
+    L_joint4: {trajectory: 1, goal: 0.01}
+    L_joint5: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+L_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+L_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+L_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+L_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint4
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+L_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint5
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+
+# default controller 
+R_xarm5_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      R_joint1: {trajectory: 1, goal: 0.01}
+      R_joint2: {trajectory: 1, goal: 0.01}
+      R_joint3: {trajectory: 1, goal: 0.01}
+      R_joint4: {trajectory: 1, goal: 0.01}
+      R_joint5: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+R_xarm5_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+  gains:
+    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    R_joint1: 0.25
+    R_joint2: 0.25
+    R_joint3: 0.25
+    R_joint4: 0.25
+    R_joint5: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    R_joint1: {trajectory: 1, goal: 0.01}
+    R_joint2: {trajectory: 1, goal: 0.01}
+    R_joint3: {trajectory: 1, goal: 0.01}
+    R_joint4: {trajectory: 1, goal: 0.01}
+    R_joint5: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+R_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+R_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+R_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+R_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint4
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+R_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint5
+  pid: {p: 500.0, i: 1.0, d: 1.0}
\ No newline at end of file
diff --git a/xarm_controller/config/xarm5_traj_controller.yaml b/xarm_controller/config/xarm5/xarm5_controllers.yaml
similarity index 64%
rename from xarm_controller/config/xarm5_traj_controller.yaml
rename to xarm_controller/config/xarm5/xarm5_controllers.yaml
index 10f5c40c..2ae5b483 100644
--- a/xarm_controller/config/xarm5_traj_controller.yaml
+++ b/xarm_controller/config/xarm5/xarm5_controllers.yaml
@@ -1,8 +1,9 @@
-# xarm:
+# Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
-  
+
+# default controller   
 xarm5_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints:
@@ -23,7 +24,7 @@ xarm5_traj_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-xarm5_traj_controller_velocity:
+xarm5_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints:
       - joint1
@@ -55,12 +56,24 @@ xarm5_traj_controller_velocity:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 500.0, i: 3.0, d: 1.0}
-#     joint5: {p: 500.0, i: 1.0, d: 1.0}
-
+# Position Controllers ---------------------------------------
+joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint4
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint5
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/xarm5_position_controllers.yaml b/xarm_controller/config/xarm5_position_controllers.yaml
deleted file mode 100644
index eab7fb5b..00000000
--- a/xarm_controller/config/xarm5_position_controllers.yaml
+++ /dev/null
@@ -1,39 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 500.0, i: 3.0, d: 1.0}
-#     joint5: {p: 500.0, i: 1.0, d: 1.0}
-
-
diff --git a/xarm_controller/config/xarm6/dual_xarm6_controllers.yaml b/xarm_controller/config/xarm6/dual_xarm6_controllers.yaml
new file mode 100644
index 00000000..a01106d1
--- /dev/null
+++ b/xarm_controller/config/xarm6/dual_xarm6_controllers.yaml
@@ -0,0 +1,174 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# default controller   
+L_xarm6_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      L_joint1: {trajectory: 1, goal: 0.01}
+      L_joint2: {trajectory: 1, goal: 0.01}
+      L_joint3: {trajectory: 1, goal: 0.01}
+      L_joint4: {trajectory: 1, goal: 0.01}
+      L_joint5: {trajectory: 1, goal: 0.01}
+      L_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+L_xarm6_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+  gains:
+    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    L_joint1: 0.25
+    L_joint2: 0.25
+    L_joint3: 0.25
+    L_joint4: 0.25
+    L_joint5: 0.25
+    L_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    L_joint1: {trajectory: 1, goal: 0.01}
+    L_joint2: {trajectory: 1, goal: 0.01}
+    L_joint3: {trajectory: 1, goal: 0.01}
+    L_joint4: {trajectory: 1, goal: 0.01}
+    L_joint5: {trajectory: 1, goal: 0.01}
+    joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+L_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+L_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+L_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+L_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+L_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+L_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+
+# default controller 
+R_xarm6_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      R_joint1: {trajectory: 1, goal: 0.01}
+      R_joint2: {trajectory: 1, goal: 0.01}
+      R_joint3: {trajectory: 1, goal: 0.01}
+      R_joint4: {trajectory: 1, goal: 0.01}
+      R_joint5: {trajectory: 1, goal: 0.01}
+      R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+R_xarm6_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+  gains:
+    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    R_joint1: 0.25
+    R_joint2: 0.25
+    R_joint3: 0.25
+    R_joint4: 0.25
+    R_joint5: 0.25
+    R_joint6: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    R_joint1: {trajectory: 1, goal: 0.01}
+    R_joint2: {trajectory: 1, goal: 0.01}
+    R_joint3: {trajectory: 1, goal: 0.01}
+    R_joint4: {trajectory: 1, goal: 0.01}
+    R_joint5: {trajectory: 1, goal: 0.01}
+    R_joint6: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+R_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+R_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+R_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+R_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+R_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+R_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/xarm6_traj_controller.yaml b/xarm_controller/config/xarm6/xarm6_controllers.yaml
old mode 100755
new mode 100644
similarity index 63%
rename from xarm_controller/config/xarm6_traj_controller.yaml
rename to xarm_controller/config/xarm6/xarm6_controllers.yaml
index 339d9f2c..5c7f902f
--- a/xarm_controller/config/xarm6_traj_controller.yaml
+++ b/xarm_controller/config/xarm6/xarm6_controllers.yaml
@@ -1,8 +1,9 @@
-# xarm:
+# Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
-  
+
+# default controller   
 xarm6_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints:
@@ -25,7 +26,7 @@ xarm6_traj_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-xarm6_traj_controller_velocity:
+xarm6_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints:
       - joint1
@@ -61,12 +62,28 @@ xarm6_traj_controller_velocity:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
+# Position Controllers ---------------------------------------
+joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
diff --git a/xarm_controller/config/xarm6_position_controllers.yaml b/xarm_controller/config/xarm6_position_controllers.yaml
deleted file mode 100755
index a2992b6b..00000000
--- a/xarm_controller/config/xarm6_position_controllers.yaml
+++ /dev/null
@@ -1,43 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-
-
-# # No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint2: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint3: {p: 600, i: 5.0, d: 30, i_clamp: 1}
-#     joint4: {p: 450.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint5: {p: 300.0, i: 5.0, d: 10, i_clamp: 1}
-#     joint6: {p: 150.0, i: 5.0, d: 10, i_clamp: 1}
-
diff --git a/xarm_controller/config/xarm7/dual_xarm7_controllers.yaml b/xarm_controller/config/xarm7/dual_xarm7_controllers.yaml
new file mode 100644
index 00000000..dfb8966c
--- /dev/null
+++ b/xarm_controller/config/xarm7/dual_xarm7_controllers.yaml
@@ -0,0 +1,194 @@
+# Publish all joint states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
+
+# default controller   
+L_xarm7_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+      - L_joint7
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      L_joint1: {trajectory: 1, goal: 0.01}
+      L_joint2: {trajectory: 1, goal: 0.01}
+      L_joint3: {trajectory: 1, goal: 0.01}
+      L_joint4: {trajectory: 1, goal: 0.01}
+      L_joint5: {trajectory: 1, goal: 0.01}
+      L_joint6: {trajectory: 1, goal: 0.01}
+      L_joint7: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+L_xarm7_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - L_joint1
+      - L_joint2
+      - L_joint3
+      - L_joint4
+      - L_joint5
+      - L_joint6
+      - L_joint7
+  gains:
+    L_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    L_joint7: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    L_joint1: 0.25
+    L_joint2: 0.25
+    L_joint3: 0.25
+    L_joint4: 0.25
+    L_joint5: 0.25
+    L_joint6: 0.25
+    L_joint7: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    L_joint1: {trajectory: 1, goal: 0.01}
+    L_joint2: {trajectory: 1, goal: 0.01}
+    L_joint3: {trajectory: 1, goal: 0.01}
+    L_joint4: {trajectory: 1, goal: 0.01}
+    L_joint5: {trajectory: 1, goal: 0.01}
+    L_joint6: {trajectory: 1, goal: 0.01}
+    L_joint7: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+L_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+L_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+L_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+L_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+L_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+L_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+L_joint7_position_controller:
+  type: position_controllers/JointPositionController
+  joint: L_joint7
+  pid: {p: 300.0, i: 0.05, d: 1.0}
+
+# default controller 
+R_xarm7_traj_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+      - R_joint7
+  constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.05
+      R_joint1: {trajectory: 1, goal: 0.01}
+      R_joint2: {trajectory: 1, goal: 0.01}
+      R_joint3: {trajectory: 1, goal: 0.01}
+      R_joint4: {trajectory: 1, goal: 0.01}
+      R_joint5: {trajectory: 1, goal: 0.01}
+      R_joint6: {trajectory: 1, goal: 0.01}
+      R_joint7: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+R_xarm7_velo_traj_controller:
+  type: velocity_controllers/JointTrajectoryController
+  joints:
+      - R_joint1
+      - R_joint2
+      - R_joint3
+      - R_joint4
+      - R_joint5
+      - R_joint6
+      - R_joint7
+  gains:
+    R_joint1: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint2: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint3: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint4: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint5: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint6: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+    R_joint7: {p: 10, i: 0.0, d: 0.0, i_clamp: 1}
+  velocity_ff:
+    R_joint1: 0.25
+    R_joint2: 0.25
+    R_joint3: 0.25
+    R_joint4: 0.25
+    R_joint5: 0.25
+    R_joint6: 0.25
+    R_joint7: 0.25
+  constraints:
+    goal_time: 0.5
+    stopped_velocity_tolerance: 0.01
+    R_joint1: {trajectory: 1, goal: 0.01}
+    R_joint2: {trajectory: 1, goal: 0.01}
+    R_joint3: {trajectory: 1, goal: 0.01}
+    R_joint4: {trajectory: 1, goal: 0.01}
+    R_joint5: {trajectory: 1, goal: 0.01}
+    R_joint6: {trajectory: 1, goal: 0.01}
+    R_joint7: {trajectory: 1, goal: 0.01}
+  stop_trajectory_duration: 0.2
+  state_publish_rate:  25
+  action_monitor_rate: 10
+
+# Position Controllers ---------------------------------------
+R_joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+R_joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+R_joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+R_joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+R_joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+R_joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+R_joint7_position_controller:
+  type: position_controllers/JointPositionController
+  joint: R_joint7
+  pid: {p: 300.0, i: 0.05, d: 1.0}
diff --git a/xarm_controller/config/xarm7_traj_controller.yaml b/xarm_controller/config/xarm7/xarm7_controllers.yaml
old mode 100755
new mode 100644
similarity index 63%
rename from xarm_controller/config/xarm7_traj_controller.yaml
rename to xarm_controller/config/xarm7/xarm7_controllers.yaml
index 248d5b47..da67c96e
--- a/xarm_controller/config/xarm7_traj_controller.yaml
+++ b/xarm_controller/config/xarm7/xarm7_controllers.yaml
@@ -1,8 +1,9 @@
-# xarm:
+# Publish all joint states
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50
-  
+
+# default controller   
 xarm7_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints:
@@ -27,7 +28,7 @@ xarm7_traj_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-xarm7_traj_controller_velocity:
+xarm7_velo_traj_controller:
   type: velocity_controllers/JointTrajectoryController
   joints:
       - joint1
@@ -67,14 +68,32 @@ xarm7_traj_controller_velocity:
   state_publish_rate:  25
   action_monitor_rate: 10
 
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
-#     joint7: {p: 300.0, i: 0.05, d: 1.0}
-
+# Position Controllers ---------------------------------------
+joint1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint1
+  pid: {p: 1200.0, i: 5.0, d: 10.0}
+joint2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint2
+  pid: {p: 1400.0, i: 5.0, d: 10.0}
+joint3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint3
+  pid: {p: 1200.0, i: 5.0, d: 5.0}
+joint4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint4
+  pid: {p: 850.0, i: 3.0, d: 5.0}
+joint5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint5
+  pid: {p: 500.0, i: 3.0, d: 1.0}
+joint6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint6
+  pid: {p: 500.0, i: 1.0, d: 1.0}
+joint7_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint7
+  pid: {p: 300.0, i: 0.05, d: 1.0}
diff --git a/xarm_controller/config/xarm7_effort_controllers.yaml b/xarm_controller/config/xarm7_effort_controllers.yaml
deleted file mode 100755
index 7d57e7ac..00000000
--- a/xarm_controller/config/xarm7_effort_controllers.yaml
+++ /dev/null
@@ -1,67 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-joint7_position_controller:
-  type: effort_controllers/JointPositionController
-  joint: joint7
-  pid: {p: 300.0, i: 0.05, d: 1.0}
-
-
-# Effort Controllers ---------------------------------------
-joint1_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-joint7_effort_controller:
-  type: effort_controllers/JointEffortController
-  joint: joint7
-  pid: {p: 300.0, i: 0.05, d: 1.0}
-
diff --git a/xarm_controller/config/xarm7_position_controllers.yaml b/xarm_controller/config/xarm7_position_controllers.yaml
deleted file mode 100755
index 0fd384db..00000000
--- a/xarm_controller/config/xarm7_position_controllers.yaml
+++ /dev/null
@@ -1,47 +0,0 @@
-# xarm:
-# Publish all joint states -----------------------------------
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50
-
-# Position Controllers ---------------------------------------
-joint1_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint1
-  pid: {p: 1200.0, i: 5.0, d: 10.0}
-joint2_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint2
-  pid: {p: 1400.0, i: 5.0, d: 10.0}
-joint3_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint3
-  pid: {p: 1200.0, i: 5.0, d: 5.0}
-joint4_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint4
-  pid: {p: 850.0, i: 3.0, d: 5.0}
-joint5_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint5
-  pid: {p: 500.0, i: 3.0, d: 1.0}
-joint6_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint6
-  pid: {p: 500.0, i: 1.0, d: 1.0}
-joint7_position_controller:
-  type: position_controllers/JointPositionController
-  joint: joint7
-  pid: {p: 300.0, i: 0.05, d: 1.0}
-
-# No Pid gains specified error fix
-# gazebo_ros_control:
-#   pid_gains:
-#     joint1: {p: 1200.0, i: 5.0, d: 10.0}
-#     joint2: {p: 1400.0, i: 5.0, d: 10.0}
-#     joint3: {p: 1200.0, i: 5.0, d: 5.0}
-#     joint4: {p: 850.0, i: 3.0, d: 5.0}
-#     joint5: {p: 500.0, i: 3.0, d: 1.0}
-#     joint6: {p: 500.0, i: 1.0, d: 1.0}
-#     joint7: {p: 300.0, i: 0.05, d: 1.0}
-
diff --git a/xarm_controller/config/xarm_gripper_controller.yaml b/xarm_controller/config/xarm_gripper_controller.yaml
deleted file mode 100644
index 8ed69e08..00000000
--- a/xarm_controller/config/xarm_gripper_controller.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-xarm_gripper:
-  joint_state_controller:
-    type: joint_state_controller/JointStateController
-    publish_rate: 50
-
-  gripper_controller:
-    type: position_controllers/GripperActionController
-    joint: drive_joint
-    action_monitor_rate: 20
-    goal_tolerance: 0.005
-    max_effort: 100
-    stall_velocity_threshold: 0.001
-    stall_timeout: 1.0
\ No newline at end of file
diff --git a/xarm_controller/launch/_robot_control.launch b/xarm_controller/launch/_robot_control.launch
index 9013c6b4..a07ded9b 100755
--- a/xarm_controller/launch/_robot_control.launch
+++ b/xarm_controller/launch/_robot_control.launch
@@ -5,6 +5,7 @@
   <arg name="velocity_control" default="false"/>
   <arg name="hw_ns" default="xarm"/>
   <arg name="add_gripper" default="false" />
+  <arg name="gripper_action" default="false" />
   <arg name="dual" default="false" />
 
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
@@ -12,10 +13,10 @@
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <!-- Load trajectory controller for xArm by default -->
-  <rosparam unless="$(arg dual)" file="$(find xarm_controller)/config/$(arg robot_name)_traj_controller.yaml" command="load" ns="$(arg hw_ns)"/>
+  <rosparam unless="$(arg dual)" file="$(find xarm_controller)/config/$(arg robot_name)/$(arg robot_name)_controllers.yaml" command="load" ns="$(arg hw_ns)"/>
   <!-- dual -->
-  <rosparam if="$(arg dual)" file="$(find xarm_controller)/config/dual_$(arg robot_name)_traj_controller.yaml" command="load" ns="$(arg hw_ns)"/>
-  <arg name="xarm_traj_controller" value="$(eval arg('robot_name') + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
+  <rosparam if="$(arg dual)" file="$(find xarm_controller)/config/$(arg robot_name)/dual_$(arg robot_name)_controllers.yaml" command="load" ns="$(arg hw_ns)"/>
+  <arg name="robot_controller_name" value="$(eval arg('robot_name') + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
   
   <node unless="$(arg dual)"
     name="traj_controller_spawner"
@@ -24,7 +25,7 @@
     respawn="false"
     output="screen"
     ns="/$(arg hw_ns)"
-    args="spawn $(arg xarm_traj_controller) joint_state_controller"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
   <!-- dual -->
   <node if="$(arg dual)"
     name="traj_controller_spawner"
@@ -33,22 +34,22 @@
     respawn="false"
     output="screen"
     ns="/$(arg hw_ns)"
-    args="spawn L_$(arg xarm_traj_controller) R_$(arg xarm_traj_controller) joint_state_controller"/>
+    args="spawn L_$(arg robot_controller_name) R_$(arg robot_controller_name) joint_state_controller"/>
 
   <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite' and not arg('dual'))"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg hw_ns)"/>
+    file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg hw_ns)"/>
   <!-- dual -->
   <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite' and arg('dual'))"
-    file="$(find xarm_controller)/config/dual_gripper_traj_controller.yaml" command="load" ns="$(arg hw_ns)"/>
+    file="$(find xarm_controller)/config/gripper/dual_gripper_controllers.yaml" command="load" ns="$(arg hw_ns)"/>
 
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <arg name="gripper_controller_name" value="$(eval 'gripper_action_controller' if arg('gripper_action') else 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(eval arg('add_gripper') and arg('robot_type') != 'lite' and not arg('dual'))" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg hw_ns)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
   <!-- dual -->
   <node if="$(eval arg('add_gripper') and arg('robot_type') != 'lite' and arg('dual'))" name="gripper_controller_spawner"
     pkg="controller_manager"
@@ -56,7 +57,7 @@
     respawn="false"
     output="screen"
     ns="/$(arg hw_ns)"
-    args="L_$(arg gripper_trajectory_controller) R_$(arg gripper_trajectory_controller) "/>
+    args="L_$(arg gripper_controller_name) R_$(arg gripper_controller_name) "/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/dual_xarm6_control.launch b/xarm_controller/launch/dual_xarm6_control.launch
index bd64c579..f154550d 100755
--- a/xarm_controller/launch/dual_xarm6_control.launch
+++ b/xarm_controller/launch/dual_xarm6_control.launch
@@ -7,9 +7,9 @@
   <arg name="add_gripper" default="false" />
   
   <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find xarm_controller)/config/xarm6/dual_xarm6_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <!-- Load trajectory controller for xArm by default -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/dual_xarm6_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -17,11 +17,9 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn L_$(arg xarm_traj_controller) R_$(arg xarm_traj_controller) joint_state_controller"/>
+    args="spawn L_$(arg robot_controller_name) R_$(arg robot_controller_name) joint_state_controller"/>
 
   <!-- run_demo: load the position controllers -->
-  <rosparam if="$(arg run_demo_traj)"
-    file="$(find xarm_controller)/config/dual_xarm6_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
     pkg="controller_manager"
     type="spawner"
@@ -36,16 +34,15 @@
           R_joint5_position_controller R_joint6_position_controller
           joint_state_controller"/>
 
-  <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/dual_gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/dual_gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="L_$(arg gripper_trajectory_controller) R_$(arg gripper_trajectory_controller)"/>
+    args="L_$(arg gripper_controller_name) R_$(arg gripper_controller_name)"/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/lite6_control.launch b/xarm_controller/launch/lite6_control.launch
index 0e5d76d2..d6c54f1b 100755
--- a/xarm_controller/launch/lite6_control.launch
+++ b/xarm_controller/launch/lite6_control.launch
@@ -7,9 +7,9 @@
   <arg name="add_gripper" default="false" />
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find xarm_controller)/config/lite6/lite6_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <!-- Load trajectory controller for xArm by default -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/lite6_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="uf_traj_controller" value="$(eval 'lite6_traj_controller_velocity' if arg('velocity_control') else 'lite6_traj_controller')" />
+  <arg name="robot_controller_name" value="$(eval 'lite6_velo_traj_controller' if arg('velocity_control') else 'lite6_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -17,11 +17,9 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn $(arg uf_traj_controller) joint_state_controller"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
 
   <!-- run_demo: load the position controllers -->
-  <rosparam if="$(arg run_demo_traj)"
-    file="$(find xarm_controller)/config/lite6_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
     pkg="controller_manager"
     type="spawner"
@@ -33,16 +31,15 @@
           joint5_position_controller joint6_position_controller
           joint_state_controller"/>
 
-  <!-- <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <!-- <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
  -->
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/uf850_control.launch b/xarm_controller/launch/uf850_control.launch
index 548de2d2..cdf5f412 100755
--- a/xarm_controller/launch/uf850_control.launch
+++ b/xarm_controller/launch/uf850_control.launch
@@ -7,9 +7,9 @@
   <arg name="add_gripper" default="false" />
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find xarm_controller)/config/uf850/uf850_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <!-- Load trajectory controller for xArm by default -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/uf850_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="uf_traj_controller" value="$(eval 'uf850_traj_controller_velocity' if arg('velocity_control') else 'uf850_traj_controller')" />
+  <arg name="robot_controller_name" value="$(eval 'uf850_velo_traj_controller' if arg('velocity_control') else 'uf850_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -17,11 +17,9 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn $(arg uf_traj_controller) joint_state_controller"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
 
   <!-- run_demo: load the position controllers -->
-  <rosparam if="$(arg run_demo_traj)"
-    file="$(find xarm_controller)/config/uf850_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
     pkg="controller_manager"
     type="spawner"
@@ -33,16 +31,15 @@
           joint5_position_controller joint6_position_controller
           joint_state_controller"/>
 
-  <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/xarm5_control.launch b/xarm_controller/launch/xarm5_control.launch
index 510b6f08..cd22552a 100755
--- a/xarm_controller/launch/xarm5_control.launch
+++ b/xarm_controller/launch/xarm5_control.launch
@@ -7,8 +7,9 @@
   <arg name="add_gripper" default="false" />
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/xarm5_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm5_traj_controller_velocity' if arg('velocity_control') else 'xarm5_traj_controller')" />
+  <rosparam file="$(find xarm_controller)/config/xarm5/xarm5_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <!-- Load trajectory controller for xArm by default -->
+  <arg name="robot_controller_name" value="$(eval 'xarm5_velo_traj_controller' if arg('velocity_control') else 'xarm5_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -16,10 +17,7 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn $(arg xarm_traj_controller) joint_state_controller"/>
-
-  <rosparam if="$(arg run_demo_traj)"
-    file="$(find xarm_controller)/config/xarm5_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
 
   <!-- run_demo: load the position controllers -->
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
@@ -32,16 +30,15 @@
           joint3_position_controller joint4_position_controller
           joint5_position_controller joint_state_controller"/>
 
-  <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/xarm6_control.launch b/xarm_controller/launch/xarm6_control.launch
index badf28ac..a8c631a6 100755
--- a/xarm_controller/launch/xarm6_control.launch
+++ b/xarm_controller/launch/xarm6_control.launch
@@ -7,9 +7,9 @@
   <arg name="add_gripper" default="false" />
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find xarm_controller)/config/xarm6/xarm6_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <!-- Load trajectory controller for xArm by default -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/xarm6_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm6_traj_controller_velocity' if arg('velocity_control') else 'xarm6_traj_controller')" />
+  <arg name="robot_controller_name" value="$(eval 'xarm6_velo_traj_controller' if arg('velocity_control') else 'xarm6_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -17,11 +17,9 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn $(arg xarm_traj_controller) joint_state_controller"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
 
   <!-- run_demo: load the position controllers -->
-  <rosparam if="$(arg run_demo_traj)"
-    file="$(find xarm_controller)/config/xarm6_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
     pkg="controller_manager"
     type="spawner"
@@ -33,16 +31,15 @@
           joint5_position_controller joint6_position_controller
           joint_state_controller"/>
 
-  <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_controller/launch/xarm7_control.launch b/xarm_controller/launch/xarm7_control.launch
index 30b39be7..7ac04809 100755
--- a/xarm_controller/launch/xarm7_control.launch
+++ b/xarm_controller/launch/xarm7_control.launch
@@ -6,9 +6,10 @@
   <arg name="namespace" default="xarm"/>
   <arg name="add_gripper" default="false" />
 
-  <!-- if don't run the demo, load the joint trajectory controller instead -->
-  <rosparam unless="$(arg run_demo_traj)" file="$(find xarm_controller)/config/xarm7_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="xarm_traj_controller" value="$(eval 'xarm7_traj_controller_velocity' if arg('velocity_control') else 'xarm7_traj_controller')" />
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find xarm_controller)/config/xarm7/xarm7_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <!-- Load trajectory controller for xArm by default -->
+  <arg name="robot_controller_name" value="$(eval 'xarm7_velo_traj_controller' if arg('velocity_control') else 'xarm7_traj_controller')" />
   <node unless="$(arg run_demo_traj)"
     name="traj_controller_spawner"
     pkg="controller_manager"
@@ -16,10 +17,8 @@
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="spawn $(arg xarm_traj_controller) joint_state_controller"/>
+    args="spawn $(arg robot_controller_name) joint_state_controller"/>
 
-  <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam if="$(arg run_demo_traj)" file="$(find xarm_controller)/config/xarm7_position_controllers.yaml" command="load" ns="$(arg namespace)"/>
   <!-- run_demo: load the position controllers -->
   <node if="$(arg run_demo_traj)" name="position_controller_spawner"
     pkg="controller_manager"
@@ -32,16 +31,15 @@
           joint5_position_controller joint6_position_controller
           joint7_position_controller joint_state_controller"/>
 
-  <rosparam if="$(arg add_gripper)"
-    file="$(find xarm_controller)/config/gripper_traj_controller.yaml" command="load" ns="$(arg namespace)"/>
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_controllers.yaml" command="load" ns="$(arg namespace)"/>
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
   <node if="$(arg add_gripper)" name="gripper_controller_spawner"
     pkg="controller_manager"
     type="spawner"
     respawn="false"
     output="screen"
     ns="/$(arg namespace)"
-    args="$(arg gripper_trajectory_controller) "/>
+    args="$(arg gripper_controller_name) "/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
diff --git a/xarm_gazebo/launch/_dual_robot_beside_table.launch b/xarm_gazebo/launch/_dual_robot_beside_table.launch
index cce80690..e949aa88 100755
--- a/xarm_gazebo/launch/_dual_robot_beside_table.launch
+++ b/xarm_gazebo/launch/_dual_robot_beside_table.launch
@@ -38,7 +38,7 @@
   <arg name="robot_name" value="$(eval 'uf850' if arg('robot_type') == 'uf850' else arg('robot_type') + str(arg('robot_dof')))" />
 
   <!-- <rosparam file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/$(arg robot_name)_params.yaml" command="load" ns="$(arg hw_ns)"/> -->
-  <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" file="$(find xarm_controller)/config/gripper/dual_gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_example1_table.world"/>
diff --git a/xarm_gazebo/launch/_robot_beside_table.launch b/xarm_gazebo/launch/_robot_beside_table.launch
index 989c53e0..47351121 100755
--- a/xarm_gazebo/launch/_robot_beside_table.launch
+++ b/xarm_gazebo/launch/_robot_beside_table.launch
@@ -19,6 +19,7 @@
   <arg name="add_realsense_d435i" default="false" />
   <arg name="add_d435i_links" default="false" />
   <arg name="add_gripper" default="false" />
+  <arg name="gripper_action" default="false" />
   <arg name="add_vacuum_gripper" default="false" />
   <arg name="add_other_geometry" default="false"/>
   <arg name="geometry_type" default="box"/>
@@ -37,7 +38,7 @@
   <arg name="robot_name" value="$(eval arg('robot_type') if arg('robot_type') == 'uf850' else arg('robot_type') + str(arg('robot_dof')))" />
 
   <!-- <rosparam file="$(find uf_robot_moveit_config)/config/$(arg robot_name)/$(arg robot_name)_params.yaml" command="load" ns="$(arg hw_ns)"/> -->
-  <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam if="$(eval arg('add_gripper') and arg('robot_type') != 'lite')" file="$(find xarm_controller)/config/gripper/gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_example1_table.world"/>
@@ -89,6 +90,7 @@
     <arg name="effort_control" value="$(arg effort_control)"/>
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="add_gripper" value="$(arg add_gripper)" />
+    <arg name="gripper_action" value="$(arg gripper_action)" />
     <arg name="hw_ns" value="$(arg hw_ns)" />
     <arg name="dual" value="false" />
   </include>
diff --git a/xarm_gazebo/launch/xarm5_beside_table.launch b/xarm_gazebo/launch/xarm5_beside_table.launch
index 4a108819..f1dde479 100755
--- a/xarm_gazebo/launch/xarm5_beside_table.launch
+++ b/xarm_gazebo/launch/xarm5_beside_table.launch
@@ -16,7 +16,7 @@
   <arg name="xarm_velocity_control" value="$(eval arg('velocity_control') and not arg('run_demo'))" />
 
   <rosparam file="$(find xarm5_moveit_config)/config/xarm5_params.yaml" command="load" ns="$(arg namespace)"/>
-  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_example1_table.world"/>
diff --git a/xarm_gazebo/launch/xarm6_beside_table.launch b/xarm_gazebo/launch/xarm6_beside_table.launch
index 9056a547..ed77c90c 100755
--- a/xarm_gazebo/launch/xarm6_beside_table.launch
+++ b/xarm_gazebo/launch/xarm6_beside_table.launch
@@ -16,7 +16,7 @@
   <arg name="xarm_velocity_control" value="$(eval arg('velocity_control') and not arg('run_demo'))" />
 
   <rosparam file="$(find xarm6_moveit_config)/config/xarm6_params.yaml" command="load" ns="$(arg namespace)"/>
-  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_example1_table.world"/>
diff --git a/xarm_gazebo/launch/xarm7_beside_table.launch b/xarm_gazebo/launch/xarm7_beside_table.launch
index a313948c..2e535302 100755
--- a/xarm_gazebo/launch/xarm7_beside_table.launch
+++ b/xarm_gazebo/launch/xarm7_beside_table.launch
@@ -16,7 +16,7 @@
   <arg name="xarm_velocity_control" value="$(eval arg('velocity_control') and not arg('run_demo'))" />
   
   <rosparam file="$(find xarm7_moveit_config)/config/xarm7_params.yaml" command="load" ns="$(arg namespace)"/>
-  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam if="$(arg add_gripper)" file="$(find xarm_controller)/config/gripper/gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_example1_table.world"/>
diff --git a/xarm_gazebo/launch/xarm_camera_scene.launch b/xarm_gazebo/launch/xarm_camera_scene.launch
index 2ab2fba7..a15f5cfa 100755
--- a/xarm_gazebo/launch/xarm_camera_scene.launch
+++ b/xarm_gazebo/launch/xarm_camera_scene.launch
@@ -12,7 +12,7 @@
   <arg name="robot_sn" default="" />
   
   <rosparam file="$(eval find('xarm' + str(arg('robot_dof')) + '_moveit_config') + '/config/xarm' + str(arg('robot_dof')) + '_params.yaml')" command="load" ns="$(arg namespace)"/>
-  <rosparam file="$(find xarm_controller)/config/gripper_gazebo_ros_control.yaml" command="load"/>
+  <rosparam file="$(find xarm_controller)/config/gripper/gripper_gazebo_ros_control.yaml" command="load"/>
   <!-- startup simulated world -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find xarm_gazebo)/worlds/xarm_camera_scene.world"/>
@@ -53,10 +53,10 @@
     <arg name="add_gripper" value="true" />
   </include>
 
-  <arg name="xarm_traj_controller" value="$(eval 'xarm' + str(arg('robot_dof')) + ('_traj_controller_velocity' if arg('velocity_control') else '_traj_controller'))" />
-  <arg name="gripper_trajectory_controller" value="$(eval 'gripper_trajectory_controller_velocity' if arg('velocity_control') else 'gripper_trajectory_controller')" />
-  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg xarm_traj_controller)/follow_joint_trajectory"/>
-  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_trajectory_controller)/follow_joint_trajectory"/>
+  <arg name="robot_controller_name" value="$(eval 'xarm' + str(arg('robot_dof')) + ('_velo_traj_controller' if arg('velocity_control') else '_traj_controller'))" />
+  <arg name="gripper_controller_name" value="$(eval 'gripper_velo_traj_controller' if arg('velocity_control') else 'gripper_traj_controller')" />
+  <remap from="arm_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg robot_controller_name)/follow_joint_trajectory"/>
+  <remap from="gripper_controller/follow_joint_trajectory" to="/$(arg namespace)/$(arg gripper_controller_name)/follow_joint_trajectory"/>
 
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <param name="/use_gui" value="false"/>
diff --git a/xarm_planner/launch/robot_planner_fake.launch b/xarm_planner/launch/robot_planner_fake.launch
index c7e3a730..42c809aa 100644
--- a/xarm_planner/launch/robot_planner_fake.launch
+++ b/xarm_planner/launch/robot_planner_fake.launch
@@ -29,7 +29,7 @@
   <arg name="ext_ns" default="" />
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="true" />
 
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
diff --git a/xarm_planner/launch/robot_planner_realmove.launch b/xarm_planner/launch/robot_planner_realmove.launch
index 9885f20e..55152463 100644
--- a/xarm_planner/launch/robot_planner_realmove.launch
+++ b/xarm_planner/launch/robot_planner_realmove.launch
@@ -38,7 +38,7 @@
   <arg name="jnt_stat_pub_rate" default="10" />
   <!-- load the default move_group planner (not xarm_simple_planner) -->
   <arg name="show_rviz" default="true" />
-  <!-- NO_GUI_CTRL means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
+  <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="true" />
 
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />