From f3cb200aef0e19c2456969808095972012a9c40a Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Fri, 8 Jan 2021 20:13:23 -0300
Subject: [PATCH] Add parameters to xacro models to consider hardware
 variations.

---
 bhand_description/launch/bhand.launch            |  3 +-
 bhand_description/xacro/bhand.urdf.xacro         | 26 +++++++++++++
 bhand_description/xacro/bhand0.urdf.xacro        | 28 --------------
 wam_description/launch/wam.launch                | 17 +--------
 wam_description/xacro/wam.urdf.xacro             | 48 ++++++++++++++++++++++++
 wam_description/xacro/wam_bhand.urdf.xacro       | 17 ---------
 wam_description/xacro/wam_bhand_table.urdf.xacro | 26 -------------
 wam_description/xacro/wam_bhand_world.urdf.xacro | 15 --------
 wam_description/xacro/wam_world.urdf.xacro       | 15 --------
 9 files changed, 76 insertions(+), 119 deletions(-)
 delete mode 100644 bhand_description/xacro/bhand0.urdf.xacro
 delete mode 100644 wam_description/xacro/wam_bhand.urdf.xacro
 delete mode 100644 wam_description/xacro/wam_bhand_table.urdf.xacro
 delete mode 100644 wam_description/xacro/wam_bhand_world.urdf.xacro
 delete mode 100644 wam_description/xacro/wam_world.urdf.xacro

diff --git a/bhand_description/launch/bhand.launch b/bhand_description/launch/bhand.launch
index 243fdbd..ec65298 100644
--- a/bhand_description/launch/bhand.launch
+++ b/bhand_description/launch/bhand.launch
@@ -1,6 +1,5 @@
 <launch>
 	<arg name="world" default="false"/>
 
-	<param unless="$(arg world)" name="robot_description" command="$(find xacro)/xacro '$(find bhand_description)/xacro/bhand.urdf.xacro'" />
-	<param if="$(arg world)" name="robot_description" command="$(find xacro)/xacro '$(find bhand_description)/xacro/bhand0.urdf.xacro'" />
+	<param name="robot_description" command="$(find xacro)/xacro '$(find bhand_description)/xacro/bhand.urdf.xacro' world:=$(arg world)" />
 </launch>
diff --git a/bhand_description/xacro/bhand.urdf.xacro b/bhand_description/xacro/bhand.urdf.xacro
index 5cc7069..018e4c1 100644
--- a/bhand_description/xacro/bhand.urdf.xacro
+++ b/bhand_description/xacro/bhand.urdf.xacro
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <robot name="bhand" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:arg name="world" default="false" />
 
   <xacro:property name="M_PI" value="3.1415926535897931" />
   
@@ -97,4 +98,29 @@
   <xacro:bhand_link3 parent="finger2_link_2" link="finger2_link_3" joint="finger2_joint_3" mimic_joint="finger2_joint_2" />
   <xacro:bhand_link3 parent="finger3_link_2" link="finger3_link_3" joint="finger3_joint_3" mimic_joint="finger3_joint_2" />
 
+  <xacro:if value="$(arg world)" >
+
+	<link name="world" />
+
+	<joint name="bhand_origin_joint" type="fixed">
+		<parent link="world"/>
+		<child link="bhand_origin" />
+		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+	</joint>
+
+	<gazebo>
+		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
+		<!--robotNamespace>bhand</robotNamespace-->
+
+		<!-- Custom plugin -->
+		<!-- robotSimType>bhand_gazebo_ros_control/BhandRobotHWSim</robotSimType -->
+
+		<!-- Default plugin -->
+		<!-- robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType -->
+
+		<controlPeriod>0.001</controlPeriod>
+		</plugin>
+	</gazebo>
+  </xacro:if>
+
 </robot>
