From 6e822d3c3d9e1fc312db3570cf9483e0612ff6c5 Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 21 Feb 2024 14:30:07 -0300
Subject: [PATCH] Add URDF property for profile density.

---
 crane_description/urdf/base.xacro         | 25 +++++++++++++------------
 crane_description/urdf/bridge.xacro       | 19 ++++++++++---------
 crane_description/urdf/crane.urdf.xacro   |  8 ++++----
 crane_description/urdf/hoist.xacro        | 29 +++++++++++++++--------------
 crane_description/urdf/ros2_control.xacro | 12 ++++++------
 crane_description/urdf/trolley.xacro      | 19 ++++++++++---------
 6 files changed, 58 insertions(+), 54 deletions(-)

diff --git a/crane_description/urdf/base.xacro b/crane_description/urdf/base.xacro
index e7953ff..28ec223 100644
--- a/crane_description/urdf/base.xacro
+++ b/crane_description/urdf/base.xacro
@@ -30,13 +30,14 @@
 	<xacro:property name="base_height" value="1080e-3"/>
 
 	<xacro:property name="profile_width" value="80e-3"/>
+	<xacro:property name="profile_density" value="832.81"/>
 
 	<xacro:macro name="base" params="name parent *origin">
 		<xacro:rectangular name="base_bottom_front"
 				width="${profile_width}"
 				length="${base_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="0 ${base_length/2} 0"/>
 		</xacro:rectangular>
@@ -45,7 +46,7 @@
 				width="${profile_width}"
 				length="${base_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="0 ${-base_length/2} 0"/>
 		</xacro:rectangular>
@@ -54,7 +55,7 @@
 				width="${base_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${-base_width/2} 0 0"/>
 		</xacro:rectangular>
@@ -63,7 +64,7 @@
 				width="${base_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${base_width/2} 0 0"/>
 		</xacro:rectangular>
@@ -72,7 +73,7 @@
 				width="${profile_width}"
 				length="${profile_width}"
 				height="${base_height}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${-base_width/2} ${base_length/2} ${base_height/2}"/>
 		</xacro:rectangular>
@@ -81,7 +82,7 @@
 				width="${profile_width}"
 				length="${profile_width}"
 				height="${base_height}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${base_width/2} ${base_length/2} ${base_height/2}"/>
 		</xacro:rectangular>
@@ -90,7 +91,7 @@
 				width="${profile_width}"
 				length="${profile_width}"
 				height="${base_height}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${-base_width/2} ${-base_length/2} ${base_height/2}"/>
 		</xacro:rectangular>
@@ -99,7 +100,7 @@
 				width="${profile_width}"
 				length="${profile_width}"
 				height="${base_height}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${base_width/2} ${-base_length/2} ${base_height/2}"/>
 		</xacro:rectangular>
@@ -108,7 +109,7 @@
 				width="${profile_width}"
 				length="${base_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="0 ${base_length/2} ${base_height}"/>
 		</xacro:rectangular>
@@ -117,7 +118,7 @@
 				width="${profile_width}"
 				length="${base_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="0 ${-base_length/2} ${base_height}"/>
 		</xacro:rectangular>
@@ -126,7 +127,7 @@
 				width="${base_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${-base_width/2} 0 ${base_height}"/>
 		</xacro:rectangular>
@@ -135,7 +136,7 @@
 				width="${base_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${base_width*profile_width*profile_width*1.0}"
+				mass="${base_width*profile_width*profile_width*profile_density}"
 				parent="${parent}">
 			<origin xyz="${base_width/2} 0 ${base_height}"/>
 		</xacro:rectangular>
diff --git a/crane_description/urdf/bridge.xacro b/crane_description/urdf/bridge.xacro
index 601aaed..5d84530 100644
--- a/crane_description/urdf/bridge.xacro
+++ b/crane_description/urdf/bridge.xacro
@@ -30,24 +30,25 @@
 	<xacro:property name="bridge_height" value="80e-3"/>
 
 	<xacro:property name="profile_width" value="80e-3"/>
