From 58361900b8682931f6b0ef91c109fbeb9d72a34e Mon Sep 17 00:00:00 2001
From: Jeppe Walther <jeppewalther@gmail.com>
Date: Tue, 6 Oct 2015 17:14:59 +0200
Subject: [PATCH] UR3 Support

---
 config/ur3_controllers.yaml             | 97 +++++++++++++++++++++++++
 launch/ur3_bringup.launch               | 29 ++++++++
 launch/ur3_bringup_joint_limited.launch | 22 ++++++
 launch/ur3_ros_control.launch           | 42 +++++++++++
 4 files changed, 190 insertions(+)
 create mode 100644 config/ur3_controllers.yaml
 create mode 100644 launch/ur3_bringup.launch
 create mode 100644 launch/ur3_bringup_joint_limited.launch
 create mode 100644 launch/ur3_ros_control.launch

diff --git a/config/ur3_controllers.yaml b/config/ur3_controllers.yaml
new file mode 100644
index 0000000..8e26689
--- /dev/null
+++ b/config/ur3_controllers.yaml
@@ -0,0 +1,97 @@
+# Currently simply a copy of ur5_controllers.yaml
+
+# Settings for ros_control control loop
+hardware_control_loop:
+   loop_hz: 125
+
+# Settings for ros_control hardware interface
+hardware_interface:
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+
+# Publish all joint states ----------------------------------
+joint_state_controller:
+   type:         joint_state_controller/JointStateController
+   publish_rate: 125
+
+# Publish wrench ----------------------------------
+force_torque_sensor_controller:
+   type:         force_torque_sensor_controller/ForceTorqueSensorController
+   publish_rate: 125
+
+# Joint Trajectory Controller - position based -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+position_based_position_trajectory_controller:
+   type: position_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      shoulder_lift_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      elbow_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      wrist_1_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      wrist_2_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      wrist_3_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #hold_trajectory_duration: 0 # Defaults to 0.5
+
+# Joint Trajectory Controller - velocity based -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+velocity_based_position_trajectory_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+      #!!These values have not been optimized!!
+      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #hold_trajectory_duration: 0 # Defaults to 0.5
diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch
new file mode 100644
index 0000000..1b28f72
--- /dev/null
+++ b/launch/ur3_bringup.launch
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<!--
+  Universal robot ur3 launch.  Loads ur3 robot description (see ur_common.launch
+  for more info)
+
+  Usage:
+    ur3_bringup.launch robot_ip:=<value>
+-->
+<launch>
+
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip"/>
+  <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="3.0"/>
+
+  <!-- robot model -->
+  <include file="$(find ur_description)/launch/ur3_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+  <!-- ur common -->
+  <include file="$(find ur_modern_driver)/launch/ur_common.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
+  </include>
+
+</launch>
diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch
new file mode 100644
index 0000000..6d40005
--- /dev/null
+++ b/launch/ur3_bringup_joint_limited.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<!--
+  Universal robot ur3 launch. Wraps ur3_bringup.launch. Uses the 'limited'
+  joint range [-PI, PI] on all joints.
+
+  Usage:
+    ur3_bringup_joint_limited.launch robot_ip:=<value>
+-->
+<launch>
+
+  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
+  <arg name="robot_ip"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="10.0"/>
+
+  <include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="limited"  value="true"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
+  </include>
+</launch>
diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch
new file mode 100644
index 0000000..1c1cd89
--- /dev/null
+++ b/launch/ur3_ros_control.launch
@@ -0,0 +1,42 @@
+<launch>
+
+  <!-- GDB functionality -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="robot_ip"/>
+  <arg name="limited" default="false"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="3.0"/>
+  <arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
+  <!-- robot model -->
+  <include file="$(find ur_description)/launch/ur3_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+
+  <!-- Load hardware interface -->
+  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+    <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
+    <param name="min_payload" type="double" value="$(arg min_payload)"/>
+    <param name="max_payload" type="double" value="$(arg max_payload)"/>
+    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
+    <param name="use_ros_control" type="bool" value="True"/>
+  </node>
+
+  <!-- Load controller settings -->
+  <rosparam file="$(find ur_modern_driver)/config/ur3_controllers.yaml" command="load"/>
+
+  <!-- spawn controller manager -->
+  <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
+
+  <!-- load other controller -->
+  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
+    output="screen" args="load position_based_position_trajectory_controller" />
+
+  <!-- Convert joint states to /tf tranforms -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
+
+</launch>