From d5d1e21959331c9c785c10acb34d687f40dc6b57 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Tue, 31 Dec 2019 13:32:19 +0100
Subject: [PATCH] Copied urdf from open_manipulator_description

---
 launch/open_manipulator.launch                |   4 +-
 urdf/open_manipulator.urdf.xacro              | 359 ++++++++++++++++++
 urdf/wild_thumper_with_manipulator.urdf.xacro |  12 +
 3 files changed, 374 insertions(+), 1 deletion(-)
 create mode 100644 urdf/open_manipulator.urdf.xacro
 create mode 100644 urdf/wild_thumper_with_manipulator.urdf.xacro

diff --git a/launch/open_manipulator.launch b/launch/open_manipulator.launch
index dcb2e7b..f6ccaf5 100644
--- a/launch/open_manipulator.launch
+++ b/launch/open_manipulator.launch
@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 <launch>
+	<param name="robot_description" command="$(find xacro)/xacro.py $(find wt_open_manipulator)/urdf/wild_thumper_with_manipulator.urdf.xacro"/>
+
 	<!-- Load controller configurations from YAML file to parameter server -->
 	<rosparam file="$(find wt_open_manipulator)/config/control.yaml" command="load" />
 
 	<node pkg="wt_open_manipulator" type="wt_open_manipulator_node" name="wt_open_manipulator" output="screen" required="true"/>
 
 	<!-- load the controllers -->
-	<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="arm_joint_state_controller arm_controller"/>
+	<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="arm_joint_state_controller arm_controller gripper_controller"/>
 </launch>
