From c5f161b73b6e664f9963f6872b8a7ddf9d5b03a9 Mon Sep 17 00:00:00 2001
From: kut <kut@ubuntu.p52.ipa>
Date: Tue, 7 Jul 2020 11:55:53 +0200
Subject: [PATCH 1/4] [ur_gazebo] Switch common files to effort controllers

---
 ur_gazebo/launch/inc/load_ur.launch.xml    | 2 +-
 ur_gazebo/launch/inc/ur_control.launch.xml | 4 ++--
 ur_gazebo/urdf/ur.xacro                    | 2 +-
 ur_gazebo/urdf/ur_macro.xacro              | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/ur_gazebo/launch/inc/load_ur.launch.xml b/ur_gazebo/launch/inc/load_ur.launch.xml
index 1924b16d1..3828d0777 100644
--- a/ur_gazebo/launch/inc/load_ur.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur.launch.xml
@@ -27,7 +27,7 @@
   <arg name="visual_params" doc="YAML file containing the visual model of the robots"/>
 
   <!--Common parameters  -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/ur_control.launch.xml b/ur_gazebo/launch/inc/ur_control.launch.xml
index b92f090d8..3d93c6d66 100644
--- a/ur_gazebo/launch/inc/ur_control.launch.xml
+++ b/ur_gazebo/launch/inc/ur_control.launch.xml
@@ -17,8 +17,8 @@
 
   <!-- Parameters we share with ur_robot_driver -->
   <arg name="controller_config_file" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller"/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller"/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller"/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller"/>
 
   <!-- Gazebo parameters -->
   <arg name="gazebo_model_name" default="robot" doc="The name to give to the model in Gazebo (after spawning it)." />
diff --git a/ur_gazebo/urdf/ur.xacro b/ur_gazebo/urdf/ur.xacro
index dc289abd9..2d202a7d6 100644
--- a/ur_gazebo/urdf/ur.xacro
+++ b/ur_gazebo/urdf/ur.xacro
@@ -45,7 +45,7 @@
     NOTE: this value must correspond to the controller configured in the
           controller .yaml files in the 'config' directory.
   -->
-  <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
+  <xacro:arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface"/>
   <xacro:arg name="safety_limits" default="false"/>
   <xacro:arg name="safety_pos_margin" default="0.15"/>
   <xacro:arg name="safety_k_position" default="20"/>
diff --git a/ur_gazebo/urdf/ur_macro.xacro b/ur_gazebo/urdf/ur_macro.xacro
index 5100baccd..fca825035 100644
--- a/ur_gazebo/urdf/ur_macro.xacro
+++ b/ur_gazebo/urdf/ur_macro.xacro
@@ -46,7 +46,7 @@
    kinematics_parameters_file
    physical_parameters_file
    visual_parameters_file
-   transmission_hw_interface:=hardware_interface/PositionJointInterface
+   transmission_hw_interface:=hardware_interface/EffortJointInterface
    safety_limits:=false safety_pos_margin:=0.15 safety_k_position:=20"
   >
     <!--

From d3f9a1a63fcc7f98e0d3b0844f5c3d348e117203 Mon Sep 17 00:00:00 2001
From: Felix Exner <exner@fzi.de>
Date: Thu, 10 Nov 2022 11:57:11 +0100
Subject: [PATCH 2/4] [moveit_configs] Change default sim controller to effort
 controller

---
 ur10_moveit_config/launch/moveit_planning_execution.launch  | 2 +-
 ur10e_moveit_config/launch/moveit_planning_execution.launch | 2 +-
 ur16e_moveit_config/launch/moveit_planning_execution.launch | 2 +-
 ur3_moveit_config/launch/moveit_planning_execution.launch   | 2 +-
 ur3e_moveit_config/launch/moveit_planning_execution.launch  | 2 +-
 ur5_moveit_config/launch/moveit_planning_execution.launch   | 2 +-
 ur5e_moveit_config/launch/moveit_planning_execution.launch  | 2 +-
 7 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/ur10_moveit_config/launch/moveit_planning_execution.launch b/ur10_moveit_config/launch/moveit_planning_execution.launch
