From bd601b59f3a67e7b1a849c43e40ab991d44fb6eb Mon Sep 17 00:00:00 2001
From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 25 Feb 2024 01:12:49 -0300
Subject: [PATCH] Add payload.

---
 urdf/crane.urdf.xacro |  3 +++
 urdf/payload.xacro    | 40 ++++++++++++++++++++++++++++++++++++++++
 urdf/prismatic.xacro  | 45 +++++++++++++++++++++++++++++++++++++++++----
 3 files changed, 84 insertions(+), 4 deletions(-)
 create mode 100644 urdf/payload.xacro

diff --git a/urdf/crane.urdf.xacro b/urdf/crane.urdf.xacro
index 9056d53..1abc6c0 100644
--- a/urdf/crane.urdf.xacro
+++ b/urdf/crane.urdf.xacro
@@ -23,6 +23,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="crane">
 
 	<xacro:arg name="ignition" default="false"/>
+	<xacro:arg name="payload" default="payload.xacro"/>
 
 	<xacro:include filename="material.xacro"/>	
 	<xacro:include filename="base.xacro"/>
@@ -54,6 +55,8 @@
 		<limit lower="${-base_height}" upper="0"  effort="100.0" velocity="100.0"/>
 	</xacro:hook>
 
+	<xacro:include filename="$(arg payload)"/>
+
 	<xacro:include filename="ros2_control.xacro"/>
  	<xacro:include filename="gazebo_control.xacro"/>
 </robot>
diff --git a/urdf/payload.xacro b/urdf/payload.xacro
new file mode 100644
index 0000000..21204c7
--- /dev/null
+++ b/urdf/payload.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0" ?>
+<!--******************************************************************************
+	                       Crane Description
+	                       Payload  Description
+          Copyright (C) 2024 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<xacro:arg name="ignition" default="false"/>
+
+	<xacro:include filename="prismatic.xacro"/>
+
+	<xacro:property name="payload_radius" value="5e-2"/>
+	<xacro:property name="payload_length" value="10e-2"/>
+
+	<xacro:property name="payload_mass" value="5"/>
+
+	<xacro:cylinder name="payload"
+			length="${payload_length}"
+			radius="${payload_radius}"
+			mass="${payload_mass}"
+			parent="hook_link">
+		<origin xyz="0 0 ${-payload_length/2}"/>
+	</xacro:cylinder>
+</robot>
diff --git a/urdf/prismatic.xacro b/urdf/prismatic.xacro
index d0554ce..8ff2ddc 100644
--- a/urdf/prismatic.xacro
+++ b/urdf/prismatic.xacro
@@ -38,8 +38,8 @@
 				</geometry>
 			</collision>
 			<inertial>
-				<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-				<mass value="${mass}" />
+				<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+				<mass value="${mass}"/>
 				<inertia ixx="${mass/12*(length*length+height*height)}"
 					ixy="0.0" ixz="0.0"
 					iyy="${mass/12*(width*width+height*height)}"
@@ -77,8 +77,8 @@
 				</geometry>
 			</collision>
 			<inertial>
-				<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-				<mass value="${mass}" />
+				<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+				<mass value="${mass}"/>
 				<inertia ixx="${mass/12*(length*length+height*height)}"
 					ixy="0.0" ixz="0.0"
 					iyy="${mass/12*(width*width+height*height)}"
@@ -97,4 +97,41 @@
 			<material>Gazebo/Grey</material>
 		</gazebo>
 	</xacro:macro>
+
+	<xacro:macro name="cylinder" params="name length radius mass parent *origin">
+		<link name="${name}_link">
+			<visual>
+				<geometry>
+        				<cylinder length="${length}" radius="${radius}"/>
+	 			</geometry>
+				<material name="red">
+					<cylinder rgba="0.7 0.0 0.0 1"/>
+				</material>
+			</visual>
+			<collision>
+				<geometry>
+					<box length="${length}" radius="${radius}"/>
+				</geometry>
+			</collision>
+			<inertial>
+				<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+				<mass value="${mass}"/>
+				<inertia ixx="${mass/12*(3*radius*radius+length*length)}"
+					ixy="0.0" ixz="0.0"
+					iyy="${mass/12*(3*radius*radius+length*length)}"
+					iyz="0.0"
+					izz="${mass/12*radius*radius}"/>
+			</inertial>
+		</link>
+
+		<joint name="${name}_joint" type="fixed">
+			<xacro:insert_block name="origin"/>
+			<parent link="${parent}" />
+			<child link="${name}_link" />
+		</joint>
+
+		<gazebo reference="${name}_link">
+			<material>Gazebo/Red</material>
+		</gazebo>
+	</xacro:macro>
 </robot>
-- 
2.12.0