diff --git a/urdf/open_manipulator.urdf.xacro b/urdf/open_manipulator.urdf.xacro
new file mode 100644
index 0000000..f1a5529
--- /dev/null
+++ b/urdf/open_manipulator.urdf.xacro
@@ -0,0 +1,359 @@
+<?xml version="1.0"?>
+<!-- Open_Manipulator Chain -->
+<robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
+
+	<!-- Import all Gazebo-customization elements, including Gazebo colors -->
+	<xacro:include filename="$(find open_manipulator_description)/urdf/open_manipulator.gazebo.xacro" />
+	<!-- Import Rviz colors -->
+	<xacro:include filename="$(find open_manipulator_description)/urdf/materials.xacro" />
+
+	<!-- Transmission macro -->
+	<xacro:macro name="SimpleTransmission" params="joint n">
+		<transmission name="tran${n}">
+			<type>transmission_interface/SimpleTransmission</type>
+			<joint name="${joint}">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+			</joint>
+			<actuator name="motor${n}">
+				<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+				<mechanicalReduction>1</mechanicalReduction>
+			</actuator>
+		</transmission>
+	</xacro:macro>
+
+	<!-- Link 1 -->
+	<link name="link1">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<material name="grey"/>
+		</visual>
+
+		<collision>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
+			</geometry>
+		</collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.082" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+	       iyy="0.1" iyz="0.0"
+	       izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+	     <origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04" />
+	     <mass value="7.9119962e-02" />
+	     <inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07"
+		     iyy="2.1898364e-05" iyz="0.0"
+		     izz="1.9267361e-05" />
+     </inertial>
+  </link>
+
+  <!-- Joint 1 -->
+  <joint name="joint1" type="revolute">
+	  <parent link="link1"/>
+	  <child link="link2"/>
+	  <origin xyz="0.012 0.0 0.017" rpy="0 0 0"/>
+	  <axis xyz="0 0 1"/>
+	  <limit velocity="4.8" effort="1" lower="${-pi}" upper="${pi}" />
+  </joint>
+
+  <!-- Transmission 1 -->
+  <xacro:SimpleTransmission n="1" joint="joint1" />
+
+  <!--  Link 2 -->
+  <link name="link2">
+	  <visual>
+		  <origin xyz="0 0 0.019" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0 0 0.019" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.098" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+	       iyy="0.1" iyz="0.0"
+	       izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+	     <origin xyz="-3.0184870e-04 5.4043684e-04 ${0.018 + 2.9433464e-02}" />
+	     <mass value="9.8406837e-02" />
+	     <inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07"
+		     iyy="3.2689329e-05" iyz="2.8511935e-08"
+		     izz="1.8850320e-05" />
+     </inertial>
+  </link>
+
+  <!--  Joint 2 -->
+  <joint name="joint2" type="revolute">
+	  <parent link="link2"/>
+	  <child link="link3"/>
+	  <origin xyz="0.0 0.0 0.0595" rpy="0 0 0"/>
+	  <axis xyz="0 1 0"/>
+	  <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.64}" />
+  </joint>
+
+  <!-- Transmission 2 -->
+  <xacro:SimpleTransmission n="2" joint="joint2" />
+
+  <!--  Link 3 -->
+  <link name="link3">
+	  <visual>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.136" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+	       iyy="0.1" iyz="0.0"
+	       izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+	     <origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01" />
+	     <mass value="1.3850917e-01" />
+	     <inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05"
+		     iyy="3.4290447e-04" iyz="-1.5717516e-06"
+		     izz="6.0346498e-05" />
+     </inertial>
+  </link>
+
+  <!--  Joint 3 -->
+  <joint name="joint3" type="revolute">
+	  <parent link="link3"/>
+	  <child link="link4"/>
+	  <origin xyz="0.024 0 0.128" rpy="0 0 0"/>
+	  <axis xyz="0 1 0"/>
+	  <limit velocity="4.8" effort="1" lower="${-pi*0.64}" upper="${pi*0.48}" />
+  </joint>
+
+  <!-- Transmission 3 -->
+  <xacro:SimpleTransmission n="3" joint="joint3" />
+
+  <!--  Link 4 -->
+  <link name="link4">
+	  <visual>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.131" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+	       iyy="0.1" iyz="0.0"
+	       izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+	     <origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04" />
+	     <mass value="1.3274562e-01" />
+	     <inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07"
+		     iyy="2.4230292e-04" iyz="1.1559550e-08"
+		     izz="2.5155057e-04" />
+     </inertial>
+  </link>
+
+  <!--  Joint 4 -->
+  <joint name="joint4" type="revolute">
+	  <parent link="link4"/>
+	  <child link="link5"/>
+	  <origin xyz="0.124 0.0 0.0" rpy="0 0 0"/>
+	  <axis xyz="0 1 0"/>
+	  <limit velocity="4.8" effort="1" lower="${-pi*0.57}" upper="${pi*0.65}" />
+  </joint>
+
+  <!-- Transmission 4 -->
+  <xacro:SimpleTransmission n="4" joint="joint4" />
+
+  <!--  Link 5 -->
+  <link name="link5">
+	  <visual>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+<!--    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="0.141" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
+	       iyy="0.1" iyz="0.0"
+	       izz="0.1" />
+    </inertial>-->
+
+     <inertial>
+	     <origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03" />
+	     <mass value="1.4327573e-01" />
+	     <inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06"
+		     iyy="7.5980465e-05" iyz="0.0"
+		     izz="9.3127351e-05" />
+     </inertial>
+  </link>
+
+  <!--  Gripper link -->
+  <link name="gripper_link">
+	  <visual>
+		  <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0.0 0.0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+	  <inertial>
+		  <origin xyz="0 0 0" />
+		  <mass value="0.017" />
+		  <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
+			  iyy="1.0e-03" iyz="0.0"
+			  izz="1.0e-03" />
+	  </inertial>
+
+<!--     <inertial>
+      <origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
+      <mass value="3.2218127e-02" />
+      <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+	       iyy="2.2552871e-05" iyz="-3.1463634e-10"
+	       izz="1.7605306e-05" />
+    </inertial>-->
+  </link>
+
+  <!--  Gripper joint -->
+  <joint name="gripper" type="prismatic">
+	  <parent link="link5"/>
+	  <child link="gripper_link"/>
+	  <origin xyz="0.0817 0.021 0.0" rpy="0 0 0"/>
+	  <axis xyz="0 1 0"/>
+	  <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+  </joint>
+
+  <!-- Transmission 5 -->
+  <xacro:SimpleTransmission n="5" joint="gripper" />
+
+  <!--  Gripper link sub -->
+  <link name="gripper_link_sub">
+	  <visual>
+		  <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+		  <material name="grey"/>
+	  </visual>
+
+	  <collision>
+		  <origin xyz="0.0 -0.0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
+		  </geometry>
+	  </collision>
+
+	  <inertial>
+		  <origin xyz="0 0 0" />
+		  <mass value="0.017" />
+		  <inertia ixx="1.0e-03" ixy="0.0" ixz="0.0"
+			  iyy="1.0e-03" iyz="0.0"
+			  izz="1.0e-03" />
+	  </inertial>
+
+<!--     <inertial>
+      <origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
+      <mass value="3.2218127e-02" />
+      <inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
+	       iyy="2.2552871e-05" iyz="-3.1463634e-10"
+	       izz="1.7605306e-05" />
+    </inertial>-->
+  </link>
+
+  <!--  Gripper joint sub -->
+  <joint name="gripper_sub" type="prismatic">
+	  <parent link="link5"/>
+	  <child link="gripper_link_sub"/>
+	  <origin xyz="0.0817 -0.021 0" rpy="0 0 0"/>
+	  <axis xyz="0 -1 0"/>
+	  <limit velocity="4.8" effort="1" lower="-0.010" upper="0.019" />
+	  <mimic joint="gripper" multiplier="1"/>
+  </joint>
+
+  <!-- Transmission 6 -->
+  <xacro:SimpleTransmission n="6" joint="gripper_sub" />
+
+  <!-- end effector joint -->
+  <joint name="end_effector_joint" type="fixed">
+	  <origin xyz="0.126 0.0 0.0" rpy="0 0 0"/>
+	  <parent link="link5"/>
+	  <child link="end_effector_link"/>
+  </joint>
+
+  <!-- end effector link -->
+  <link name="end_effector_link">
+	  <visual>
+		  <origin xyz="0 0 0" rpy="0 0 0"/>
+		  <geometry>
+			  <box size="0.01 0.01 0.01" />
+		  </geometry>
+		  <material name="red"/>
+	  </visual>
+
+	  <inertial>
+		  <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+		  <mass value="0.001"/>
+		  <inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06" />
+	  </inertial>
+  </link>
+
+</robot>
diff --git a/urdf/wild_thumper_with_manipulator.urdf.xacro b/urdf/wild_thumper_with_manipulator.urdf.xacro
new file mode 100644
index 0000000..3144258
--- /dev/null
+++ b/urdf/wild_thumper_with_manipulator.urdf.xacro
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot name="wild_thumper_with_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
+	<xacro:property name="PI" value="3.1415926535897931" />
+	<xacro:include filename="$(find wild_thumper)/urdf/wild_thumper.urdf.xacro"/>
+	<xacro:include filename="$(find wt_open_manipulator)/urdf/open_manipulator.urdf.xacro"/>
+
+	<joint name="open_manipulator_joint" type="fixed">
+		<parent link="mounting_plate"/>
+		<child link="link1"/>
+		<origin xyz="-0.075 0.0 0.045" rpy="0 0 ${PI}"/>
+	</joint>
+</robot>
-- 
2.39.2