From 6bb82f931c218c3c5b0b6d83a4489d2767d69918 Mon Sep 17 00:00:00 2001
From: dg-shadow <dg@shadowrobot.com>
Date: Thu, 23 Apr 2020 10:00:33 +0000
Subject: [PATCH 01/28] first attempt

---
 .../urdf/right_srhand_ur10.urdf.xacro         |  2 +-
 .../ra_trajectory_controller_no_wrist.yaml    | 20 ++++++++++++++++++-
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro b/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
index 4a9497e22..678718125 100644
--- a/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
+++ b/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
@@ -12,7 +12,7 @@
     </joint>
 
     <xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
-    <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
+    <xacro:arg name="transmission_hw_interface" default="hardware_interface/VelocityJointInterface"/>
 
     <xacro:ur10_robot prefix="ra_" joint_limited="false" transmission_hw_interface="$(arg transmission_hw_interface)" kinematics_file="${load_yaml('$(arg kinematics_config)')}"/>
 
diff --git a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
index d5b0d2f6d..488b80468 100644
--- a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
@@ -1,5 +1,5 @@
 ra_trajectory_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "velocity_controllers/ComplianceController"
   joints:
     - ra_shoulder_pan_joint
     - ra_shoulder_lift_joint
@@ -7,6 +7,7 @@ ra_trajectory_controller:
     - ra_wrist_1_joint
     - ra_wrist_2_joint
     - ra_wrist_3_joint
+  compliance_command_timeout: 1.5
   constraints:
       goal_time: 0.6
       stopped_velocity_tolerance: 1.0
@@ -20,3 +21,20 @@ ra_trajectory_controller:
   state_publish_rate:  25
   action_monitor_rate: 10
   allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    ra_shoulder_pan_joint: 1.0
+    ra_shoulder_lift_joint: 1.0
+    ra_elbow_joint: 1.0
+    ra_wrist_1_joint: 1.0
+    ra_wrist_2_joint: 1.0
+    ra_wrist_3_joint: 1.0

From 5eb47bb83470bea2d84d472ee52bda223dccfe3e Mon Sep 17 00:00:00 2001
From: dg-shadow <dg@shadowrobot.com>
Date: Tue, 28 Apr 2020 09:48:17 +0000
Subject: [PATCH 02/28] Least terrible tuning I could find

---
 .../config/robot_configs/right_sh_ur10.yaml            |  2 +-
 .../controller/ra_trajectory_controller_no_wrist.yaml  | 10 ++++++++++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/sr_multi_moveit/sr_multi_moveit_config/config/robot_configs/right_sh_ur10.yaml b/sr_multi_moveit/sr_multi_moveit_config/config/robot_configs/right_sh_ur10.yaml
index c3c1a58a7..052ea3d2e 100644
--- a/sr_multi_moveit/sr_multi_moveit_config/config/robot_configs/right_sh_ur10.yaml
+++ b/sr_multi_moveit/sr_multi_moveit_config/config/robot_configs/right_sh_ur10.yaml
@@ -6,7 +6,7 @@ robot:
       arm:
         name: ur10
         main_group: manipulator
-        moveit_path: 
+        moveit_path:
           package: sr_multi_moveit_config
           relative_path: /config/ur
         extra_groups_config_path: /config