index 80f728f0e..fc782e940 100644
--- a/ur10_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur10_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur10_moveit_config)/launch/move_group.launch">
diff --git a/ur10e_moveit_config/launch/moveit_planning_execution.launch b/ur10e_moveit_config/launch/moveit_planning_execution.launch
index de9c149f5..500c0e0c5 100644
--- a/ur10e_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur10e_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur10e_moveit_config)/launch/move_group.launch">
diff --git a/ur16e_moveit_config/launch/moveit_planning_execution.launch b/ur16e_moveit_config/launch/moveit_planning_execution.launch
index e3c2882f0..3d22516ee 100644
--- a/ur16e_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur16e_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur16e_moveit_config)/launch/move_group.launch">
diff --git a/ur3_moveit_config/launch/moveit_planning_execution.launch b/ur3_moveit_config/launch/moveit_planning_execution.launch
index 3c97229dd..6c28f97e7 100644
--- a/ur3_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur3_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur3_moveit_config)/launch/move_group.launch">
diff --git a/ur3e_moveit_config/launch/moveit_planning_execution.launch b/ur3e_moveit_config/launch/moveit_planning_execution.launch
index 939c0769b..a83527931 100644
--- a/ur3e_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur3e_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur3e_moveit_config)/launch/move_group.launch">
diff --git a/ur5_moveit_config/launch/moveit_planning_execution.launch b/ur5_moveit_config/launch/moveit_planning_execution.launch
index 3022d867d..7ae8caed1 100644
--- a/ur5_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur5_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur5_moveit_config)/launch/move_group.launch">
diff --git a/ur5e_moveit_config/launch/moveit_planning_execution.launch b/ur5e_moveit_config/launch/moveit_planning_execution.launch
index 82a6e2105..832ab2d23 100644
--- a/ur5e_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur5e_moveit_config/launch/moveit_planning_execution.launch
@@ -3,7 +3,7 @@
   <arg name="debug" default="false" />
 
   <!-- Remap follow_joint_trajectory -->
-  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/pos_joint_traj_controller/follow_joint_trajectory"/>
+  <remap if="$(arg sim)" from="/scaled_pos_joint_traj_controller/follow_joint_trajectory" to="/eff_joint_traj_controller/follow_joint_trajectory"/>
 
   <!-- Launch moveit -->
   <include file="$(find ur5e_moveit_config)/launch/move_group.launch">

From bb2666e7d749eedd456a83cb6769d671c655bfe3 Mon Sep 17 00:00:00 2001
From: kut <kut@ubuntu.p52.ipa>
Date: Tue, 7 Jul 2020 13:38:25 +0200
Subject: [PATCH 3/4] [ur_gazebo] Switch to effort controllers

---
 ur_gazebo/config/ur10_controllers.yaml     | 15 +++++++++++----
 ur_gazebo/config/ur10e_controllers.yaml    | 15 +++++++++++----
 ur_gazebo/config/ur16e_controllers.yaml    | 15 +++++++++++----
 ur_gazebo/config/ur3_controllers.yaml      | 15 +++++++++++----
 ur_gazebo/config/ur3e_controllers.yaml     | 15 +++++++++++----
 ur_gazebo/config/ur5_controllers.yaml      | 15 +++++++++++----
 ur_gazebo/config/ur5e_controllers.yaml     | 15 +++++++++++----
 ur_gazebo/launch/inc/load_ur10.launch.xml  |  2 +-
 ur_gazebo/launch/inc/load_ur10e.launch.xml |  2 +-
 ur_gazebo/launch/inc/load_ur16e.launch.xml |  2 +-
 ur_gazebo/launch/inc/load_ur3.launch.xml   |  2 +-
 ur_gazebo/launch/inc/load_ur3e.launch.xml  |  2 +-
 ur_gazebo/launch/inc/load_ur5.launch.xml   |  2 +-
 ur_gazebo/launch/inc/load_ur5e.launch.xml  |  2 +-
 ur_gazebo/launch/ur10_bringup.launch       |  4 ++--
 ur_gazebo/launch/ur10e_bringup.launch      |  4 ++--
 ur_gazebo/launch/ur16e_bringup.launch      |  4 ++--
 ur_gazebo/launch/ur3_bringup.launch        |  4 ++--
 ur_gazebo/launch/ur3e_bringup.launch       |  4 ++--
 ur_gazebo/launch/ur5_bringup.launch        |  4 ++--
 ur_gazebo/launch/ur5e_bringup.launch       |  4 ++--
 21 files changed, 98 insertions(+), 49 deletions(-)