diff --git a/bhand_description/xacro/bhand0.urdf.xacro b/bhand_description/xacro/bhand0.urdf.xacro
deleted file mode 100644
index c414dfd..0000000
--- a/bhand_description/xacro/bhand0.urdf.xacro
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0"?>
-<robot name="bhand" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-	<link name="world" />
-
-	<xacro:include filename="$(find bhand_description)/xacro/bhand.urdf.xacro" />
-
-	<joint name="bhand_origin_joint" type="fixed">
-		<parent link="world"/>
-		<child link="bhand_origin" />
-		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-	</joint>
-	
-	<gazebo>
-		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
-		<!--robotNamespace>bhand</robotNamespace-->
-
-		<!-- Custom plugin -->
-		<!-- robotSimType>bhand_gazebo_ros_control/BhandRobotHWSim</robotSimType -->
-
-		<!-- Default plugin -->
-		<!-- robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType -->
-
-		<controlPeriod>0.001</controlPeriod>
-		</plugin>
-	</gazebo>
-
-</robot>
diff --git a/wam_description/launch/wam.launch b/wam_description/launch/wam.launch
index cec8ec6..c383509 100644
--- a/wam_description/launch/wam.launch
+++ b/wam_description/launch/wam.launch
@@ -3,20 +3,5 @@
 	<arg name="bhand" default="true"/>
 	<arg name="world" default="false"/>
 
-	<group unless="$(arg world)">
-		<group unless="$(arg table)">
-			<param unless="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam.urdf.xacro'" />
-			<param if="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam_bhand.urdf.xacro'" />
-		</group>
-		<group if="$(arg table)">
-			<param unless="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam_table.urdf.xacro'" />
-			<param if="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam_bhand_table.urdf.xacro'" />
-		</group>
-	</group>
-
-	<group if="$(arg world)">
-		<param unless="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam_world.urdf.xacro'" />
-		<param if="$(arg bhand)" name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam_bhand_world.urdf.xacro'" />
-	</group>
-
+	<param name="robot_description" command="$(find xacro)/xacro '$(find wam_description)/xacro/wam.urdf.xacro' world:=$(arg world) table:=$(arg table) bhand:=$(arg bhand)" />
 </launch>
diff --git a/wam_description/xacro/wam.urdf.xacro b/wam_description/xacro/wam.urdf.xacro
index 83f0b07..c1cd7f7 100644
--- a/wam_description/xacro/wam.urdf.xacro
+++ b/wam_description/xacro/wam.urdf.xacro
@@ -2,6 +2,12 @@
 
 <robot name="wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <xacro:arg name="world" default="true" />
+  <xacro:arg name="table" default="false" />
+  <xacro:arg name="bhand" default="false" />
+
+  <xacro:property name="M_PI" value="3.1415926535897931" />
+
   <xacro:include filename="$(find wam_description)/xacro/wam_base.urdf.xacro" />
   <xacro:include filename="$(find wam_description)/xacro/wam_j1.urdf.xacro" />
   <xacro:include filename="$(find wam_description)/xacro/wam_j2.urdf.xacro" />
@@ -37,4 +43,46 @@
     </plugin>
   </gazebo>
 
+  <xacro:if value="$(arg world)">
+    <xacro:unless value="$(arg table)">
+
+      <link name="world" />
+
+      <joint name="world_wam_joint" type="fixed">
+        <parent link="world"/>
+        <child link="wam_origin" />
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      </joint>
+    </xacro:unless>
+  </xacro:if>
+
+  <xacro:if value="$(arg table)">
+
+    <link name="world" />
+
+    <xacro:include filename="$(find wam_description)/xacro/table.urdf.xacro" />
+    <joint name="world_table_joint" type="fixed">
+      <parent link="world"/>
+        <child link="table_top_link" />
+        <origin xyz="${-table_x} ${-table_y} ${-table_z}" rpy="0.0 0.0 0.0" />
+      </joint>
+
+      <joint name="table_wam_joint" type="fixed">
+        <parent link="table_top_link"/>
+        <child link="wam_origin" />
+        <origin xyz="${(1.0-0.220)} -0.140 ${table_height}" rpy="0.0 0.0 0.0" />
+      </joint>
+  </xacro:if>
+
+  <xacro:if value="$(arg bhand)">
+    <xacro:include filename="$(find bhand_description)/xacro/bhand.urdf.xacro" />
+
+    <joint name="wam_tool_plate_bhand_joint" type="fixed">
+      <parent link="wam_tool_plate"/>
+      <child link="bhand_origin" />
+      <!--origin xyz="0.0 0.013 0.0" rpy="${-M_PI/2} 0.0 0.0" /-->
+      <origin xyz="0.0 0.0 -0.06" rpy="0.0 0.0 ${M_PI}" />
+    </joint>
+  </xacro:if>
+
 </robot>