diff --git a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
index 488b80468..7abd83f9e 100644
--- a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
@@ -1,3 +1,13 @@
+gazebo_ros_control:
+    pid_gains:
+      ra_shoulder_pan_joint:  {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+      ra_shoulder_lift_joint: {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+      ra_elbow_joint:         {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+      ra_wrist_1_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+      ra_wrist_2_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+      ra_wrist_3_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+
+
 ra_trajectory_controller:
   type: "velocity_controllers/ComplianceController"
   joints:

From 0096de90b52e5efb36f7dd3195ed3b5ac48945e9 Mon Sep 17 00:00:00 2001
From: Toni Oliver <toni@shadowrobot.com>
Date: Fri, 1 May 2020 09:31:19 +0000
Subject: [PATCH 03/28] Comment out gazebo pid gains

---
 .../ra_trajectory_controller_no_wrist.yaml       | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
index 7abd83f9e..ee760ec23 100644
--- a/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/gazebo/controller/ra_trajectory_controller_no_wrist.yaml
@@ -1,11 +1,11 @@
-gazebo_ros_control:
-    pid_gains:
-      ra_shoulder_pan_joint:  {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
-      ra_shoulder_lift_joint: {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
-      ra_elbow_joint:         {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
-      ra_wrist_1_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
-      ra_wrist_2_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
-      ra_wrist_3_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+# gazebo_ros_control:
+#     pid_gains:
+#       ra_shoulder_pan_joint:  {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_shoulder_lift_joint: {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_elbow_joint:         {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_1_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_2_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
+#       ra_wrist_3_joint:       {p: 500, i: 1000, d: 0.1, i_clamp_max: 1000, i_clamp_min: -1000, antiwindup: true}
 
 
 ra_trajectory_controller:

From 1e6ee3c97497a8de4f4f9e99bc9832096933b5ea Mon Sep 17 00:00:00 2001
From: Toni Oliver <toni@shadowrobot.com>
Date: Wed, 23 Sep 2020 11:20:00 +0000
Subject: [PATCH 04/28] Add FT sensor to right hand UR10e xacro

---
 .../urdf/right_srhand_ur10e.urdf.xacro        | 32 +++++++++++++++++--
 1 file changed, 29 insertions(+), 3 deletions(-)

diff --git a/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro b/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
index 82fa8bd48..a2d6d96f3 100644
--- a/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
+++ b/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
@@ -3,7 +3,7 @@
     <!--<xacro:include filename="$(find ur_description)/urdf/gazebo.urdf.xacro" />-->
     <xacro:include filename="$(find ur_e_description)/urdf/ur10e.urdf.xacro"/>
     <xacro:arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
-    <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
+    <xacro:arg name="transmission_hw_interface" default="hardware_interface/VelocityJointInterface"/>
 
     <link name="world"/>
     <xacro:arg name="initial_z" default="0.0"/>
@@ -23,9 +23,35 @@
     <xacro:shadowhand muscletrans="false" muscle="false" bio="false" bt_sp="false" ubi="false" eli="false"
                       reflect="1.0" prefix="rh_" lf="true" cable_mesh="true"/>
 
+    <!-- We use wrist_3_link instead of ee_link due to a bug in preserveFixedJoint
+         where it currently won't work with links that don't have an inertial component
+         as is the case with ee_link. These links get removed when converting to SDF.
+         See more on this subject in this issue https://github.com/osrf/sdformat/issues/199 -->
     <joint name="ra_arm_to_hand" type="fixed">
-        <parent link="ra_ee_link"/>
+        <parent link="ra_wrist_3_link"/>
         <child link="rh_forearm"/>
-        <origin xyz="0.012 0.0 0.0" rpy="${pi/2.0} ${0.0} ${pi/2.0}"/>
+        <origin xyz="0.0 0.0 0.012" rpy="${0.0} ${0.0} ${pi}"/>
     </joint>
+
+    <gazebo reference="ra_arm_to_hand">
+      <preserveFixedJoint>true</preserveFixedJoint>
+      <sensor name="ra_force_torque" type="force_torque">
+        <always_on>true</always_on>
+        <visualize>true</visualize>
+        <force_torque>
+          <frame>sensor</frame>
+          <measure_direction>child_to_parent</measure_direction>
+        </force_torque>
+      </sensor>
+
+      <provideFeedback>true</provideFeedback>
+    </gazebo>
+
+    <gazebo>
+      <plugin name="ft_sensor" filename="libgazebo_ros_ft_sensor.so">
+        <updateRate>125.0</updateRate>
+        <topicName>ra_wrench</topicName>
+        <jointName>ra_arm_to_hand</jointName>
+      </plugin>
+    </gazebo>
 </robot>

From 2ddd51fd5d3c8739461e35f7b0e965495a44af38 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Mon, 16 Nov 2020 08:42:42 +0000
Subject: [PATCH 05/28] debugging

---
 .../config/ra_trajectory_controller.yaml      | 26 ------------------
 .../ra_trajectory_controller_no_wrist.yaml    | 27 +++++++++++++++++--
 .../config/right_ur10e_arm_robot_hw.yaml      |  4 +--
 .../config/right_ur_arm_robot_hw.yaml         |  4 +--
 .../launch/sr_hardware_control_loop.launch    |  4 +--
 5 files changed, 31 insertions(+), 34 deletions(-)
 delete mode 100644 sr_robot_launch/config/ra_trajectory_controller.yaml

diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
deleted file mode 100644
index 15a1b1cfe..000000000
--- a/sr_robot_launch/config/ra_trajectory_controller.yaml
+++ /dev/null
@@ -1,26 +0,0 @@
-ra_trajectory_controller:
-  type: "position_controllers/JointTrajectoryController"
-  hardware_interface:
-    joints: &robot_joints
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint
-      - rh_WRJ1
-      - rh_WRJ2
-  joints: *robot_joints
-  constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
-      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
-      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
-      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
-      rh_WRJ1: {trajectory: 0.9, goal: 0.4}
-      rh_WRJ2: {trajectory: 0.9, goal: 0.4}
-  stop_trajectory_duration: 0.5
-  allow_partial_joints_goal: true
diff --git a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
index b39084d67..056fbc001 100644
--- a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
@@ -1,5 +1,7 @@
+# type: "velocity_controllers/ComplianceController"
+#   type: "velocity_controllers/JointTrajectoryController"
 ra_trajectory_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "velocity_controllers/ComplianceController"
   joints:
     - ra_shoulder_pan_joint
     - ra_shoulder_lift_joint
@@ -7,9 +9,10 @@ ra_trajectory_controller:
     - ra_wrist_1_joint
     - ra_wrist_2_joint
     - ra_wrist_3_joint
+  compliance_command_timeout: 1.5
   constraints:
       goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
+      stopped_velocity_tolerance: 1.0
       ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
       ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
       ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
@@ -17,4 +20,24 @@ ra_trajectory_controller:
       ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
       ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
   stop_trajectory_duration: 0.5
+  state_publish_rate:  500
+  action_monitor_rate: 10
   allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    var: &v_ff 0.8
+    ra_shoulder_pan_joint: *v_ff
+    ra_shoulder_lift_joint: *v_ff
+    ra_elbow_joint: *v_ff
+    ra_wrist_1_joint: *v_ff
+    ra_wrist_2_joint: *v_ff
+    ra_wrist_3_joint: *v_ff
diff --git a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
index a8da278c7..bcb5b8620 100644
--- a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
@@ -15,11 +15,11 @@ ra_sr_ur_robot_hw:
   
   # for sending robot programs
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  robot_ip: 10.8.1.1
+  robot_ip: 192.168.1.1
   
   # for the server that moves the robot
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  control_pc_ip_address: 10.8.1.100
+  control_pc_ip_address: 192.168.1.100
   
   input_recipe_file: /home/user/projects/shadow_robot/base/src/Universal_Robots_ROS_Driver/ur_robot_driver/resources/rtde_input_recipe.txt
   
diff --git a/sr_robot_launch/config/right_ur_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
index affded94d..23103e172 100644
--- a/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
@@ -15,11 +15,11 @@ ra_sr_ur_robot_hw:
   
   # for sending robot programs
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  robot_ip: 10.8.1.1
+  robot_ip: 192.168.1.1
   
   # for the server that moves the robot
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  control_pc_ip_address: 10.8.1.100
+  control_pc_ip_address: 192.168.1.100
   
   input_recipe_file: /home/user/projects/shadow_robot/base/src/Universal_Robots_ROS_Driver/ur_robot_driver/resources/rtde_input_recipe.txt
   
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index db324008a..4fc3820f0 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -12,7 +12,7 @@
   <!-- Whether there is an arm. -->
   <arg name="arm" default="true"/>
   <!-- Whether there is a hand. -->
-  <arg name="hand" default="true"/>
+  <arg name="hand" default="false"/>
   <!-- Whether to run arm controllers. -->
   <arg name="arm_ctrl" default="$(arg arm)"/>
   <!-- Whether to run hand controllers. -->
@@ -53,7 +53,7 @@
   <arg name="arm_speed_scale" default="0.5"/>
   <arg if="$(arg hand_ctrl)" name="arm_payload_mass" default="4.55"/>
   <arg if="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
-  <arg unless="$(arg hand_ctrl)" name="arm_payload_mass" default="0.0"/>
+  <arg unless="$(arg hand_ctrl)" name="arm_payload_mass" default="0.012"/>
   <arg unless="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
   <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>

From 1beca708ab6ea0b487b945a7f261ad941c5b0979 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Mon, 16 Nov 2020 08:50:17 +0000
Subject: [PATCH 06/28] debugging

---
 .../ra_trajectory_controller_no_wrist.yaml    |  24 ++-
 sr_robot_launch/launch/sr_ur10arm_box.launch  | 160 ++++++++++++++++++
 2 files changed, 182 insertions(+), 2 deletions(-)
 create mode 100644 sr_robot_launch/launch/sr_ur10arm_box.launch

diff --git a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
index b39084d67..488b80468 100644
--- a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
@@ -1,5 +1,5 @@
 ra_trajectory_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "velocity_controllers/ComplianceController"
   joints:
     - ra_shoulder_pan_joint
     - ra_shoulder_lift_joint
@@ -7,9 +7,10 @@ ra_trajectory_controller:
     - ra_wrist_1_joint
     - ra_wrist_2_joint
     - ra_wrist_3_joint
+  compliance_command_timeout: 1.5
   constraints:
       goal_time: 0.6
-      stopped_velocity_tolerance: 0.05
+      stopped_velocity_tolerance: 1.0
       ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
       ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
       ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
@@ -17,4 +18,23 @@ ra_trajectory_controller:
       ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
       ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
   stop_trajectory_duration: 0.5
+  state_publish_rate:  25
+  action_monitor_rate: 10
   allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    ra_shoulder_pan_joint: 1.0
+    ra_shoulder_lift_joint: 1.0
+    ra_elbow_joint: 1.0
+    ra_wrist_1_joint: 1.0
+    ra_wrist_2_joint: 1.0
+    ra_wrist_3_joint: 1.0
diff --git a/sr_robot_launch/launch/sr_ur10arm_box.launch b/sr_robot_launch/launch/sr_ur10arm_box.launch
new file mode 100644
index 000000000..afb6e458a
--- /dev/null
+++ b/sr_robot_launch/launch/sr_ur10arm_box.launch
@@ -0,0 +1,160 @@
+<launch>
+  <!-- This launch file is for a ur arm and box -->
+
+  <!-- set to false if you want to use real robots, true if you need the gazebo simulation -->
+  <arg name="sim" default="true"/>
+  <!-- Set to true for debugging -->
+  <arg name="debug" default="false"/>
+  <!-- Set to true to allow planning trajectories-->
+  <arg name="use_moveit" default="true"/>
+  <!--Choose planner library, options are ompl, stomp or sbpl-->
+  <arg name="planning" default="ompl"/>
+  <!-- Set to false if you want to start without guis -->
+  <arg name="gui" default="true"/>
+  <arg name="verbose" default="true"/>
+  <!-- Set whether to load a scene to restrain the movement of the robot -->
+  <arg name="scene" default="false"/>
+    <!-- Set the scene file -->
+  <arg name="scene_file" default="$(find sr_description_common)/scenes/demo_space_large.scene"/>
+  <!-- Set world value dependant on scene -->
+  <arg name="world" default="$(find sr_description_common)/worlds/shadowhands_and_arms.world" unless="$(arg scene)"/>
+  <arg name="world" default="$(find sr_description_common)/worlds/demo_space_large.world" if="$(arg scene)"/>
+  <!-- Set to true if you want the robot to start in the "home" positions -->
+  <arg name="start_home" default="false"/>
+
+  <!-- ROBOT CONFIG-->
+  <arg name="side" default="right"/>
+  <arg name="side_letter" value="$(eval side[0])"/>
+  <arg name="arm_prefix" value="$(arg side_letter)a"/>
+  <arg name="robot_model" default="ur10e"/>
+  <arg name="robot_description" default="'$(find sr_box_ur10_moveit_config)/config/$(arg arm_prefix)_$(arg robot_model)_with_box.urdf.xacro'"/>
+  <arg name="robot_config_file" default="$(find sr_box_ur10_moveit_config)/config/robot_configs/$(arg side)_$(arg robot_model).yaml"/>
+
+  <!-- Definition of "home" position, in joint-angle pairs -->
+  <arg name="home_angles" unless="$(eval side=='left')" default="-J ra_shoulder_pan_joint 1.147 -J ra_elbow_joint 1.695 -J ra_wrist_1_joint -1.395 -J ra_wrist_2_joint -1.584 -J ra_shoulder_lift_joint -1.926 -J ra_wrist_3_joint 1.830"/>
+  <arg name="home_angles" if="$(eval side=='left')" default="-J la_shoulder_pan_joint 1.147 -J la_elbow_joint -1.695 -J la_wrist_1_joint -1.75 -J la_wrist_2_joint 1.57 -J la_shoulder_lift_joint -1.22 -J la_wrist_3_joint -1.830"/>
+  <!-- Set this to true if you run the hardware loop on a remote computer -->
+  <arg name="external_control_loop" default="false"/>
+  <!-- The robot state publisher publishing frequency  -->
+  <arg name="robot_state_pub_frequency" default="250"/>
+  <!-- The joint state publisher publishing frequency  -->
+  <arg name="joint_state_pub_frequency" default="125"/>
+
+  <!-- ARM CONFIG-->
+  <arg name="initial_z" default="0.7551" if="$(arg scene)"/>
+  <arg name="initial_z" default="0.1" unless="$(arg scene)"/>
+  <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
+  <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
+  <rosparam command="load" file="$(arg kinematics_config)" ns="$(arg arm_prefix)_sr_ur_robot_hw"/>
+
+  <!-- TODO: check if we need a different tuning for ur5/ur5e  -->
+  <arg name="arm_robot_hw" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur_arm_robot_hw.yaml"/>
+  <arg name="arm_robot_hw" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur10e_arm_robot_hw.yaml"/>
+
+  <arg name="arm_speed_scale" default="0.5"/>
+  <arg name="arm_payload_mass" default="0"/>
+  <arg name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
+
+  <!-- Set to true to spawn trajectory controllers for the arms (the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
+  <arg name="arm_trajectory" default="true"/>
+  <!-- Set to true to spawn group position controllers for the arms -->
+  <arg name="arm_position" default="$(eval not arm_trajectory)"/>
+  <param name="description_read_repetitions" value="50"/>
+
+  <!-- ROBOT CONFIG-->
+  <arg name="robot_name" default="ursrbox"/>
+
+  <!-- Logging -->
+  <include file="$(find sr_logging_common)/launch/sr_rosbag_log.launch"/>
+
+  <!-- SIMULATED ROBOTS -->
+  <group if="$(arg sim)">
+    <arg name="paused" value="$(arg start_home)"/>
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+      <arg name="world_name" value="$(arg world)"/>
+      <arg name="gui" default="$(arg gui)"/>
+      <arg name="paused" value="$(arg paused)"/>
+      <arg name="debug" value="$(arg debug)"/>
+      <arg name="verbose" value="$(arg verbose)"/>
+    </include>
+    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config)"/>
+
+    <arg if="$(arg start_home)" name="spawn_model_extra_args" value="-unpause $(arg home_angles)" />
+    <arg unless="$(arg start_home)" name="spawn_model_extra_args" value="" />
+    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model $(arg robot_name) $(arg spawn_model_extra_args)" respawn="false" output="screen"/>
+
+    <!-- ARM -->
+    <rosparam if="$(arg arm_trajectory)" file="$(find sr_box_ur10_moveit_config)/config/gazebo/$(arg arm_prefix)_trajectory_controller.yaml" command="load"/>
+    <rosparam if="$(arg arm_position)" file="$(find sr_box_ur10_moveit_config)/config/gazebo/$(arg arm_prefix)_group_position_controller.yaml" command="load"/>
+    <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch" >
+      <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
+    </include>
+
+    <node if="$(arg arm_trajectory)" name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="$(arg arm_prefix)_trajectory_controller"/>
+    <node if="$(arg arm_position)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="$(arg arm_prefix)_group_position_controller"/>
+
+    <!-- Robot state publisher -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)"/>
+      <param name="tf_prefix" type="string" value=""/>
+    </node>
+
+    <!-- Set scene for robot - restrains movement -->
+    <group if="$(arg scene)">
+      <include if="$(arg use_moveit)" file="$(find gazebo2rviz)/launch/gazebo2moveit.launch">
+        <arg name="frequency" default="1" />
+      </include>
+    </group>
+  </group>
+
+  <!-- REAL ROBOTS -->
+  <group unless="$(arg sim)">
+    <group unless="$(arg external_control_loop)">
+      <include file="$(find sr_robot_launch)/launch/sr_hardware_control_loop.launch">
+        <arg name="debug" default="$(arg debug)"/>
+        <arg name="initial_z" default="0.7551" if="$(arg scene)"/>
+        <arg name="initial_z" default="0.1" unless="$(arg scene)"/>
+        <arg name="robot_description" value="$(arg robot_description)"/>
+        <arg name="side" value="$(arg side)"/>
+        <arg name="robot_model" value="$(arg robot_model)"/>
+        <arg name="arm_robot_hw" value="$(arg arm_robot_hw)"/>
+        <arg name="arm_trajectory" default="$(arg arm_trajectory)"/>
+        <arg name="arm_position" default="$(arg arm_position)"/>
+        <arg name="arm_speed_scale" value="$(arg arm_speed_scale)"/>
+        <arg name="arm_payload_mass" value="$(arg arm_payload_mass)"/>
+        <arg name="arm_payload_cog" value="$(arg arm_payload_cog)"/>
+        <arg name="hand" default="false"/>
+        <arg name="robot_state_pub_frequency" value="$(arg robot_state_pub_frequency)"/>
+        <arg name="joint_state_pub_frequency" value="$(arg joint_state_pub_frequency)"/>
+      </include>
+    </group>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)"/>
+      <param name="tf_prefix" type="string" value="" />
+    </node>
+    <!-- Set scene for robot after delay to start after moveit - restrains movement -->
+    <group if="$(arg scene)">
+      <node name="conditional_delayed_rostool_arm_scene" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
+        <param name="package_name" value="sr_box_ur10_moveit_config" />
+        <param name="executable_name" value="scene.launch" />
+        <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
+        <param name="launch_args_list" value="scene_file:=$(arg scene_file)" />
+        <param name="timeout" value="20.0" />
+      </node>
+    </group>
+  </group>
+
+  <rosparam file="$(arg robot_config_file)"/>
+
+  <!-- Launch moveit configuration after delay to wait for robot description to be avalible -->
+  <group if="$(arg use_moveit)">
+    <node name="conditional_delayed_rostool_rviz_multi" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen" required="true">
+      <param name="package_name" value="sr_box_ur10_moveit_config" />
+      <param name="executable_name" value="planning_and_execution.launch" />
+      <rosparam param="params_list">[/robot_description]</rosparam>
+      <param name="launch_args_list" value="side:=$(arg side) use_gui:=$(arg gui)" />
+      <param name="timeout" value="20.0" />
+    </node>
+  </group>
+</launch>

From 2da2801858e721b241c88245fb14909818409328 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Tue, 24 Nov 2020 15:54:17 +0000
Subject: [PATCH 07/28] parameters

---
 .../ra_trajectory_controller_no_wrist.yaml    | 20 +++++++++++-------
 .../config/right_ur10e_arm_robot_hw.yaml      | 21 +++++++++++--------
 2 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
index 3324033df..9702e8209 100644
--- a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
@@ -12,7 +12,7 @@ ra_trajectory_controller:
   compliance_command_timeout: 1.5
   constraints:
       goal_time: 0.6
-      stopped_velocity_tolerance: 1.0
+      stopped_velocity_tolerance: 0.1
       ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
       ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
       ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
@@ -25,16 +25,20 @@ ra_trajectory_controller:
   allow_partial_joints_goal: true
   #!!These values have not been optimized!!
   gains:
-    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    p: &p_gain 20.0
+    i: &i_gain 0.0
+    d: &d_gain 0.0
+    clamp: &i_clamp 1.0
+    ra_shoulder_pan_joint:  {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_shoulder_lift_joint: {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_elbow_joint:         {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_1_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_2_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_3_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
 
   # Use a feedforward term to reduce the size of PID gains
   velocity_ff:
-    var: &v_ff 0.8
+    var: &v_ff 0.0
     ra_shoulder_pan_joint: *v_ff
     ra_shoulder_lift_joint: *v_ff
     ra_elbow_joint: *v_ff
diff --git a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
index bcb5b8620..fdea02ea2 100644
--- a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
@@ -1,12 +1,11 @@
 ra_sr_ur_robot_hw:
-  hardware_interface:
-    joints: &robot_joints
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint
+  joints: &robot_joints
+    - ra_shoulder_pan_joint
+    - ra_shoulder_lift_joint
+    - ra_elbow_joint
+    - ra_wrist_1_joint
+    - ra_wrist_2_joint
+    - ra_wrist_3_joint
 
   type: ur_driver/HardwareInterface
   
@@ -41,6 +40,10 @@ ra_sr_ur_robot_hw:
 
   use_local_hardware_interface_param: true
   
-  non_blocking_read: true
+#  non_blocking_read: true
+
+  non_blocking_read: false
 
   wrench_prefix: right_
+
+  tf_prefix: ra_

From a7f008f304175bdca6a413bc0eba222229d225bc Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Tue, 24 Nov 2020 15:57:14 +0000
Subject: [PATCH 08/28] params

---
 .../ra_trajectory_controller_no_wrist.yaml    | 25 +++++++++++--------
 .../config/right_ur10e_arm_robot_hw.yaml      | 19 +++++++-------
 2 files changed, 25 insertions(+), 19 deletions(-)

diff --git a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
index 056fbc001..58c15bee4 100644
--- a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
@@ -11,8 +11,8 @@ ra_trajectory_controller:
     - ra_wrist_3_joint
   compliance_command_timeout: 1.5
   constraints:
-      goal_time: 0.6
-      stopped_velocity_tolerance: 1.0
+      goal_time: 6.0
+      stopped_velocity_tolerance: 0.05
       ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
       ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
       ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
@@ -20,24 +20,29 @@ ra_trajectory_controller:
       ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
       ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
   stop_trajectory_duration: 0.5
-  state_publish_rate:  500
+  state_publish_rate:  250
   action_monitor_rate: 10
   allow_partial_joints_goal: true
   #!!These values have not been optimized!!
   gains:
-    ra_shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
-    ra_wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+    p: &p_gain 20.0
+    i: &i_gain 0.0
+    d: &d_gain 0.0
+    clamp: &i_clamp 1.0
+    ra_shoulder_pan_joint:  {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_shoulder_lift_joint: {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_elbow_joint:         {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_1_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_2_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_3_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
 
   # Use a feedforward term to reduce the size of PID gains
   velocity_ff:
-    var: &v_ff 0.8
+    var: &v_ff 0.0
     ra_shoulder_pan_joint: *v_ff
     ra_shoulder_lift_joint: *v_ff
     ra_elbow_joint: *v_ff
     ra_wrist_1_joint: *v_ff
     ra_wrist_2_joint: *v_ff
     ra_wrist_3_joint: *v_ff
+
diff --git a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
index bcb5b8620..7f4decd9f 100644
--- a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
@@ -1,12 +1,11 @@
 ra_sr_ur_robot_hw:
-  hardware_interface:
-    joints: &robot_joints
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint
+  joints: &robot_joints
+    - ra_shoulder_pan_joint
+    - ra_shoulder_lift_joint
+    - ra_elbow_joint
+    - ra_wrist_1_joint
+    - ra_wrist_2_joint
+    - ra_wrist_3_joint
 
   type: ur_driver/HardwareInterface
   
@@ -41,6 +40,8 @@ ra_sr_ur_robot_hw:
 
   use_local_hardware_interface_param: true
   
-  non_blocking_read: true
+  non_blocking_read: false
 
   wrench_prefix: right_
+
+  tf_prefix: ra_

From 965884a6f1aff563449f8ed0cce7445e0bdeb29d Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Wed, 25 Nov 2020 09:21:10 +0000
Subject: [PATCH 09/28] remove box

---
 .../config/ra_ur10e_with_box.urdf.xacro                   | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
index b0a136f1b..f83481e46 100644
--- a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
+++ b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
@@ -23,13 +23,13 @@
   </joint>
 
   <!-- Box representation of hand -->
-  <xacro:include filename="$(find sr_box_ur10_moveit_config)/config/hand_box.xacro" />
-  <xacro:hand_box name="hand_box" width="0.22" depth="0.17" height="0.24"/>
+  <!--xacro:include filename="$(find sr_box_ur10_moveit_config)/config/hand_box.xacro" />
+  <xacro:hand_box name="hand_box" width="0.22" depth="0.17" height="0.24"/-->
 
   <!-- Join arm and hand -->
-  <joint name="ra_arm_to_hand" type="fixed">
+  <!--joint name="ra_arm_to_hand" type="fixed">
     <parent link="ra_ee_link"/>
     <child link="hand_base_attach"/>
     <origin rpy="1.570796325 3.14159265 1.570796325" xyz="0.012 0.0 0.0"/>
-  </joint>
+  </joint-->
 </robot>

From 554958cf46d2342ef7a2f4824ec7f776a8039a37 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Wed, 25 Nov 2020 09:22:40 +0000
Subject: [PATCH 10/28] mass and kinematics

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 4fc3820f0..19a431aea 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -53,10 +53,11 @@
   <arg name="arm_speed_scale" default="0.5"/>
   <arg if="$(arg hand_ctrl)" name="arm_payload_mass" default="4.55"/>
   <arg if="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
-  <arg unless="$(arg hand_ctrl)" name="arm_payload_mass" default="0.012"/>
+  <arg unless="$(arg hand_ctrl)" name="arm_payload_mass" default="0.0"/>
   <arg unless="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
-  <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
-  <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
+  <!--arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/-->
+  <!--arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/-->
+  <arg name="kinematics_config" value="/home/user/calib.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
   <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
 

From 29f71164c10f92942fa10f8f9289f7b879082f08 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Tue, 15 Dec 2020 14:29:30 +0000
Subject: [PATCH 11/28] add transmission interface to launch files for hw

---
 .../launch/sr_bimanual_hardware_control_loop.launch    |  5 ++++-
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 10 ++++++----
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
index eba1c15e8..f4be1440f 100644
--- a/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_bimanual_hardware_control_loop.launch
@@ -48,6 +48,9 @@
   <!-- This will include the wrist joints in the hand controller and exclude them from the arm one -->
   <arg name="include_wrist_in_arm_controller" default="true" if="$(arg arm_ctrl)"/>
   <arg name="include_wrist_in_arm_controller" default="false" unless="$(arg arm_ctrl)"/>
+  <arg name="arm_compliance_control" default="false"/>
+  <arg if="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/VelocityJointInterface"/>
+  <arg unless="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/PositionJointInterface"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
 
   <arg name="arm_trajectory" default="true"/>
@@ -66,7 +69,7 @@
   <arg name="kinematics_config_right" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find ur_description)/config/ur10_default.yaml"/>
   <arg name="kinematics_config_right" if="$(eval arg('robot_model') == 'ur10e')" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
 
-  <arg name="load_robot_description_command" default="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config_right:=$(arg kinematics_config_right) kinematics_config_left:=$(arg kinematics_config_left)"/>
+  <arg name="load_robot_description_command" default="xacro $(arg robot_description) arm_1_z:=$(arg arm_1_z) arm_2_z:=$(arg arm_2_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config_right:=$(arg kinematics_config_right) kinematics_config_left:=$(arg kinematics_config_left) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_right_speed_scale" args="call --wait /ra_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg right_arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_left_speed_scale" args="call --wait /la_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg left_arm_speed_scale)'"/>
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 19a431aea..00869d4a1 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -39,6 +39,7 @@
   <arg name="hand_trajectory" default="true"/>
 
   <!-- ARMS CONFIG-->
+  <arg name="arm_compliance_control" default="false"/>
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
   <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
   <arg name="arm_x_separation" default="0.0"/>
@@ -55,9 +56,10 @@
   <arg if="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.12]"/>
   <arg unless="$(arg hand_ctrl)" name="arm_payload_mass" default="0.0"/>
   <arg unless="$(arg hand_ctrl)" name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
-  <!--arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/-->
-  <!--arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/-->
-  <arg name="kinematics_config" value="/home/user/calib.yaml"/>
+  <arg if="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/VelocityJointInterface"/>
+  <arg unless="$(arg arm_compliance_control)" name="arm_transmission_interface" default="hardware_interface/PositionJointInterface"/>
+  <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
+  <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
   <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
 
@@ -150,7 +152,7 @@
     </include>
     <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
     <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
-    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config)"/>
+    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arm_transmission_interface)"/>
   </group>
   <!-- These will be loaded if arm and hand are enabled -->
   <!-- Trajectory mode -->

From ec2c54c9385a9e25f05ff652787d3e4e9383ffce Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Wed, 16 Dec 2020 13:22:42 +0000
Subject: [PATCH 12/28] fix arg bug

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 00869d4a1..122a8df05 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -61,7 +61,7 @@
   <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
-  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config)"/>
+  <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_speed_scale" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg arm_speed_scale)'"/>
   <node pkg="rosservice" type="rosservice" name="set_payload" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_payload '{payload: $(arg arm_payload_mass), center_of_gravity: $(arg arm_payload_cog)}'"/>
@@ -152,7 +152,7 @@
     </include>
     <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
     <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
-    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arm_transmission_interface)"/>
+    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
   </group>
   <!-- These will be loaded if arm and hand are enabled -->
   <!-- Trajectory mode -->

From 3722df86b27b6723703d0e78b96f42e49cf82074 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 13:35:35 +0000
Subject: [PATCH 13/28] trying to load both controllers

---
 ...> ra_compliant_trajectory_controller.yaml} |  2 +-
 .../config/ra_trajectory_controller.yaml      | 22 +++++++++++++++++++
 .../launch/sr_hardware_control_loop.launch    | 14 +++++-------
 3 files changed, 29 insertions(+), 9 deletions(-)
 rename sr_robot_launch/config/{ra_trajectory_controller_no_wrist.yaml => ra_compliant_trajectory_controller.yaml} (98%)
 create mode 100644 sr_robot_launch/config/ra_trajectory_controller.yaml

diff --git a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml b/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
similarity index 98%
rename from sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
rename to sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
index 58c15bee4..1b2bb5621 100644
--- a/sr_robot_launch/config/ra_trajectory_controller_no_wrist.yaml
+++ b/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
@@ -1,6 +1,6 @@
 # type: "velocity_controllers/ComplianceController"
 #   type: "velocity_controllers/JointTrajectoryController"
-ra_trajectory_controller:
+ra_compliant_trajectory_controller:
   type: "velocity_controllers/ComplianceController"
   joints:
     - ra_shoulder_pan_joint
diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
new file mode 100644
index 000000000..8160c541b
--- /dev/null
+++ b/sr_robot_launch/config/ra_trajectory_controller.yaml
@@ -0,0 +1,22 @@
+ra_trajectory_controller:
+  type: "position_controllers/JointTrajectoryController"
+  hardware_interface:
+    joints: &robot_joints
+      - ra_shoulder_pan_joint
+      - ra_shoulder_lift_joint
+      - ra_elbow_joint
+      - ra_wrist_1_joint
+      - ra_wrist_2_joint
+      - ra_wrist_3_joint
+  joints: *robot_joints
+  constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
+      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
+      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  allow_partial_joints_goal: true
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 122a8df05..7426c6e6e 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -40,6 +40,8 @@
 
   <!-- ARMS CONFIG-->
   <arg name="arm_compliance_control" default="false"/>
+  <arg if="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_compliant_trajectory_controller"/>
+  <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_trajectory_controller"/>
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
   <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
   <arg name="arm_x_separation" default="0.0"/>
@@ -47,7 +49,6 @@
   <arg name="arm_robot_hw" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur_arm_robot_hw.yaml"/>
   <arg name="arm_robot_hw" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur10e_arm_robot_hw.yaml"/>
   <arg name="force_torque_sensor_hw" default="$(find sr_robot_launch)/config/$(arg arm_prefix)_force_torque_controller.yaml"/>
-  <arg name="include_wrist_in_arm_controller" default="$(arg arm_ctrl)"/>
   <arg name="arm_trajectory" default="true"/>
   <!-- Set to true to spawn the position controllers for the arm-->
   <arg name="arm_position" default="$(eval not arm_trajectory)"/>
@@ -110,7 +111,6 @@
     </include>
     <node if="$(arg hand_ctrl)" name="$(arg hand_id)_controller_spawner" pkg="sr_utilities" type="controller_spawner.py" output="screen">
       <param name="controller_group" value="$(arg hand_controller_group)"/>
-      <param name="exclude_wrist" value="$(arg include_wrist_in_arm_controller)"/>
       <param name="wait_for" value ="calibrated"/>
     </node>
   </group>
@@ -159,18 +159,16 @@
   <group if="$(arg arm_ctrl)">
     <group if="$(arg arm_trajectory)">
       <group if="$(arg hand_ctrl)">
-        <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
-        <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller_no_wrist.yaml"/>
+        <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
       </group>
-      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller_no_wrist.yaml"/>
+      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
       <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg trajectory_controller_type)"/>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">
       <group if="$(arg hand_ctrl)">
-        <rosparam if="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller.yaml"/>
-        <rosparam unless="$(arg include_wrist_in_arm_controller)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller_no_wrist.yaml"/>
+        <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller.yaml"/>
       </group>
       <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_group_position_controller_no_wrist.yaml"/>
       <node if="$(arg hand_ctrl)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen"

From a7ef978b8e95f8f5a72cd507029eaf3cc709c204 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 13:37:26 +0000
Subject: [PATCH 14/28] test flag

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 7426c6e6e..f79b6db9f 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -39,7 +39,7 @@
   <arg name="hand_trajectory" default="true"/>
 
   <!-- ARMS CONFIG-->
-  <arg name="arm_compliance_control" default="false"/>
+  <arg name="arm_compliance_control" default="true"/>
   <arg if="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_compliant_trajectory_controller"/>
   <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_trajectory_controller"/>
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>

From b3f1474bcf7e7a897c346e8adc03bf41201a8eff Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 14:41:37 +0000
Subject: [PATCH 15/28] load both

---
 .../config/ra_trajectory_controller.yaml      | 47 +++++++++++++++++++
 .../launch/sr_hardware_control_loop.launch    |  6 ++-
 2 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
index 8160c541b..3fbfd6b93 100644
--- a/sr_robot_launch/config/ra_trajectory_controller.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller.yaml
@@ -20,3 +20,50 @@ ra_trajectory_controller:
       ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
   stop_trajectory_duration: 0.5
   allow_partial_joints_goal: true
+
+ra_compliant_trajectory_controller:
+  type: "velocity_controllers/ComplianceController"
+  joints:
+    - ra_shoulder_pan_joint
+    - ra_shoulder_lift_joint
+    - ra_elbow_joint
+    - ra_wrist_1_joint
+    - ra_wrist_2_joint
+    - ra_wrist_3_joint
+  compliance_command_timeout: 1.5
+  constraints:
+      goal_time: 6.0
+      stopped_velocity_tolerance: 0.05
+      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
+      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
+      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
+      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
+  stop_trajectory_duration: 0.5
+  state_publish_rate:  250
+  action_monitor_rate: 10
+  allow_partial_joints_goal: true
+  #!!These values have not been optimized!!
+  gains:
+    p: &p_gain 20.0
+    i: &i_gain 0.0
+    d: &d_gain 0.0
+    clamp: &i_clamp 1.0
+    ra_shoulder_pan_joint:  {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_shoulder_lift_joint: {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_elbow_joint:         {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_1_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_2_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+    ra_wrist_3_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
+
+  # Use a feedforward term to reduce the size of PID gains
+  velocity_ff:
+    var: &v_ff 0.0
+    ra_shoulder_pan_joint: *v_ff
+    ra_shoulder_lift_joint: *v_ff
+    ra_elbow_joint: *v_ff
+    ra_wrist_1_joint: *v_ff
+    ra_wrist_2_joint: *v_ff
+    ra_wrist_3_joint: *v_ff
+
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index f79b6db9f..61358f2d4 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -161,9 +161,11 @@
       <group if="$(arg hand_ctrl)">
         <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
       </group>
-      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
+      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
       <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg trajectory_controller_type)"/>
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
+      <node name="compliant_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_compliant_trajectory_controller"/>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">

From 4bac55a1796f3f108f0a7ace8e458ea38d1e43b6 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 14:41:54 +0000
Subject: [PATCH 16/28] load both

---
 .../config/ra_controllers.yaml                        | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
index 9ce587b34..608ed94f3 100644
--- a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
+++ b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
@@ -10,3 +10,14 @@
       - ra_wrist_1_joint
       - ra_wrist_2_joint
       - ra_wrist_3_joint
+  - name: ra_compliant_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - ra_shoulder_pan_joint
+      - ra_shoulder_lift_joint
+      - ra_elbow_joint
+      - ra_wrist_1_joint
+      - ra_wrist_2_joint
+      - ra_wrist_3_joint

From 5f1ded8fd40569eff52cb0fea9c5808474f7f037 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 14:50:25 +0000
Subject: [PATCH 17/28] both controllers

---
 .../config/ra_trajectory_controller.yaml      | 25 ++++++++-----------
 .../launch/sr_hardware_control_loop.launch    |  4 +--
 2 files changed, 12 insertions(+), 17 deletions(-)

diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
index 3fbfd6b93..3b72ecc3e 100644
--- a/sr_robot_launch/config/ra_trajectory_controller.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller.yaml
@@ -1,13 +1,16 @@
+
+joints: &robot_joints
+  - ra_shoulder_pan_joint
+  - ra_shoulder_lift_joint
+  - ra_elbow_joint
+  - ra_wrist_1_joint
+  - ra_wrist_2_joint
+  - ra_wrist_3_joint
+
 ra_trajectory_controller:
   type: "position_controllers/JointTrajectoryController"
   hardware_interface:
-    joints: &robot_joints
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint
+    joints: *robot_joints
   joints: *robot_joints
   constraints:
       goal_time: 0.6
@@ -23,13 +26,7 @@ ra_trajectory_controller:
 
 ra_compliant_trajectory_controller:
   type: "velocity_controllers/ComplianceController"
-  joints:
-    - ra_shoulder_pan_joint
-    - ra_shoulder_lift_joint
-    - ra_elbow_joint
-    - ra_wrist_1_joint
-    - ra_wrist_2_joint
-    - ra_wrist_3_joint
+  joints: *robot_joints
   compliance_command_timeout: 1.5
   constraints:
       goal_time: 6.0
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 61358f2d4..4a7c50492 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -163,9 +163,7 @@
       </group>
       <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
       <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
-      <node name="compliant_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_compliant_trajectory_controller"/>
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller --stopped $(arg arm_prefix)_compliant_trajectory_controller"/>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">

From 56c7b4289d8cb5e5cf7fb7eee4adaf635a7e7b8a Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 14:55:50 +0000
Subject: [PATCH 18/28] both

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 4a7c50492..0275a5d18 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -163,7 +163,9 @@
       </group>
       <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
       <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller --stopped $(arg arm_prefix)_compliant_trajectory_controller"/>
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
+      <node name="compliant_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running --stopped $(arg arm_prefix)_compliant_trajectory_controller"/>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">

From 464195511d7ce50de8e8be22ba5183be9d4ddbd4 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 16:23:32 +0000
Subject: [PATCH 19/28] test

---
 .../ra_compliant_trajectory_controller.yaml   | 21 +++++-----
 .../config/ra_trajectory_controller.yaml      | 40 -------------------
 .../launch/sr_hardware_control_loop.launch    | 13 +++---
 3 files changed, 17 insertions(+), 57 deletions(-)

diff --git a/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml b/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
index 1b2bb5621..741acc6b9 100644
--- a/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
+++ b/sr_robot_launch/config/ra_compliant_trajectory_controller.yaml
@@ -1,14 +1,15 @@
-# type: "velocity_controllers/ComplianceController"
-#   type: "velocity_controllers/JointTrajectoryController"
-ra_compliant_trajectory_controller:
+
+joints: &robot_joints
+  - ra_shoulder_pan_joint
+  - ra_shoulder_lift_joint
+  - ra_elbow_joint
+  - ra_wrist_1_joint
+  - ra_wrist_2_joint
+  - ra_wrist_3_joint
+
+ra_trajectory_controller:
   type: "velocity_controllers/ComplianceController"
-  joints:
-    - ra_shoulder_pan_joint
-    - ra_shoulder_lift_joint
-    - ra_elbow_joint
-    - ra_wrist_1_joint
-    - ra_wrist_2_joint
-    - ra_wrist_3_joint
+  joints: *robot_joints
   compliance_command_timeout: 1.5
   constraints:
       goal_time: 6.0
diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
index 3b72ecc3e..f6c6d0833 100644
--- a/sr_robot_launch/config/ra_trajectory_controller.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller.yaml
@@ -24,43 +24,3 @@ ra_trajectory_controller:
   stop_trajectory_duration: 0.5
   allow_partial_joints_goal: true
 
-ra_compliant_trajectory_controller:
-  type: "velocity_controllers/ComplianceController"
-  joints: *robot_joints
-  compliance_command_timeout: 1.5
-  constraints:
-      goal_time: 6.0
-      stopped_velocity_tolerance: 0.05
-      ra_shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
-      ra_shoulder_lift_joint: {trajectory: 0.3, goal: 0.1}
-      ra_elbow_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_1_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_2_joint: {trajectory: 0.3, goal: 0.1}
-      ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
-  stop_trajectory_duration: 0.5
-  state_publish_rate:  250
-  action_monitor_rate: 10
-  allow_partial_joints_goal: true
-  #!!These values have not been optimized!!
-  gains:
-    p: &p_gain 20.0
-    i: &i_gain 0.0
-    d: &d_gain 0.0
-    clamp: &i_clamp 1.0
-    ra_shoulder_pan_joint:  {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-    ra_shoulder_lift_joint: {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-    ra_elbow_joint:         {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-    ra_wrist_1_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-    ra_wrist_2_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-    ra_wrist_3_joint:       {p: *p_gain,  i: *i_gain, d: *d_gain, i_clamp: *i_clamp}
-
-  # Use a feedforward term to reduce the size of PID gains
-  velocity_ff:
-    var: &v_ff 0.0
-    ra_shoulder_pan_joint: *v_ff
-    ra_shoulder_lift_joint: *v_ff
-    ra_elbow_joint: *v_ff
-    ra_wrist_1_joint: *v_ff
-    ra_wrist_2_joint: *v_ff
-    ra_wrist_3_joint: *v_ff
-
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 0275a5d18..cd9f1e3e7 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -40,8 +40,6 @@
 
   <!-- ARMS CONFIG-->
   <arg name="arm_compliance_control" default="true"/>
-  <arg if="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_compliant_trajectory_controller"/>
-  <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_type" default="$(arg arm_prefix)_trajectory_controller"/>
   <arg name="initial_z" if="$(arg arm)" default="0.7551"/>
   <arg name="initial_z" unless="$(arg arm)" default="0.0"/>
   <arg name="arm_x_separation" default="0.0"/>
@@ -161,11 +159,12 @@
       <group if="$(arg hand_ctrl)">
         <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
       </group>
-      <rosparam unless="$(arg hand_ctrl)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
-      <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
-      <node name="compliant_arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running --stopped $(arg arm_prefix)_compliant_trajectory_controller"/>
+      <group unless="$(arg hand_ctrl)"/>
+        <rosparam unless="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
+        <rosparam if="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
+        <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+              args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
+      </group>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">

From fa3b9cec268ddc35c42c9fbdf6ec7570805d487a Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 17 Dec 2020 16:39:52 +0000
Subject: [PATCH 20/28] start wrench_to_joint_vel_pub when using compliance
 controller

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index cd9f1e3e7..d614240e1 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -142,6 +142,8 @@
     robot_hardware:
       - unique_robot_hw
   </rosparam>
+  
+  <include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch">
 
   <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
   <group unless="$(arg hand)">
@@ -159,7 +161,7 @@
       <group if="$(arg hand_ctrl)">
         <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
       </group>
-      <group unless="$(arg hand_ctrl)"/>
+      <group unless="$(arg hand_ctrl)">
         <rosparam unless="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
         <rosparam if="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
         <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"

From 36914b7778ff1c75f386b00d412b7236e581d93d Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 11:58:51 +0000
Subject: [PATCH 21/28] bug

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index d614240e1..89b45b0da 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -143,7 +143,7 @@
       - unique_robot_hw
   </rosparam>
   
-  <include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch">
+  <include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/>
 
   <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
   <group unless="$(arg hand)">

From 176feb9b3b1aaa86b21547dca660be00f279453e Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 12:03:25 +0000
Subject: [PATCH 22/28] condition around compliance

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 89b45b0da..60c87ce28 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -143,7 +143,13 @@
       - unique_robot_hw
   </rosparam>
   
-  <include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/>
+  <!--include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/-->
+  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
+    <param name="package_name" value="wrench_to_joint_vel_pub" />
+    <param name="executable_name" value="ur_compliance.launch" />
+    <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
+    <param name="timeout" value="30.0" />
+  </node>
 
   <!-- These will be loaded if hand is false so UR10 with box will load instead. -->
   <group unless="$(arg hand)">

From 54b4b795de52fbfcd4436d749cbf4780734b4681 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 12:50:11 +0000
Subject: [PATCH 23/28] remove old controller

---
 .../config/ra_controllers.yaml                        | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
index 608ed94f3..9ce587b34 100644
--- a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
+++ b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_controllers.yaml
@@ -10,14 +10,3 @@
       - ra_wrist_1_joint
       - ra_wrist_2_joint
       - ra_wrist_3_joint
-  - name: ra_compliant_trajectory_controller
-    action_ns: follow_joint_trajectory
-    type: FollowJointTrajectory
-    default: true
-    joints:
-      - ra_shoulder_pan_joint
-      - ra_shoulder_lift_joint
-      - ra_elbow_joint
-      - ra_wrist_1_joint
-      - ra_wrist_2_joint
-      - ra_wrist_3_joint

From 4e2f0761014bf804fdab86545a218cee581a5fd6 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 12:51:31 +0000
Subject: [PATCH 24/28] test

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index 60c87ce28..2007623b5 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -144,7 +144,7 @@
   </rosparam>
   
   <!--include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/-->
-  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
+  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool_2.py" output="screen">
     <param name="package_name" value="wrench_to_joint_vel_pub" />
     <param name="executable_name" value="ur_compliance.launch" />
     <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>

From 0c3007a21571e75045d84129427ee16a8fe8fa25 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 15:04:09 +0000
Subject: [PATCH 25/28] resetting xacros

---
 sr_multi_description/urdf/right_srhand_ur10.urdf.xacro  | 2 +-
 sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro b/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
index 678718125..4a9497e22 100644
--- a/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
+++ b/sr_multi_description/urdf/right_srhand_ur10.urdf.xacro
@@ -12,7 +12,7 @@
     </joint>
 
     <xacro:arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
-    <xacro:arg name="transmission_hw_interface" default="hardware_interface/VelocityJointInterface"/>
+    <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
 
     <xacro:ur10_robot prefix="ra_" joint_limited="false" transmission_hw_interface="$(arg transmission_hw_interface)" kinematics_file="${load_yaml('$(arg kinematics_config)')}"/>
 
diff --git a/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro b/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
index a2d6d96f3..3286679fc 100644
--- a/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
+++ b/sr_multi_description/urdf/right_srhand_ur10e.urdf.xacro
@@ -3,7 +3,7 @@
     <!--<xacro:include filename="$(find ur_description)/urdf/gazebo.urdf.xacro" />-->
     <xacro:include filename="$(find ur_e_description)/urdf/ur10e.urdf.xacro"/>
     <xacro:arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_default.yaml"/>
-    <xacro:arg name="transmission_hw_interface" default="hardware_interface/VelocityJointInterface"/>
+    <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
 
     <link name="world"/>
     <xacro:arg name="initial_z" default="0.0"/>

From 63263064d2fe3267e90f5c874d56c4a2bee077d4 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Fri, 18 Dec 2020 15:48:07 +0000
Subject: [PATCH 26/28] fix

---
 sr_robot_launch/launch/sr_hardware_control_loop.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index f67f71fdb..bd1abfce8 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -158,7 +158,7 @@
   </rosparam>
   
   <!--include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/-->
-  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool_2.py" output="screen">
+  <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
     <param name="package_name" value="wrench_to_joint_vel_pub" />
     <param name="executable_name" value="ur_compliance.launch" />
     <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>

From f0efa259a158053fa70de1e38377803ecdcc3eb4 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 7 Jan 2021 16:19:30 +0000
Subject: [PATCH 27/28] tidying

---
 .../config/ra_ur10e_with_box.urdf.xacro       |   8 +-
 .../config/right_ur10e_arm_robot_hw.yaml      |   5 +-
 .../config/right_ur_arm_robot_hw.yaml         |   4 +-
 .../launch/sr_hardware_control_loop.launch    |  21 +--
 sr_robot_launch/launch/sr_ur10arm_box.launch  | 160 ------------------
 5 files changed, 15 insertions(+), 183 deletions(-)
 delete mode 100644 sr_robot_launch/launch/sr_ur10arm_box.launch

diff --git a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
index f83481e46..b0a136f1b 100644
--- a/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
+++ b/sr_multi_moveit/sr_box_ur10_moveit_config/config/ra_ur10e_with_box.urdf.xacro
@@ -23,13 +23,13 @@
   </joint>
 
   <!-- Box representation of hand -->
-  <!--xacro:include filename="$(find sr_box_ur10_moveit_config)/config/hand_box.xacro" />
-  <xacro:hand_box name="hand_box" width="0.22" depth="0.17" height="0.24"/-->
+  <xacro:include filename="$(find sr_box_ur10_moveit_config)/config/hand_box.xacro" />
+  <xacro:hand_box name="hand_box" width="0.22" depth="0.17" height="0.24"/>
 
   <!-- Join arm and hand -->
-  <!--joint name="ra_arm_to_hand" type="fixed">
+  <joint name="ra_arm_to_hand" type="fixed">
     <parent link="ra_ee_link"/>
     <child link="hand_base_attach"/>
     <origin rpy="1.570796325 3.14159265 1.570796325" xyz="0.012 0.0 0.0"/>
-  </joint-->
+  </joint>
 </robot>
diff --git a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
index 73ede01f7..00ef54e7d 100644
--- a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
@@ -14,11 +14,11 @@ ra_sr_ur_robot_hw:
   
   # for sending robot programs
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  robot_ip: 192.168.1.1
+  robot_ip: 10.8.1.1
   
   # for the server that moves the robot
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  control_pc_ip_address: 192.168.1.100
+  control_pc_ip_address: 10.8.1.100
   
   input_recipe_file: /home/user/projects/shadow_robot/base/src/Universal_Robots_ROS_Driver/ur_robot_driver/resources/rtde_input_recipe.txt
   
@@ -44,4 +44,3 @@ ra_sr_ur_robot_hw:
 
   wrench_prefix: right_
 
-  tf_prefix: ra_
diff --git a/sr_robot_launch/config/right_ur_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
index 246385863..dd79fe2f4 100644
--- a/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur_arm_robot_hw.yaml
@@ -14,11 +14,11 @@ ra_sr_ur_robot_hw:
   
   # for sending robot programs
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  robot_ip: 192.168.1.1
+  robot_ip: 10.8.1.1
   
   # for the server that moves the robot
   # aurora (https://github.com/shadow-robot/aurora) uses sed to replace this IP with the user-defined IP
-  control_pc_ip_address: 192.168.1.100
+  control_pc_ip_address: 10.8.1.100
   
   input_recipe_file: /home/user/projects/shadow_robot/base/src/Universal_Robots_ROS_Driver/ur_robot_driver/resources/rtde_input_recipe.txt
   
diff --git a/sr_robot_launch/launch/sr_hardware_control_loop.launch b/sr_robot_launch/launch/sr_hardware_control_loop.launch
index bd1abfce8..54d0b1b5f 100644
--- a/sr_robot_launch/launch/sr_hardware_control_loop.launch
+++ b/sr_robot_launch/launch/sr_hardware_control_loop.launch
@@ -18,7 +18,7 @@
   <arg name="load_robot_description_at_lower_level" value="$(eval not arm)"/>
 
   <!-- Whether there is a hand. -->
-  <arg name="hand" default="false"/>
+  <arg name="hand" default="true"/>
   <!-- Whether to run arm controllers. -->
   <arg name="arm_ctrl" default="$(arg arm)"/>
   <!-- Whether to run hand controllers. -->
@@ -67,6 +67,8 @@
   <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
   <arg name="urcap_program_name" default="external_ctrl.urp"/>
+  <arg if="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
+  <arg unless="$(arg arm_compliance_control)" name="trajectory_controller_config" default="$(arg arm_prefix)_trajectory_controller.yaml"/>
   <arg name="load_robot_description_command" default="xacro $(arg robot_description) prefix:=$(arg hand_id)_ initial_z:=$(arg initial_z) arm_x_separation:=$(arg arm_x_separation) arm_y_separation:=$(arg arm_y_separation) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arg arm_transmission_interface)"/>
 
   <node pkg="rosservice" type="rosservice" name="set_speed_scale" args="call --wait /$(arg arm_prefix)_sr_ur_robot_hw/set_speed_slider 'speed_slider_fraction: $(arg arm_speed_scale)'"/>
@@ -156,8 +158,7 @@
     robot_hardware:
       - unique_robot_hw
   </rosparam>
-  
-  <!--include if="$(arg arm_compliance_control)" file="$(find wrench_to_joint_vel_pub)/launch/ur_compliance.launch"/-->
+
   <node if="$(arg arm_compliance_control)" name="conditional_delayed_rostool_wrench_to_joint_vel_pub" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
     <param name="package_name" value="wrench_to_joint_vel_pub" />
     <param name="executable_name" value="ur_compliance.launch" />
@@ -173,22 +174,14 @@
     <node if="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="gdb -ex run -args"/>
     <node unless="$(arg debug)" name="ur_arm_robot" pkg="ros_control_robot" type="ros_control_robot" args="" output="screen" launch-prefix="ethercat_grant"/>
 
-    <!--param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config) transmission_hw_interface:=$(arg arm_transmission_interface)"/-->
-
   </group>
   <!-- These will be loaded if arm and hand are enabled -->
   <!-- Trajectory mode -->
   <group if="$(arg arm_ctrl)">
     <group if="$(arg arm_trajectory)">
-      <group if="$(arg hand_ctrl)">
-        <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_type).yaml"/>
-      </group>
-      <group unless="$(arg hand_ctrl)">
-        <rosparam unless="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_trajectory_controller.yaml"/>
-        <rosparam if="$(arg arm_compliance_control)" command="load" file="$(find sr_robot_launch)/config/$(arg arm_prefix)_compliant_trajectory_controller.yaml"/>
-        <node name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
-              args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
-      </group>
+      <rosparam command="load" file="$(find sr_robot_launch)/config/$(arg trajectory_controller_config)"/>
+      <node unless="$(arg hand_ctrl)" name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen"
+            args="--wait-for=/$(arg arm_prefix)_sr_ur_robot_hw/robot_program_running $(arg arm_prefix)_trajectory_controller"/>
     </group>
     <!-- Position mode -->
     <group if="$(arg arm_position)">
diff --git a/sr_robot_launch/launch/sr_ur10arm_box.launch b/sr_robot_launch/launch/sr_ur10arm_box.launch
deleted file mode 100644
index afb6e458a..000000000
--- a/sr_robot_launch/launch/sr_ur10arm_box.launch
+++ /dev/null
@@ -1,160 +0,0 @@
-<launch>
-  <!-- This launch file is for a ur arm and box -->
-
-  <!-- set to false if you want to use real robots, true if you need the gazebo simulation -->
-  <arg name="sim" default="true"/>
-  <!-- Set to true for debugging -->
-  <arg name="debug" default="false"/>
-  <!-- Set to true to allow planning trajectories-->
-  <arg name="use_moveit" default="true"/>
-  <!--Choose planner library, options are ompl, stomp or sbpl-->
-  <arg name="planning" default="ompl"/>
-  <!-- Set to false if you want to start without guis -->
-  <arg name="gui" default="true"/>
-  <arg name="verbose" default="true"/>
-  <!-- Set whether to load a scene to restrain the movement of the robot -->
-  <arg name="scene" default="false"/>
-    <!-- Set the scene file -->
-  <arg name="scene_file" default="$(find sr_description_common)/scenes/demo_space_large.scene"/>
-  <!-- Set world value dependant on scene -->
-  <arg name="world" default="$(find sr_description_common)/worlds/shadowhands_and_arms.world" unless="$(arg scene)"/>
-  <arg name="world" default="$(find sr_description_common)/worlds/demo_space_large.world" if="$(arg scene)"/>
-  <!-- Set to true if you want the robot to start in the "home" positions -->
-  <arg name="start_home" default="false"/>
-
-  <!-- ROBOT CONFIG-->
-  <arg name="side" default="right"/>
-  <arg name="side_letter" value="$(eval side[0])"/>
-  <arg name="arm_prefix" value="$(arg side_letter)a"/>
-  <arg name="robot_model" default="ur10e"/>
-  <arg name="robot_description" default="'$(find sr_box_ur10_moveit_config)/config/$(arg arm_prefix)_$(arg robot_model)_with_box.urdf.xacro'"/>
-  <arg name="robot_config_file" default="$(find sr_box_ur10_moveit_config)/config/robot_configs/$(arg side)_$(arg robot_model).yaml"/>
-
-  <!-- Definition of "home" position, in joint-angle pairs -->
-  <arg name="home_angles" unless="$(eval side=='left')" default="-J ra_shoulder_pan_joint 1.147 -J ra_elbow_joint 1.695 -J ra_wrist_1_joint -1.395 -J ra_wrist_2_joint -1.584 -J ra_shoulder_lift_joint -1.926 -J ra_wrist_3_joint 1.830"/>
-  <arg name="home_angles" if="$(eval side=='left')" default="-J la_shoulder_pan_joint 1.147 -J la_elbow_joint -1.695 -J la_wrist_1_joint -1.75 -J la_wrist_2_joint 1.57 -J la_shoulder_lift_joint -1.22 -J la_wrist_3_joint -1.830"/>
-  <!-- Set this to true if you run the hardware loop on a remote computer -->
-  <arg name="external_control_loop" default="false"/>
-  <!-- The robot state publisher publishing frequency  -->
-  <arg name="robot_state_pub_frequency" default="250"/>
-  <!-- The joint state publisher publishing frequency  -->
-  <arg name="joint_state_pub_frequency" default="125"/>
-
-  <!-- ARM CONFIG-->
-  <arg name="initial_z" default="0.7551" if="$(arg scene)"/>
-  <arg name="initial_z" default="0.1" unless="$(arg scene)"/>
-  <arg name="kinematics_config" if="$(eval robot_model[-1] == 'e')" default="$(find ur_e_description)/config/$(arg robot_model)_default.yaml"/>
-  <arg name="kinematics_config" if="$(eval not robot_model[-1] == 'e')" default="$(find ur_description)/config/$(arg robot_model)_default.yaml"/>
-  <rosparam command="load" file="$(arg kinematics_config)" ns="$(arg arm_prefix)_sr_ur_robot_hw"/>
-
-  <!-- TODO: check if we need a different tuning for ur5/ur5e  -->
-  <arg name="arm_robot_hw" if="$(eval not arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur_arm_robot_hw.yaml"/>
-  <arg name="arm_robot_hw" if="$(eval arg('robot_model') == 'ur10e')" default="$(find sr_robot_launch)/config/$(arg side)_ur10e_arm_robot_hw.yaml"/>
-
-  <arg name="arm_speed_scale" default="0.5"/>
-  <arg name="arm_payload_mass" default="0"/>
-  <arg name="arm_payload_cog" default="[0.0, 0.0, 0.0]"/>
-
-  <!-- Set to true to spawn trajectory controllers for the arms (the trajectory controller overwrites continuously the joint position command, preventing direct control via topics-->
-  <arg name="arm_trajectory" default="true"/>
-  <!-- Set to true to spawn group position controllers for the arms -->
-  <arg name="arm_position" default="$(eval not arm_trajectory)"/>
-  <param name="description_read_repetitions" value="50"/>
-
-  <!-- ROBOT CONFIG-->
-  <arg name="robot_name" default="ursrbox"/>
-
-  <!-- Logging -->
-  <include file="$(find sr_logging_common)/launch/sr_rosbag_log.launch"/>
-
-  <!-- SIMULATED ROBOTS -->
-  <group if="$(arg sim)">
-    <arg name="paused" value="$(arg start_home)"/>
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-      <arg name="world_name" value="$(arg world)"/>
-      <arg name="gui" default="$(arg gui)"/>
-      <arg name="paused" value="$(arg paused)"/>
-      <arg name="debug" value="$(arg debug)"/>
-      <arg name="verbose" value="$(arg verbose)"/>
-    </include>
-    <param name="robot_description" command="xacro $(arg robot_description) initial_z:=$(arg initial_z) kinematics_config:=$(arg kinematics_config)"/>
-
-    <arg if="$(arg start_home)" name="spawn_model_extra_args" value="-unpause $(arg home_angles)" />
-    <arg unless="$(arg start_home)" name="spawn_model_extra_args" value="" />
-    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model $(arg robot_name) $(arg spawn_model_extra_args)" respawn="false" output="screen"/>
-
-    <!-- ARM -->
-    <rosparam if="$(arg arm_trajectory)" file="$(find sr_box_ur10_moveit_config)/config/gazebo/$(arg arm_prefix)_trajectory_controller.yaml" command="load"/>
-    <rosparam if="$(arg arm_position)" file="$(find sr_box_ur10_moveit_config)/config/gazebo/$(arg arm_prefix)_group_position_controller.yaml" command="load"/>
-    <include file="$(find ros_ethercat_model)/launch/joint_state_publisher.launch" >
-      <arg name="publish_rate" value="$(arg joint_state_pub_frequency)"/>
-    </include>
-
-    <node if="$(arg arm_trajectory)" name="arm_trajectory_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="$(arg arm_prefix)_trajectory_controller"/>
-    <node if="$(arg arm_position)" name="arm_group_position_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="$(arg arm_prefix)_group_position_controller"/>
-
-    <!-- Robot state publisher -->
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)"/>
-      <param name="tf_prefix" type="string" value=""/>
-    </node>
-
-    <!-- Set scene for robot - restrains movement -->
-    <group if="$(arg scene)">
-      <include if="$(arg use_moveit)" file="$(find gazebo2rviz)/launch/gazebo2moveit.launch">
-        <arg name="frequency" default="1" />
-      </include>
-    </group>
-  </group>
-
-  <!-- REAL ROBOTS -->
-  <group unless="$(arg sim)">
-    <group unless="$(arg external_control_loop)">
-      <include file="$(find sr_robot_launch)/launch/sr_hardware_control_loop.launch">
-        <arg name="debug" default="$(arg debug)"/>
-        <arg name="initial_z" default="0.7551" if="$(arg scene)"/>
-        <arg name="initial_z" default="0.1" unless="$(arg scene)"/>
-        <arg name="robot_description" value="$(arg robot_description)"/>
-        <arg name="side" value="$(arg side)"/>
-        <arg name="robot_model" value="$(arg robot_model)"/>
-        <arg name="arm_robot_hw" value="$(arg arm_robot_hw)"/>
-        <arg name="arm_trajectory" default="$(arg arm_trajectory)"/>
-        <arg name="arm_position" default="$(arg arm_position)"/>
-        <arg name="arm_speed_scale" value="$(arg arm_speed_scale)"/>
-        <arg name="arm_payload_mass" value="$(arg arm_payload_mass)"/>
-        <arg name="arm_payload_cog" value="$(arg arm_payload_cog)"/>
-        <arg name="hand" default="false"/>
-        <arg name="robot_state_pub_frequency" value="$(arg robot_state_pub_frequency)"/>
-        <arg name="joint_state_pub_frequency" value="$(arg joint_state_pub_frequency)"/>
-      </include>
-    </group>
-
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-      <param name="publish_frequency" type="double" value="$(arg robot_state_pub_frequency)"/>
-      <param name="tf_prefix" type="string" value="" />
-    </node>
-    <!-- Set scene for robot after delay to start after moveit - restrains movement -->
-    <group if="$(arg scene)">
-      <node name="conditional_delayed_rostool_arm_scene" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen">
-        <param name="package_name" value="sr_box_ur10_moveit_config" />
-        <param name="executable_name" value="scene.launch" />
-        <rosparam param="params_list">[/robot_description, /robot_description_semantic]</rosparam>
-        <param name="launch_args_list" value="scene_file:=$(arg scene_file)" />
-        <param name="timeout" value="20.0" />
-      </node>
-    </group>
-  </group>
-
-  <rosparam file="$(arg robot_config_file)"/>
-
-  <!-- Launch moveit configuration after delay to wait for robot description to be avalible -->
-  <group if="$(arg use_moveit)">
-    <node name="conditional_delayed_rostool_rviz_multi" pkg="sr_utilities_common" type="conditional_delayed_rostool.py" output="screen" required="true">
-      <param name="package_name" value="sr_box_ur10_moveit_config" />
-      <param name="executable_name" value="planning_and_execution.launch" />
-      <rosparam param="params_list">[/robot_description]</rosparam>
-      <param name="launch_args_list" value="side:=$(arg side) use_gui:=$(arg gui)" />
-      <param name="timeout" value="20.0" />
-    </node>
-  </group>
-</launch>

From 4c848f8dfe03005b52a4b570862bca0fb599f2a7 Mon Sep 17 00:00:00 2001
From: carebare47 <tom@shadowrobot.com>
Date: Thu, 7 Jan 2021 16:23:43 +0000
Subject: [PATCH 28/28] tidying

---
 sr_robot_launch/config/ra_trajectory_controller.yaml | 1 -
 sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml | 1 -
 2 files changed, 2 deletions(-)

diff --git a/sr_robot_launch/config/ra_trajectory_controller.yaml b/sr_robot_launch/config/ra_trajectory_controller.yaml
index f6c6d0833..3ea63e484 100644
--- a/sr_robot_launch/config/ra_trajectory_controller.yaml
+++ b/sr_robot_launch/config/ra_trajectory_controller.yaml
@@ -23,4 +23,3 @@ ra_trajectory_controller:
       ra_wrist_3_joint: {trajectory: 0.3, goal: 0.1}
   stop_trajectory_duration: 0.5
   allow_partial_joints_goal: true
-
diff --git a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
index 00ef54e7d..c1caa73b7 100644
--- a/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
+++ b/sr_robot_launch/config/right_ur10e_arm_robot_hw.yaml
@@ -43,4 +43,3 @@ ra_sr_ur_robot_hw:
   non_blocking_read: true
 
   wrench_prefix: right_
-