+	<xacro:property name="profile_density" value="832.81"/>
 
-	<xacro:macro name="bridge" params="name parent *origin">
+	<xacro:macro name="bridge" params="name parent effort velocity *origin">
 		
 		<link name="${name}_link"/>
 	
 		<joint name="${name}_joint" type="prismatic">
 			<xacro:insert_block name="origin"/>
-			<parent link="${parent}" />
-			<child link="${name}_link" />
-			<axis xyz="1 0 0" />
-			<limit lower="${-(base_width-bridge_width)/2}" upper="${(base_width-bridge_width)/2}" effort="1.0" velocity="10.0"/>
+			<parent link="${parent}"/>
+			<child link="${name}_link"/>
+			<axis xyz="1 0 0"/>
+			<limit lower="${-(base_width-bridge_width)/2}" upper="${(base_width-bridge_width)/2}" effort="${effort}" velocity="${velocity}"/>
 		</joint>
 	
 		<xacro:rectangular name="bridge_front"
 				width="${profile_width}"
 				length="${bridge_width}"
 				height="${profile_width}"
-				mass="${bridge_width*profile_width*profile_width*1.0}"
+				mass="${bridge_width*profile_width*profile_width*profile_density}"
 				parent="${name}_link">
 			<origin xyz="0 ${bridge_length/2} 0"/>
 		</xacro:rectangular>
@@ -56,7 +57,7 @@
 				width="${profile_width}"
 				length="${bridge_width}"
 				height="${profile_width}"
-				mass="${bridge_width*profile_width*profile_width*1.0}"
+				mass="${bridge_width*profile_width*profile_width*profile_density}"
 				parent="${name}_link">
 			<origin xyz="0 ${-bridge_length/2} 0"/>
 		</xacro:rectangular>
@@ -65,7 +66,7 @@
 				width="${bridge_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${bridge_width*profile_width*profile_width*1.0}"
+				mass="${bridge_width*profile_width*profile_width*profile_density}"
 				parent="${name}_link">
 			<origin xyz="${-(bridge_width-profile_width)/2} 0 0"/>
 		</xacro:rectangular>
@@ -74,7 +75,7 @@
 				width="${bridge_length}"
 				length="${profile_width}"
 				height="${profile_width}"
-				mass="${bridge_width*profile_width*profile_width*1.0}"
+				mass="${bridge_width*profile_width*profile_width*profile_density}"
 				parent="${name}_link">
 			<origin xyz="${(bridge_width-profile_width)/2} 0 0"/>
 		</xacro:rectangular>
diff --git a/crane_description/urdf/crane.urdf.xacro b/crane_description/urdf/crane.urdf.xacro
index 5b4bfd9..9056d53 100644
--- a/crane_description/urdf/crane.urdf.xacro
+++ b/crane_description/urdf/crane.urdf.xacro
@@ -37,21 +37,21 @@
 		<origin xyz="0 0 0"/>
 	</xacro:base>
 
-	<xacro:bridge name="bridge" parent="base_link">
+	<xacro:bridge name="bridge" parent="base_link" effort="100.0" velocity="100.0">
 		<origin xyz="0 0 ${base_height+profile_width}"/>
 	</xacro:bridge>
 
-	<xacro:trolley name="trolley" parent="bridge_link">
+	<xacro:trolley name="trolley" parent="bridge_link" effort="100.0" velocity="100.0">
 		<origin xyz="0 0 ${(bridge_height+trolley_height)/2}"/>
 	</xacro:trolley>
 
-	<xacro:hoist name="hoist" parent="trolley_link">
+	<xacro:hoist name="hoist" parent="trolley_link" effort="100.0" velocity="100.0">
 		<origin xyz="0 0 0"/>
 	</xacro:hoist>
 
 	<xacro:hook name="hook" parent="hoist_yaw_link">
 		<origin xyz="0 0 0"/>
