From 70ed994c2cf98307eeca142408669b0a07409eb4 Mon Sep 17 00:00:00 2001
From: ipa-fxm <felix.messmer@ipa.fhg.de>
Date: Sat, 20 Jun 2015 17:30:36 +0200
Subject: [PATCH] provide launch files for ur3

---
 ur_bringup/launch/ur3_bringup.launch          | 31 +++++++++++++++++++
 .../launch/ur3_bringup_joint_limited.launch   | 24 ++++++++++++++
 ur_gazebo/controller/arm_controller_ur3.yaml  | 21 +++++++++++++
 ur_gazebo/launch/ur3.launch                   | 27 ++++++++++++++++
 ur_gazebo/launch/ur3_joint_limited.launch     | 10 ++++++
 5 files changed, 113 insertions(+)
 create mode 100644 ur_bringup/launch/ur3_bringup.launch
 create mode 100644 ur_bringup/launch/ur3_bringup_joint_limited.launch
 create mode 100644 ur_gazebo/controller/arm_controller_ur3.yaml
 create mode 100644 ur_gazebo/launch/ur3.launch
 create mode 100644 ur_gazebo/launch/ur3_joint_limited.launch

diff --git a/ur_bringup/launch/ur3_bringup.launch b/ur_bringup/launch/ur3_bringup.launch
new file mode 100644
index 0000000..ecb0009
--- /dev/null
+++ b/ur_bringup/launch/ur3_bringup.launch
@@ -0,0 +1,31 @@
+<?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="reverse_port" default="50001"/>
+  <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_bringup)/launch/ur_common.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="reverse_port" value="$(arg reverse_port)"/>
+    <arg name="min_payload"  value="$(arg min_payload)"/>
+    <arg name="max_payload"  value="$(arg max_payload)"/>
+  </include>
+
+</launch>
diff --git a/ur_bringup/launch/ur3_bringup_joint_limited.launch b/ur_bringup/launch/ur3_bringup_joint_limited.launch
new file mode 100644
index 0000000..6714d34
--- /dev/null
+++ b/ur_bringup/launch/ur3_bringup_joint_limited.launch
@@ -0,0 +1,24 @@
+<?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="reverse_port" default="50001"/>
+  <arg name="min_payload"  default="0.0"/>
+  <arg name="max_payload"  default="3.0"/>
+
+  <include file="$(find ur_bringup)/launch/ur3_bringup.launch">
+    <arg name="robot_ip" value="$(arg robot_ip)"/>
+    <arg name="reverse_port" value="$(arg reverse_port)"/>
+    <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/ur_gazebo/controller/arm_controller_ur3.yaml b/ur_gazebo/controller/arm_controller_ur3.yaml
new file mode 100644
index 0000000..7ab8870
--- /dev/null
+++ b/ur_gazebo/controller/arm_controller_ur3.yaml
@@ -0,0 +1,21 @@
+arm_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:  25
+  action_monitor_rate: 10
diff --git a/ur_gazebo/launch/ur3.launch b/ur_gazebo/launch/ur3.launch
new file mode 100644
index 0000000..3f50986
--- /dev/null
+++ b/ur_gazebo/launch/ur3.launch
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="limited" default="false"/>
+  <arg name="paused" default="false"/>
+  <arg name="gui" default="true"/>
+  
+  <!-- startup simulated world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" default="worlds/empty.world"/>
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+
+  <!-- send robot urdf to param server -->
+  <include file="$(find ur_description)/launch/ur3_upload.launch">
+    <arg name="limited" value="$(arg limited)"/>
+  </include>
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
+
+  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
+
+  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur3.yaml" command="load"/>
+  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>
+
+</launch>
diff --git a/ur_gazebo/launch/ur3_joint_limited.launch b/ur_gazebo/launch/ur3_joint_limited.launch
new file mode 100644
index 0000000..2c111e8
--- /dev/null
+++ b/ur_gazebo/launch/ur3_joint_limited.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="gui" default="true"/>
+
+  <include file="$(find ur_gazebo)/launch/ur3.launch">
+    <arg name="limited" value="true"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+
+</launch>
-- 
GitLab