diff --git a/wam_description/xacro/wam_bhand.urdf.xacro b/wam_description/xacro/wam_bhand.urdf.xacro
deleted file mode 100644
index db1f97e..0000000
--- a/wam_description/xacro/wam_bhand.urdf.xacro
+++ /dev/null
@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-<robot name="wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-  <xacro:property name="M_PI" value="3.1415926535897931" />  
-  
-  <xacro:include filename="$(find wam_description)/xacro/wam.urdf.xacro" />
-
-  <xacro:include filename="$(find bhand_description)/xacro/bhand.urdf.xacro" />
-
-	<joint name="wam_tool_plate_bhand_joint" type="fixed">
-		<parent link="wam_tool_plate"/>
-		<child link="bhand_origin" />
-		<!--origin xyz="0.0 0.013 0.0" rpy="${-M_PI/2} 0.0 0.0" /-->
-		<origin xyz="0.0 0.0 -0.06" rpy="0.0 0.0 ${M_PI}" />
-	</joint>
-
-</robot>
diff --git a/wam_description/xacro/wam_bhand_table.urdf.xacro b/wam_description/xacro/wam_bhand_table.urdf.xacro
deleted file mode 100644
index 432d8e2..0000000
--- a/wam_description/xacro/wam_bhand_table.urdf.xacro
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name ="wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-	<xacro:property name="M_PI" value="3.1415926535897931" />
-
-	<link name="world" />
-
-	<xacro:include filename="$(find wam_description)/xacro/table.urdf.xacro" />
-
-	<joint name="world_table_joint" type="fixed">
-		<parent link="world"/>
-		<child link="table_top_link" />
-		<origin xyz="${-table_x} ${-table_y} ${-table_z}" rpy="0.0 0.0 0.0" />
-	</joint>
-
-  
-	<xacro:include filename="$(find wam_description)/xacro/wam_bhand.urdf.xacro" />
-
-	<joint name="table_wam_joint" type="fixed">
-		<parent link="table_top_link"/>
-		<child link="wam_origin" />
-		<origin xyz="${(1.0-0.220)} -0.140 ${table_height}" rpy="0.0 0.0 0.0" />
-	</joint>
-
-</robot>
diff --git a/wam_description/xacro/wam_bhand_world.urdf.xacro b/wam_description/xacro/wam_bhand_world.urdf.xacro
deleted file mode 100644
index 6b07214..0000000
--- a/wam_description/xacro/wam_bhand_world.urdf.xacro
+++ /dev/null
@@ -1,15 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name ="wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-	<link name="world" />
-
-	<xacro:include filename="$(find wam_description)/xacro/wam_bhand.urdf.xacro" />
-
-	<joint name="world_wam_joint" type="fixed">
-		<parent link="world"/>
-		<child link="wam_origin" />
-		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-	</joint>
-
-</robot>
diff --git a/wam_description/xacro/wam_world.urdf.xacro b/wam_description/xacro/wam_world.urdf.xacro
deleted file mode 100644
index de57ace..0000000
--- a/wam_description/xacro/wam_world.urdf.xacro
+++ /dev/null
@@ -1,15 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name ="wam" xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-	<link name="world" />
-
-	<xacro:include filename="$(find wam_description)/xacro/wam.urdf.xacro" />
-
-	<joint name="world_wam_joint" type="fixed">
-		<parent link="world"/>
-		<child link="wam_origin" />
-		<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-	</joint>
-
-</robot>
-- 
2.12.0