-		<limit lower="${-base_height}" upper="0"  effort="1.0" velocity="10.0"/>
+		<limit lower="${-base_height}" upper="0"  effort="100.0" velocity="100.0"/>
 	</xacro:hook>
 
 	<xacro:include filename="ros2_control.xacro"/>
diff --git a/crane_description/urdf/hoist.xacro b/crane_description/urdf/hoist.xacro
index 44e084d..c60bd2a 100644
--- a/crane_description/urdf/hoist.xacro
+++ b/crane_description/urdf/hoist.xacro
@@ -28,44 +28,45 @@
 	<xacro:property name="hoist_width" value="${300e-3-2*trolley_height}"/>
 	<xacro:property name="hoist_length" value="40e-3"/>
 	<xacro:property name="hoist_height" value="40e-3"/>
+	<xacro:property name="profile_density" value="832.81"/>
 
-	<xacro:macro name="hoist" params="name parent *origin">
+	<xacro:macro name="hoist" params="name parent effort velocity *origin">
 		
 		<xacro:rectangular name="${name}"
 				width="${hoist_height}"
 				length="${hoist_width}"
 				height="${hoist_height}"
-				mass="${hoist_width*hoist_height*hoist_height*1.0}"
+				mass="${hoist_width*hoist_height*hoist_height*profile_density}"
 				parent="${parent}">
 			<xacro:insert_block name="origin"/>
 		</xacro:rectangular>
 
 		<joint name="${name}_roll_joint" type="revolute">
 			<origin rpy="0 0 0"/>
-			<parent link="${name}_link" />
-			<child link="${name}_roll_link" />
-			<axis xyz="1 0 0" />
-			<limit lower="${-pi/4}" upper="${pi/4}" effort="1.0" velocity="10.0"/>
+			<parent link="${name}_link"/>
+			<child link="${name}_roll_link"/>
+			<axis xyz="1 0 0"/>
+			<limit lower="${-pi/4}" upper="${pi/4}" effort="${effort}" velocity="${velocity}"/>
 		</joint>
 		
 		<link name="${name}_roll_link"/>
 
 		<joint name="${name}_pitch_joint" type="revolute">
 			<origin rpy="0 0 0"/>
-			<parent link="${name}_roll_link" />
-			<child link="${name}_pitch_link" />
-			<axis xyz="0 1 0" />
-			<limit lower="${-pi/4}" upper="${pi/4}" effort="1.0" velocity="10.0"/>
+			<parent link="${name}_roll_link"/>
+			<child link="${name}_pitch_link"/>
+			<axis xyz="0 1 0"/>
+			<limit lower="${-pi/4}" upper="${pi/4}" effort="${effort}" velocity="${velocity}"/>
 		</joint>
 		
 		<link name="${name}_pitch_link"/>
 
 		<joint name="${name}_yaw_joint" type="revolute">
 			<origin rpy="0 0 0"/>
-			<parent link="${name}_pitch_link" />
-			<child link="${name}_yaw_link" />
-			<axis xyz="0 0 1" />
-			<limit lower="${-pi/4}" upper="${pi/4}" effort="1.0" velocity="10.0"/>
+			<parent link="${name}_pitch_link"/>
+			<child link="${name}_yaw_link"/>
+			<axis xyz="0 0 1"/>
+			<limit lower="${-pi/4}" upper="${pi/4}" effort="${effort}" velocity="${velocity}"/>
 		</joint>
 		
 		<link name="${name}_yaw_link"/>
diff --git a/crane_description/urdf/ros2_control.xacro b/crane_description/urdf/ros2_control.xacro
index 2955fdb..99e9cd7 100644
--- a/crane_description/urdf/ros2_control.xacro
+++ b/crane_description/urdf/ros2_control.xacro
@@ -51,8 +51,8 @@
 
 		<joint name="bridge_joint">
 			<command_interface name="effort">
-				<param name="min">-1.0</param>
-				<param name="max">1.0</param>
+				<param name="min">-100.0</param>
+				<param name="max">100.0</param>
 			</command_interface>
 			<state_interface name="position"/>
 			<state_interface name="velocity"/>