diff --git a/ur_gazebo/config/ur10_controllers.yaml b/ur_gazebo/config/ur10_controllers.yaml
index 7b6ab9c2c..673d72e69 100644
--- a/ur_gazebo/config/ur10_controllers.yaml
+++ b/ur_gazebo/config/ur10_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 125
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur10e_controllers.yaml b/ur_gazebo/config/ur10e_controllers.yaml
index 3599c8e7d..dda72bf85 100644
--- a/ur_gazebo/config/ur10e_controllers.yaml
+++ b/ur_gazebo/config/ur10e_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 500
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur16e_controllers.yaml b/ur_gazebo/config/ur16e_controllers.yaml
index 3599c8e7d..069aa6b3b 100644
--- a/ur_gazebo/config/ur16e_controllers.yaml
+++ b/ur_gazebo/config/ur16e_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 500
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur3_controllers.yaml b/ur_gazebo/config/ur3_controllers.yaml
index 7b6ab9c2c..86d427084 100644
--- a/ur_gazebo/config/ur3_controllers.yaml
+++ b/ur_gazebo/config/ur3_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 125
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur3e_controllers.yaml b/ur_gazebo/config/ur3e_controllers.yaml
index 3599c8e7d..dda72bf85 100644
--- a/ur_gazebo/config/ur3e_controllers.yaml
+++ b/ur_gazebo/config/ur3e_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 500
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur5_controllers.yaml b/ur_gazebo/config/ur5_controllers.yaml
index 7b6ab9c2c..86d427084 100644
--- a/ur_gazebo/config/ur5_controllers.yaml
+++ b/ur_gazebo/config/ur5_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 125
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/config/ur5e_controllers.yaml b/ur_gazebo/config/ur5e_controllers.yaml
index 3599c8e7d..069aa6b3b 100644
--- a/ur_gazebo/config/ur5e_controllers.yaml
+++ b/ur_gazebo/config/ur5e_controllers.yaml
@@ -2,8 +2,8 @@ joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: &loop_hz 500
 
-pos_joint_traj_controller:
-  type: position_controllers/JointTrajectoryController
+eff_joint_traj_controller:
+  type: effort_controllers/JointTrajectoryController
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
@@ -11,6 +11,13 @@ pos_joint_traj_controller:
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
+  gains: # Required because we're controlling an effort interface
+    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
@@ -24,6 +31,6 @@ pos_joint_traj_controller:
   state_publish_rate: *loop_hz
   action_monitor_rate: 10
 
-joint_group_pos_controller:
-  type: position_controllers/JointGroupPositionController
+joint_group_eff_controller:
+  type: effort_controllers/JointGroupEffortController
   joints: *robot_joints
diff --git a/ur_gazebo/launch/inc/load_ur10.launch.xml b/ur_gazebo/launch/inc/load_ur10.launch.xml
index 5df947b1c..a5f6c0162 100644
--- a/ur_gazebo/launch/inc/load_ur10.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur10.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur10/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur10e.launch.xml b/ur_gazebo/launch/inc/load_ur10e.launch.xml
index f4cf1c99d..9acf8ffc6 100644
--- a/ur_gazebo/launch/inc/load_ur10e.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur10e.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur10e/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur16e.launch.xml b/ur_gazebo/launch/inc/load_ur16e.launch.xml
index 884d1b69c..8c79bd9c9 100644
--- a/ur_gazebo/launch/inc/load_ur16e.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur16e.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur16e/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur3.launch.xml b/ur_gazebo/launch/inc/load_ur3.launch.xml
index 3548b8bbf..beb2de095 100644
--- a/ur_gazebo/launch/inc/load_ur3.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur3.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur3/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur3e.launch.xml b/ur_gazebo/launch/inc/load_ur3e.launch.xml
index 2e351ab27..95eb214ed 100644
--- a/ur_gazebo/launch/inc/load_ur3e.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur3e.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur3e/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur5.launch.xml b/ur_gazebo/launch/inc/load_ur5.launch.xml
index 2569f319f..1bc482b6e 100644
--- a/ur_gazebo/launch/inc/load_ur5.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur5.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur5/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/inc/load_ur5e.launch.xml b/ur_gazebo/launch/inc/load_ur5e.launch.xml
index 634fe643c..72f7f9cac 100644
--- a/ur_gazebo/launch/inc/load_ur5e.launch.xml
+++ b/ur_gazebo/launch/inc/load_ur5e.launch.xml
@@ -7,7 +7,7 @@
   <arg name="visual_params" default="$(find ur_description)/config/ur5e/visual_parameters.yaml"/>
 
   <!--Common parameters -->