@@ -61,8 +61,8 @@
 
 		<joint name="trolley_joint">
 			<command_interface name="effort">
-				<param name="min">-1.0</param>
-				<param name="max">1.0</param>
+				<param name="min">-100.0</param>
+				<param name="max">100.0</param>
 			</command_interface>
 			<state_interface name="position"/>
 			<state_interface name="velocity"/>
@@ -86,8 +86,8 @@
 
 		<joint name="hook_joint">
 			<command_interface name="effort">
-				<param name="min">-1.0</param>
-				<param name="max">1.0</param>
+				<param name="min">-100.0</param>
+				<param name="max">100.0</param>
 			</command_interface>
 			<state_interface name="position"/>
 			<state_interface name="velocity"/>
diff --git a/crane_description/urdf/trolley.xacro b/crane_description/urdf/trolley.xacro
index bea6409..a636266 100644
--- a/crane_description/urdf/trolley.xacro
+++ b/crane_description/urdf/trolley.xacro
@@ -28,24 +28,25 @@
 	<xacro:property name="trolley_width" value="300e-3"/>
 	<xacro:property name="trolley_length" value="300e-3"/>
 	<xacro:property name="trolley_height" value="40e-3"/>
+	<xacro:property name="profile_density" value="832.81"/>
 
-	<xacro:macro name="trolley" params="name parent *origin">
+	<xacro:macro name="trolley" params="name parent effort velocity *origin">
 		
 		<link name="${name}_link"/>
 	
 		<joint name="${name}_joint" type="prismatic">
 			<xacro:insert_block name="origin"/>
-			<parent link="${parent}" />
-			<child link="${name}_link" />
-			<axis xyz="0 1 0" />
-			<limit lower="${-(bridge_length-trolley_width)/2}" upper="${(bridge_length-trolley_width)/2}" effort="1.0" velocity="10.0"/>
+			<parent link="${parent}"/>
+			<child link="${name}_link"/>
+			<axis xyz="0 1 0"/>
+			<limit lower="${-(bridge_length-trolley_width)/2}" upper="${(bridge_length-trolley_width)/2}" effort="${effort}" velocity="${velocity}"/>
 		</joint>
 	
 		<xacro:rectangular name="trolley_front"
 				width="${trolley_height}"
 				length="${trolley_width}"
 				height="${trolley_height}"
-				mass="${trolley_width*trolley_height*trolley_height*1.0}"
+				mass="${trolley_width*trolley_height*trolley_height*profile_density}"
 				parent="${name}_link">
 			<origin xyz="0 ${trolley_length/2} 0"/>
 		</xacro:rectangular>
@@ -54,7 +55,7 @@
 				width="${trolley_height}"
 				length="${trolley_width}"
 				height="${trolley_height}"
-				mass="${trolley_width*trolley_height*trolley_height*1.0}"
+				mass="${trolley_width*trolley_height*trolley_height*profile_density}"
 				parent="${name}_link">
 			<origin xyz="0 ${-trolley_length/2} 0"/>
 		</xacro:rectangular>
@@ -63,7 +64,7 @@
 				width="${trolley_length}"
 				length="${trolley_height}"
 				height="${trolley_height}"
-				mass="${trolley_width*trolley_height*trolley_height*1.0}"
+				mass="${trolley_width*trolley_height*trolley_height*profile_density}"
 				parent="${name}_link">
 			<origin xyz="${-(trolley_width-trolley_height)/2} 0 0"/>
 		</xacro:rectangular>
@@ -72,7 +73,7 @@
 				width="${trolley_length}"
 				length="${trolley_height}"
 				height="${trolley_height}"
-				mass="${trolley_width*trolley_height*trolley_height*1.0}"
+				mass="${trolley_width*trolley_height*trolley_height*profile_density}"
 				parent="${name}_link">
 			<origin xyz="${(trolley_width-trolley_height)/2} 0 0"/>
 		</xacro:rectangular>
-- 
2.12.0