-  <arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
+  <arg name="transmission_hw_interface" default="hardware_interface/EffortJointInterface" doc="The hardware_interface to expose for each joint in the simulated robot (one of: [PositionJointInterface, VelocityJointInterface, EffortJointInterface])"/>
   <arg name="safety_limits" default="false" doc="If True, enable the safety limits controller"/>
   <arg name="safety_pos_margin" default="0.15" doc="The lower/upper limits in the safety controller" />
   <arg name="safety_k_position" default="20" doc="Used to set k position in the safety controller" />
diff --git a/ur_gazebo/launch/ur10_bringup.launch b/ur_gazebo/launch/ur10_bringup.launch
index acd6c96fe..5102f8230 100644
--- a/ur_gazebo/launch/ur10_bringup.launch
+++ b/ur_gazebo/launch/ur10_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur10_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur10e_bringup.launch b/ur_gazebo/launch/ur10e_bringup.launch
index 7d39bda8c..177d8e59e 100644
--- a/ur_gazebo/launch/ur10e_bringup.launch
+++ b/ur_gazebo/launch/ur10e_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur10e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur16e_bringup.launch b/ur_gazebo/launch/ur16e_bringup.launch
index d0f193ed6..a2018ff7c 100644
--- a/ur_gazebo/launch/ur16e_bringup.launch
+++ b/ur_gazebo/launch/ur16e_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur16e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur3_bringup.launch b/ur_gazebo/launch/ur3_bringup.launch
index 035d156db..6f22a1d71 100644
--- a/ur_gazebo/launch/ur3_bringup.launch
+++ b/ur_gazebo/launch/ur3_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur3_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur3e_bringup.launch b/ur_gazebo/launch/ur3e_bringup.launch
index 8e3ec545d..93cec225f 100644
--- a/ur_gazebo/launch/ur3e_bringup.launch
+++ b/ur_gazebo/launch/ur3e_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur3e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur5_bringup.launch b/ur_gazebo/launch/ur5_bringup.launch
index bff4f9323..f4edf3307 100644
--- a/ur_gazebo/launch/ur5_bringup.launch
+++ b/ur_gazebo/launch/ur5_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur5_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
diff --git a/ur_gazebo/launch/ur5e_bringup.launch b/ur_gazebo/launch/ur5e_bringup.launch
index 24c884a6c..4dcf940c5 100644
--- a/ur_gazebo/launch/ur5e_bringup.launch
+++ b/ur_gazebo/launch/ur5e_bringup.launch
@@ -39,8 +39,8 @@
 
   <!-- Controller configuration -->
   <arg name="controller_config_file" default="$(find ur_gazebo)/config/ur5e_controllers.yaml" doc="Config file used for defining the ROS-Control controllers."/>
-  <arg name="controllers" default="joint_state_controller pos_joint_traj_controller" doc="Controllers that are activated by default."/>
-  <arg name="stopped_controllers" default="joint_group_pos_controller" doc="Controllers that are initally loaded, but not started."/>
+  <arg name="controllers" default="joint_state_controller eff_joint_traj_controller" doc="Controllers that are activated by default."/>
+  <arg name="stopped_controllers" default="joint_group_eff_controller" doc="Controllers that are initally loaded, but not started."/>
 
   <!-- robot_state_publisher configuration -->
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>

From e60abd3013442e6516916794b05a4a8380421e17 Mon Sep 17 00:00:00 2001
From: "Lucchi, Matteo" <matteo.lucchi@joanneum.at>
Date: Fri, 10 Jul 2020 15:44:01 +0200
Subject: [PATCH 4/4] [ur_gazebo] update effort_controllers' pid parameters

---
 ur_gazebo/config/ur10_controllers.yaml  | 12 ++++++------
 ur_gazebo/config/ur10e_controllers.yaml |  8 ++++----
 ur_gazebo/config/ur16e_controllers.yaml | 12 ++++++------
 ur_gazebo/config/ur3_controllers.yaml   |  2 +-
 ur_gazebo/config/ur3e_controllers.yaml  |  2 +-
 ur_gazebo/config/ur5_controllers.yaml   |  6 +++---
 ur_gazebo/config/ur5e_controllers.yaml  |  6 +++---
 7 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/ur_gazebo/config/ur10_controllers.yaml b/ur_gazebo/config/ur10_controllers.yaml
index 673d72e69..ad5c1e955 100644
--- a/ur_gazebo/config/ur10_controllers.yaml
+++ b/ur_gazebo/config/ur10_controllers.yaml
@@ -12,12 +12,12 @@ eff_joint_traj_controller:
     - wrist_2_joint
     - wrist_3_joint
   gains: # Required because we're controlling an effort interface
-    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
-    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
+    shoulder_pan_joint: {p: 20000,  d: 500, i: 10, i_clamp: 1}
+    shoulder_lift_joint: {p: 20000,  d: 500, i: 10, i_clamp: 1}
+    elbow_joint: {p: 20000,  d: 500, i: 10, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 10, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 10, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur10e_controllers.yaml b/ur_gazebo/config/ur10e_controllers.yaml
index dda72bf85..1c94e84c2 100644
--- a/ur_gazebo/config/ur10e_controllers.yaml
+++ b/ur_gazebo/config/ur10e_controllers.yaml
@@ -12,12 +12,12 @@ eff_joint_traj_controller:
     - wrist_2_joint
     - wrist_3_joint
   gains: # Required because we're controlling an effort interface
-    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
-    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
+    shoulder_pan_joint: {p: 20000,  d: 100, i: 10, i_clamp: 1}
+    shoulder_lift_joint: {p: 20000,  d: 100, i: 20, i_clamp: 1}
+    elbow_joint: {p: 10000,  d: 100, i: 10, i_clamp: 1}
     wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
     wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur16e_controllers.yaml b/ur_gazebo/config/ur16e_controllers.yaml
index 069aa6b3b..ad04faf38 100644
--- a/ur_gazebo/config/ur16e_controllers.yaml
+++ b/ur_gazebo/config/ur16e_controllers.yaml
@@ -12,12 +12,12 @@ eff_joint_traj_controller:
     - wrist_2_joint
     - wrist_3_joint
   gains: # Required because we're controlling an effort interface
-    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
-    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
+    shoulder_pan_joint: {p: 20000,  d: 500, i: 10, i_clamp: 1}
+    shoulder_lift_joint: {p: 20000,  d: 500, i: 10, i_clamp: 1}
+    elbow_joint: {p: 10000,  d: 200, i: 10, i_clamp: 1}
+    wrist_1_joint: {p: 500,  d: 10, i: 1, i_clamp: 1}
+    wrist_2_joint: {p: 500,  d: 10, i: 1, i_clamp: 1}
+    wrist_3_joint: {p: 100,  d: 0.1, i: 0, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur3_controllers.yaml b/ur_gazebo/config/ur3_controllers.yaml
index 86d427084..b4ed47511 100644
--- a/ur_gazebo/config/ur3_controllers.yaml
+++ b/ur_gazebo/config/ur3_controllers.yaml
@@ -17,7 +17,7 @@ eff_joint_traj_controller:
     elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
     wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
     wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur3e_controllers.yaml b/ur_gazebo/config/ur3e_controllers.yaml
index dda72bf85..c159a612f 100644
--- a/ur_gazebo/config/ur3e_controllers.yaml
+++ b/ur_gazebo/config/ur3e_controllers.yaml
@@ -17,7 +17,7 @@ eff_joint_traj_controller:
     elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
     wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
     wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}  
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1}  
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur5_controllers.yaml b/ur_gazebo/config/ur5_controllers.yaml
index 86d427084..a4ff6fff0 100644
--- a/ur_gazebo/config/ur5_controllers.yaml
+++ b/ur_gazebo/config/ur5_controllers.yaml
@@ -12,12 +12,12 @@ eff_joint_traj_controller:
     - wrist_2_joint
     - wrist_3_joint
   gains: # Required because we're controlling an effort interface
-    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    shoulder_pan_joint: {p: 4000,  d: 200, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 10000,  d: 200, i: 1, i_clamp: 1}
     elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
     wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
     wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05
diff --git a/ur_gazebo/config/ur5e_controllers.yaml b/ur_gazebo/config/ur5e_controllers.yaml
index 069aa6b3b..70836f212 100644
--- a/ur_gazebo/config/ur5e_controllers.yaml
+++ b/ur_gazebo/config/ur5e_controllers.yaml
@@ -12,12 +12,12 @@ eff_joint_traj_controller:
     - wrist_2_joint
     - wrist_3_joint
   gains: # Required because we're controlling an effort interface
-    shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
-    shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
+    shoulder_pan_joint: {p: 4000,  d: 200, i: 1, i_clamp: 1}
+    shoulder_lift_joint: {p: 10000,  d: 200, i: 1, i_clamp: 1}
     elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
     wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
     wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
-    wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1} 
+    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1} 
   constraints:
     goal_time: 0.6
     stopped_velocity_tolerance: 0.05