From 594778fcc1f0dac11ba2055cafa40d27dddb2a0e Mon Sep 17 00:00:00 2001
From: Paul <paul_d_blum@yahoo.com>
Date: Mon, 29 Apr 2024 17:40:13 -0400
Subject: [PATCH 1/4] Add xacro macro for Z1

---
 robots/z1_description/xacro/robot.xacro | 304 +----------------------
 robots/z1_description/xacro/z1.xacro    | 309 ++++++++++++++++++++++++
 2 files changed, 313 insertions(+), 300 deletions(-)
 create mode 100644 robots/z1_description/xacro/z1.xacro

diff --git a/robots/z1_description/xacro/robot.xacro b/robots/z1_description/xacro/robot.xacro
index 7f57a05a..184d6244 100644
--- a/robots/z1_description/xacro/robot.xacro
+++ b/robots/z1_description/xacro/robot.xacro
@@ -2,12 +2,11 @@
 
 <robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:include filename="$(find z1_description)/xacro/const.xacro"/>
-    <xacro:include filename="$(find z1_description)/xacro/gazebo.xacro"/>
-    <xacro:include filename="$(find z1_description)/xacro/transmission.xacro"/>
-
     <xacro:arg name="UnitreeGripper" default="false"/>
 
+    <xacro:include filename="$(find z1_description)/xacro/z1.xacro"/>
+    <xacro:include filename="$(find z1_description)/xacro/gazebo.xacro"/>
+
     <link name="world"/>
 
     <joint name="base_static_joint" type="fixed">
@@ -16,301 +15,6 @@
         <child link="link00"/>
     </joint>
 
-    <link name="link00">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link00.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <geometry>
-                <cylinder length="${motor_height}" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="0 0 0" xyz="0 0 ${motor_height/2.0}"/>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L00_ComX} ${L00_ComY} ${L00_ComZ}"/>
-            <mass value="${L00_Mass}"/>
-            <inertia
-                ixx="${L00_Ixx}" ixy="${L00_Ixy}" ixz="${L00_Ixz}"
-                iyy="${L00_Iyy}" iyz="${L00_Iyz}"
-                izz="${L00_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint1" type="revolute">
-        <origin rpy="0 0 0" xyz="0 0 0.0585"/>
-        <parent link="link00"/>
-        <child link="link01"/>
-        <axis xyz="0 0 1"/>
-        <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-        <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint1_PositionMin}" upper="${joint1_PositionMax}"/>
-    </joint>
-
-    <link name="link01">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L01_ComX} ${L01_ComY} ${L01_ComZ}"/>
-            <mass value="${L01_Mass}"/>
-            <inertia
-                ixx="${L01_Ixx}" ixy="${L01_Ixy}" ixz="${L01_Ixz}"
-                iyy="${L01_Iyy}" iyz="${L01_Iyz}"
-                izz="${L01_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint2" type="revolute">
-        <origin rpy="0 0 0" xyz="0 0 0.045"/>
-        <parent link="link01"/>
-        <child link="link02"/>
-        <axis xyz="0 1 0"/>
-        <dynamics damping="${2*jointDamping}" friction="${2*jointFriction}"/>
-        <limit effort="${2*torqueMax}" velocity="${velocityMax}" lower="${joint2_PositionMin}" upper="${joint2_PositionMax}"/>
-    </joint>
-
-    <link name="link02">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link02.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <geometry>
-                <cylinder length="${2.0*motor_height}" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
-        </collision>
-        <collision>
-            <geometry>
-                <cylinder length="${arm1_height}" radius="${arm1_diameter}"/>
-            </geometry>
-            <origin rpy="0 ${PI/2.0} 0" xyz="${-0.045-arm1_height/2.0} 0 0"/>
-        </collision>
-        <collision>
-            <geometry>
-                <cylinder length="${motor_height}" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="${PI/2.0} 0 0" xyz="-0.35 0 0"/>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L02_ComX} ${L02_ComY} ${L02_ComZ}"/>
-            <mass value="${L02_Mass}"/>
-            <inertia
-                ixx="${L02_Ixx}" ixy="${L02_Ixy}" ixz="${L02_Ixz}"
-                iyy="${L02_Iyy}" iyz="${L02_Iyz}"
-                izz="${L02_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint3" type="revolute">
-        <origin rpy="0 0 0" xyz="-0.35 0 0"/>
-        <parent link="link02"/>
-        <child link="link03"/>
-        <axis xyz="0 1 0"/>
-        <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-        <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint3_PositionMin}" upper="${joint3_PositionMax}"/>
-    </joint>
-
-    <link name="link03">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link03.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <geometry>
-                <cylinder length="${arm2_height-0.01}" radius="${arm2_diameter}"/>
-            </geometry>
-            <origin rpy="0 ${PI/2.0} 0" xyz="${0.065+arm2_height/2.0} 0 0.055"/>
-        </collision>
-        <collision>
-            <geometry>
-                <cylinder length="0.059" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="0 ${PI/2.0} ${PI/2.0}" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/>
-            <mass value="${L03_Mass}"/>
-            <inertia
-                ixx="${L03_Ixx}" ixy="${L03_Ixy}" ixz="${L03_Ixz}"
-                iyy="${L03_Iyy}" iyz="${L03_Iyz}"
-                izz="${L03_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint4" type="revolute">
-        <origin rpy="0 0 0" xyz="0.218 0 0.057"/>
-        <parent link="link03"/>
-        <child link="link04"/>
-        <axis xyz="0 1 0"/>
-        <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-        <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint4_PositionMin}" upper="${joint4_PositionMax}"/>
-    </joint>
-
-    <link name="link04">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link04.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <geometry>
-                <cylinder length="${motor_height + 2*0.008}" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="0 0 0" xyz="0.072 0 0"/>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/>
-            <mass value="${L04_Mass}"/>
-            <inertia
-                ixx="${L04_Ixx}" ixy="${L04_Ixy}" ixz="${L04_Ixz}"
-                iyy="${L04_Iyy}" iyz="${L04_Iyz}"
-                izz="${L04_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint5" type="revolute">
-        <origin rpy="0 0 0" xyz="0.07 0.0 0.0"/>
-        <parent link="link04"/>
-        <child link="link05"/>
-        <axis xyz="0 0 1"/>
-        <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-        <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint5_PositionMin}" upper="${joint5_PositionMax}"/>
-    </joint>
-
-    <link name="link05">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link05.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L05_ComX} ${L05_ComY} ${L05_ComZ}"/>
-            <mass value="${L05_Mass}"/>
-            <inertia
-                ixx="${L05_Ixx}" ixy="${L05_Ixy}" ixz="${L05_Ixz}"
-                iyy="${L05_Iyy}" iyz="${L05_Iyz}"
-                izz="${L05_Izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="joint6" type="revolute">
-        <origin rpy="0 0 0" xyz="0.0492 0.0 0.0"/>
-        <parent link="link05"/>
-        <child link="link06"/>
-        <axis xyz="1 0 0"/>
-        <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-        <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint6_PositionMin}" upper="${joint6_PositionMax}"/>
-    </joint>
-
-    <link name="link06">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://z1_description/meshes/visual/z1_Link06.dae" scale="1 1 1"/>
-            </geometry>
-        </visual>
-        <collision>
-            <geometry>
-                <cylinder length="${motor_height}" radius="${motor_diameter}"/>
-            </geometry>
-            <origin rpy="0 ${PI/2.0} 0" xyz="${motor_height/2.0} 0 0"/>
-        </collision>
-        <inertial>
-            <origin rpy="0 0 0" xyz="${L06_ComX} ${L06_ComY} ${L06_ComZ}"/>
-            <mass value="${L06_Mass}"/>
-            <inertia
-                ixx="${L06_Ixx}" ixy="${L06_Ixy}" ixz="${L06_Ixz}"
-                iyy="${L06_Iyy}" iyz="${L06_Iyz}"
-                izz="${L06_Izz}"/> 
-        </inertial>
-    </link>
-
-    <xacro:if value="$(arg UnitreeGripper)">
-        <joint name="gripperStator" type="fixed">
-            <origin rpy="0 0 0" xyz="0.051 0.0 0.0"/>
-            <parent link="link06"/>
-            <child link="gripperStator"/>
-        </joint>
-
-        <link name="gripperStator">
-            <visual>
-                <geometry>
-                    <mesh filename="package://z1_description/meshes/visual/z1_GripperStator.dae" scale="1 1 1"/>
-                </geometry>
-            </visual>
-            <collision>
-                <geometry>
-                    <mesh filename="package://z1_description/meshes/collision/z1_GripperStator.STL" scale="1 1 1"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${GripperStator_ComX} ${GripperStator_ComY} ${GripperStator_ComZ}"/>
-                <mass value="${GripperStator_Mass}"/>
-                <inertia
-                    ixx="${GripperStator_Ixx}" ixy="${GripperStator_Ixy}" ixz="${GripperStator_Ixz}"
-                    iyy="${GripperStator_Iyy}" iyz="${GripperStator_Iyz}"
-                    izz="${GripperStator_Izz}"/> 
-            </inertial>
-        </link>
-
-        <joint name="jointGripper" type="revolute">
-            <origin rpy="0 0 0" xyz="0.049 0.0 0"/>
-            <parent link="gripperStator"/>
-            <child link="gripperMover"/>
-            <axis xyz="0 1 0"/>
-            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
-            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${Gripper_PositionMin}" upper="${Gripper_PositionMax}"/>
-        </joint>
-
-        <link name="gripperMover">
-            <visual>
-                <geometry>
-                    <mesh filename="package://z1_description/meshes/visual/z1_GripperMover.dae" scale="1 1 1"/>
-                </geometry>
-            </visual>
-            <collision>
-                <geometry>
-                    <mesh filename="package://z1_description/meshes/collision/z1_GripperMover.STL" scale="1 1 1"/>
-                </geometry>
-            </collision>
-            <inertial>
-                <origin rpy="0 0 0" xyz="${GripperMover_ComX} ${GripperMover_ComY} ${GripperMover_ComZ}"/>
-                <mass value="${GripperMover_Mass}"/>
-                <inertia
-                    ixx="${GripperMover_Ixx}" ixy="${GripperMover_Ixy}" ixz="${GripperMover_Ixz}"
-                    iyy="${GripperMover_Iyy}" iyz="${GripperMover_Iyz}"
-                    izz="${GripperMover_Izz}"/> 
-            </inertial>
-        </link>
-
-        <xacro:motorTransmission name="Gripper"/>
-
-        <gazebo reference="gripperStator">
-            <self_collide>true</self_collide>
-        </gazebo>
-
-        <gazebo reference="gripperMover">
-            <self_collide>true</self_collide>
-        </gazebo>
-    </xacro:if>
-
-    <xacro:motorTransmission name="1"/>
-    <xacro:motorTransmission name="2"/>
-    <xacro:motorTransmission name="3"/>
-    <xacro:motorTransmission name="4"/>
-    <xacro:motorTransmission name="5"/>
-    <xacro:motorTransmission name="6"/>
+    <xacro:z1 UnitreeGripper="$(arg UnitreeGripper)"/>
 
 </robot>
diff --git a/robots/z1_description/xacro/z1.xacro b/robots/z1_description/xacro/z1.xacro
new file mode 100644
index 00000000..2497df6f
--- /dev/null
+++ b/robots/z1_description/xacro/z1.xacro
@@ -0,0 +1,309 @@
+<?xml version="1.0"?>
+
+<robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="z1" params="UnitreeGripper:=false">
+
+        <xacro:include filename="$(find z1_description)/xacro/const.xacro"/>
+        <xacro:include filename="$(find z1_description)/xacro/transmission.xacro"/>
+
+        <link name="link00">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link00.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder length="${motor_height}" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="0 0 0" xyz="0 0 ${motor_height/2.0}"/>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L00_ComX} ${L00_ComY} ${L00_ComZ}"/>
+                <mass value="${L00_Mass}"/>
+                <inertia
+                    ixx="${L00_Ixx}" ixy="${L00_Ixy}" ixz="${L00_Ixz}"
+                    iyy="${L00_Iyy}" iyz="${L00_Iyz}"
+                    izz="${L00_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint1" type="revolute">
+            <origin rpy="0 0 0" xyz="0 0 0.0585"/>
+            <parent link="link00"/>
+            <child link="link01"/>
+            <axis xyz="0 0 1"/>
+            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint1_PositionMin}" upper="${joint1_PositionMax}"/>
+        </joint>
+
+        <link name="link01">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link01.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L01_ComX} ${L01_ComY} ${L01_ComZ}"/>
+                <mass value="${L01_Mass}"/>
+                <inertia
+                    ixx="${L01_Ixx}" ixy="${L01_Ixy}" ixz="${L01_Ixz}"
+                    iyy="${L01_Iyy}" iyz="${L01_Iyz}"
+                    izz="${L01_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint2" type="revolute">
+            <origin rpy="0 0 0" xyz="0 0 0.045"/>
+            <parent link="link01"/>
+            <child link="link02"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${2*jointDamping}" friction="${2*jointFriction}"/>
+            <limit effort="${2*torqueMax}" velocity="${velocityMax}" lower="${joint2_PositionMin}" upper="${joint2_PositionMax}"/>
+        </joint>
+
+        <link name="link02">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link02.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder length="${2.0*motor_height}" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
+            </collision>
+            <collision>
+                <geometry>
+                    <cylinder length="${arm1_height}" radius="${arm1_diameter}"/>
+                </geometry>
+                <origin rpy="0 ${PI/2.0} 0" xyz="${-0.045-arm1_height/2.0} 0 0"/>
+            </collision>
+            <collision>
+                <geometry>
+                    <cylinder length="${motor_height}" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="${PI/2.0} 0 0" xyz="-0.35 0 0"/>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L02_ComX} ${L02_ComY} ${L02_ComZ}"/>
+                <mass value="${L02_Mass}"/>
+                <inertia
+                    ixx="${L02_Ixx}" ixy="${L02_Ixy}" ixz="${L02_Ixz}"
+                    iyy="${L02_Iyy}" iyz="${L02_Iyz}"
+                    izz="${L02_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint3" type="revolute">
+            <origin rpy="0 0 0" xyz="-0.35 0 0"/>
+            <parent link="link02"/>
+            <child link="link03"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint3_PositionMin}" upper="${joint3_PositionMax}"/>
+        </joint>
+
+        <link name="link03">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link03.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder length="${arm2_height-0.01}" radius="${arm2_diameter}"/>
+                </geometry>
+                <origin rpy="0 ${PI/2.0} 0" xyz="${0.065+arm2_height/2.0} 0 0.055"/>
+            </collision>
+            <collision>
+                <geometry>
+                    <cylinder length="0.059" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="0 ${PI/2.0} ${PI/2.0}" xyz="${0.065+0.185-0.059/2.0} 0 0.055"/>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L03_ComX} ${L03_ComY} ${L03_ComZ}"/>
+                <mass value="${L03_Mass}"/>
+                <inertia
+                    ixx="${L03_Ixx}" ixy="${L03_Ixy}" ixz="${L03_Ixz}"
+                    iyy="${L03_Iyy}" iyz="${L03_Iyz}"
+                    izz="${L03_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint4" type="revolute">
+            <origin rpy="0 0 0" xyz="0.218 0 0.057"/>
+            <parent link="link03"/>
+            <child link="link04"/>
+            <axis xyz="0 1 0"/>
+            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint4_PositionMin}" upper="${joint4_PositionMax}"/>
+        </joint>
+
+        <link name="link04">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link04.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder length="${motor_height + 2*0.008}" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="0 0 0" xyz="0.072 0 0"/>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L04_ComX} ${L04_ComY} ${L04_ComZ}"/>
+                <mass value="${L04_Mass}"/>
+                <inertia
+                    ixx="${L04_Ixx}" ixy="${L04_Ixy}" ixz="${L04_Ixz}"
+                    iyy="${L04_Iyy}" iyz="${L04_Iyz}"
+                    izz="${L04_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint5" type="revolute">
+            <origin rpy="0 0 0" xyz="0.07 0.0 0.0"/>
+            <parent link="link04"/>
+            <child link="link05"/>
+            <axis xyz="0 0 1"/>
+            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint5_PositionMin}" upper="${joint5_PositionMax}"/>
+        </joint>
+
+        <link name="link05">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link05.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L05_ComX} ${L05_ComY} ${L05_ComZ}"/>
+                <mass value="${L05_Mass}"/>
+                <inertia
+                    ixx="${L05_Ixx}" ixy="${L05_Ixy}" ixz="${L05_Ixz}"
+                    iyy="${L05_Iyy}" iyz="${L05_Iyz}"
+                    izz="${L05_Izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="joint6" type="revolute">
+            <origin rpy="0 0 0" xyz="0.0492 0.0 0.0"/>
+            <parent link="link05"/>
+            <child link="link06"/>
+            <axis xyz="1 0 0"/>
+            <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+            <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${joint6_PositionMin}" upper="${joint6_PositionMax}"/>
+        </joint>
+
+        <link name="link06">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://z1_description/meshes/visual/z1_Link06.dae" scale="1 1 1"/>
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <cylinder length="${motor_height}" radius="${motor_diameter}"/>
+                </geometry>
+                <origin rpy="0 ${PI/2.0} 0" xyz="${motor_height/2.0} 0 0"/>
+            </collision>
+            <inertial>
+                <origin rpy="0 0 0" xyz="${L06_ComX} ${L06_ComY} ${L06_ComZ}"/>
+                <mass value="${L06_Mass}"/>
+                <inertia
+                    ixx="${L06_Ixx}" ixy="${L06_Ixy}" ixz="${L06_Ixz}"
+                    iyy="${L06_Iyy}" iyz="${L06_Iyz}"
+                    izz="${L06_Izz}"/>
+            </inertial>
+        </link>
+
+        <xacro:if value="${UnitreeGripper}">
+            <joint name="gripperStator" type="fixed">
+                <origin rpy="0 0 0" xyz="0.051 0.0 0.0"/>
+                <parent link="link06"/>
+                <child link="gripperStator"/>
+            </joint>
+
+            <link name="gripperStator">
+                <visual>
+                    <geometry>
+                        <mesh filename="package://z1_description/meshes/visual/z1_GripperStator.dae" scale="1 1 1"/>
+                    </geometry>
+                </visual>
+                <collision>
+                    <geometry>
+                        <mesh filename="package://z1_description/meshes/collision/z1_GripperStator.STL" scale="1 1 1"/>
+                    </geometry>
+                </collision>
+                <inertial>
+                    <origin rpy="0 0 0" xyz="${GripperStator_ComX} ${GripperStator_ComY} ${GripperStator_ComZ}"/>
+                    <mass value="${GripperStator_Mass}"/>
+                    <inertia
+                        ixx="${GripperStator_Ixx}" ixy="${GripperStator_Ixy}" ixz="${GripperStator_Ixz}"
+                        iyy="${GripperStator_Iyy}" iyz="${GripperStator_Iyz}"
+                        izz="${GripperStator_Izz}"/>
+                </inertial>
+            </link>
+
+            <joint name="jointGripper" type="revolute">
+                <origin rpy="0 0 0" xyz="0.049 0.0 0"/>
+                <parent link="gripperStator"/>
+                <child link="gripperMover"/>
+                <axis xyz="0 1 0"/>
+                <dynamics damping="${jointDamping}" friction="${jointFriction}"/>
+                <limit effort="${torqueMax}" velocity="${velocityMax}" lower="${Gripper_PositionMin}" upper="${Gripper_PositionMax}"/>
+            </joint>
+
+            <link name="gripperMover">
+                <visual>
+                    <geometry>
+                        <mesh filename="package://z1_description/meshes/visual/z1_GripperMover.dae" scale="1 1 1"/>
+                    </geometry>
+                </visual>
+                <collision>
+                    <geometry>
+                        <mesh filename="package://z1_description/meshes/collision/z1_GripperMover.STL" scale="1 1 1"/>
+                    </geometry>
+                </collision>
+                <inertial>
+                    <origin rpy="0 0 0" xyz="${GripperMover_ComX} ${GripperMover_ComY} ${GripperMover_ComZ}"/>
+                    <mass value="${GripperMover_Mass}"/>
+                    <inertia
+                        ixx="${GripperMover_Ixx}" ixy="${GripperMover_Ixy}" ixz="${GripperMover_Ixz}"
+                        iyy="${GripperMover_Iyy}" iyz="${GripperMover_Iyz}"
+                        izz="${GripperMover_Izz}"/>
+                </inertial>
+            </link>
+
+            <xacro:motorTransmission name="Gripper"/>
+
+            <gazebo reference="gripperStator">
+                <self_collide>true</self_collide>
+            </gazebo>
+
+            <gazebo reference="gripperMover">
+                <self_collide>true</self_collide>
+            </gazebo>
+        </xacro:if>
+
+        <xacro:motorTransmission name="1"/>
+        <xacro:motorTransmission name="2"/>
+        <xacro:motorTransmission name="3"/>
+        <xacro:motorTransmission name="4"/>
+        <xacro:motorTransmission name="5"/>
+        <xacro:motorTransmission name="6"/>
+
+    </xacro:macro>
+
+</robot>

From 3435263d52a8b09bebaad48f4f097dce8e1315f0 Mon Sep 17 00:00:00 2001
From: Paul <paul_d_blum@yahoo.com>
Date: Mon, 29 Apr 2024 17:41:11 -0400
Subject: [PATCH 2/4] Add xacro macro for B1

---
 robots/b1_description/xacro/b1.xacro    | 93 +++++++++++++++++++++++++
 robots/b1_description/xacro/robot.xacro | 88 ++---------------------
 2 files changed, 98 insertions(+), 83 deletions(-)
 create mode 100644 robots/b1_description/xacro/b1.xacro

diff --git a/robots/b1_description/xacro/b1.xacro b/robots/b1_description/xacro/b1.xacro
new file mode 100644
index 00000000..5540992d
--- /dev/null
+++ b/robots/b1_description/xacro/b1.xacro
@@ -0,0 +1,93 @@
+<?xml version="1.0"?>
+
+<robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="b1">
+
+        <xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
+        <xacro:include filename="$(find b1_description)/xacro/materials.xacro"/>
+        <xacro:include filename="$(find b1_description)/xacro/leg.xacro"/>
+
+        <!-- Rotor related joint and link is only for demonstrate location. -->
+        <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+
+        <link name="base">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <box size="0.001 0.001 0.001"/>
+                </geometry>
+            </visual>
+        </link>
+
+        <joint name="floating_base" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="base"/>
+            <child link="trunk"/>
+        </joint>
+
+        <link name="trunk">
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
+                </geometry>
+                <material name="orange"/>
+            </visual>
+            <collision>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
+                </geometry>
+            </collision>
+            <!-- <collision>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
+                </geometry>
+            </collision> -->
+            <inertial>
+                <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
+                <mass value="${trunk_mass}"/>
+                <inertia
+                    ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
+                    iyy="${trunk_iyy}" iyz="${trunk_iyz}"
+                    izz="${trunk_izz}"/>
+            </inertial>
+        </link>
+
+        <joint name="imu_joint" type="fixed">
+            <parent link="trunk"/>
+            <child link="imu_link"/>
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+        </joint>
+
+        <link name="imu_link">
+            <inertial>
+                <mass value="0.001"/>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
+            </inertial>
+            <visual>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <box size="0.001 0.001 0.001"/>
+                </geometry>
+                <material name="red"/>
+            </visual>
+            <collision>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <geometry>
+                    <box size=".001 .001 .001"/>
+                </geometry>
+            </collision>
+        </link>
+
+        <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
+        <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
+        <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
+        <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
+
+    </xacro:macro>
+
+</robot>
diff --git a/robots/b1_description/xacro/robot.xacro b/robots/b1_description/xacro/robot.xacro
index 30b6127e..70f2aadc 100644
--- a/robots/b1_description/xacro/robot.xacro
+++ b/robots/b1_description/xacro/robot.xacro
@@ -2,13 +2,10 @@
 
 <robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
-    <xacro:include filename="$(find b1_description)/xacro/materials.xacro"/>
-    <xacro:include filename="$(find b1_description)/xacro/leg.xacro"/>
-    <xacro:include filename="$(find b1_description)/xacro/gazebo.xacro"/>
+    <xacro:arg name="DEBUG" default="false"/>
 
-    <!-- Rotor related joint and link is only for demonstrate location. -->
-    <!-- Actually, the rotor will rotate and the joint is not fixed. Reduction ratio should be considered. -->
+    <xacro:include filename="$(find b1_description)/xacro/b1.xacro"/>
+    <xacro:include filename="$(find b1_description)/xacro/gazebo.xacro"/>
 
     <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
     <xacro:if value="$(arg DEBUG)">
@@ -18,84 +15,9 @@
             <parent link="world"/>
             <child link="base"/>
         </joint>
-    </xacro:if> 
-
-    <link name="base">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size="0.001 0.001 0.001"/>
-            </geometry>
-        </visual>
-    </link>
-
-    <joint name="floating_base" type="fixed">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <parent link="base"/>
-        <child link="trunk"/>
-    </joint>
-
-    <link name="trunk">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
-            </geometry>
-            <material name="orange"/>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size="${trunk_length} ${trunk_width} ${trunk_height}"/>
-            </geometry>
-        </collision>
-        <!-- <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <mesh filename="package://b1_description/meshes/trunk.dae" scale="1 1 1"/>
-            </geometry>
-        </collision> -->
-        <inertial>
-            <origin rpy="0 0 0" xyz="${trunk_com_x} ${trunk_com_y} ${trunk_com_z}"/>
-            <mass value="${trunk_mass}"/>
-            <inertia
-                ixx="${trunk_ixx}" ixy="${trunk_ixy}" ixz="${trunk_ixz}"
-                iyy="${trunk_iyy}" iyz="${trunk_iyz}"
-                izz="${trunk_izz}"/>  
-        </inertial>
-    </link>
-
-    <joint name="imu_joint" type="fixed">
-        <parent link="trunk"/>
-        <child link="imu_link"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-    </joint>
-
-    <link name="imu_link">
-        <inertial>
-            <mass value="0.001"/>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size="0.001 0.001 0.001"/>
-            </geometry>
-            <material name="red"/>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0"/>
-            <geometry>
-                <box size=".001 .001 .001"/>
-            </geometry>
-        </collision>
-    </link>
+    </xacro:if>
 
-    <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
-    <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
-    <xacro:leg name="RR" mirror="-1" mirror_dae="False"  front_hind="-1" front_hind_dae="False" />
-    <xacro:leg name="RL" mirror="1"  mirror_dae="True"   front_hind="-1" front_hind_dae="False" />
+    <xacro:b1/>
 
     <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
 

From 9f3e0710db1bfbc40edd8920b8254ac5775e5e15 Mon Sep 17 00:00:00 2001
From: Paul <paul_d_blum@yahoo.com>
Date: Mon, 29 Apr 2024 17:42:46 -0400
Subject: [PATCH 3/4] Add b1z1_description package

---
 robots/b1z1_description/CMakeLists.txt        |   4 +
 .../config/robot_control.yaml                 | 105 +++++++
 .../launch/b1z1_gazebo.launch                 |  73 +++++
 .../b1z1_description/launch/b1z1_rviz.launch  |  18 ++
 robots/b1z1_description/package.xml           |  12 +
 robots/b1z1_description/xacro/const.xacro     |  17 ++
 robots/b1z1_description/xacro/gazebo.xacro    | 279 ++++++++++++++++++
 robots/b1z1_description/xacro/robot.xacro     |  42 +++
 8 files changed, 550 insertions(+)
 create mode 100644 robots/b1z1_description/CMakeLists.txt
 create mode 100644 robots/b1z1_description/config/robot_control.yaml
 create mode 100644 robots/b1z1_description/launch/b1z1_gazebo.launch
 create mode 100644 robots/b1z1_description/launch/b1z1_rviz.launch
 create mode 100644 robots/b1z1_description/package.xml
 create mode 100644 robots/b1z1_description/xacro/const.xacro
 create mode 100644 robots/b1z1_description/xacro/gazebo.xacro
 create mode 100644 robots/b1z1_description/xacro/robot.xacro

diff --git a/robots/b1z1_description/CMakeLists.txt b/robots/b1z1_description/CMakeLists.txt
new file mode 100644
index 00000000..143bb632
--- /dev/null
+++ b/robots/b1z1_description/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(b1z1_description)
+
+find_package(catkin REQUIRED)
diff --git a/robots/b1z1_description/config/robot_control.yaml b/robots/b1z1_description/config/robot_control.yaml
new file mode 100644
index 00000000..26075320
--- /dev/null
+++ b/robots/b1z1_description/config/robot_control.yaml
@@ -0,0 +1,105 @@
+b1z1_gazebo:
+    # Publish all joint states -----------------------------------
+    joint_state_controller:
+        type: joint_state_controller/JointStateController
+        publish_rate: 1000
+
+    # FL Controllers ---------------------------------------
+    FL_hip_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+    FL_thigh_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    FL_calf_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    # FR Controllers ---------------------------------------
+    FR_hip_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FR_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+    FR_thigh_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FR_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    FR_calf_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: FR_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    # RL Controllers ---------------------------------------
+    RL_hip_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+    RL_thigh_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    RL_calf_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    # RR Controllers ---------------------------------------
+    RR_hip_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RR_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+    RR_thigh_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RR_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    RR_calf_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: RR_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+    # Z1 Controllers ---------------------------------------
+    Joint01_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint1
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    Joint02_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint2
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    Joint03_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint3
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    Joint04_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint4
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    Joint05_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint5
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    Joint06_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: joint6
+        pid: {p: 300.0, i: 0.0, d: 5.0}
+
+    gripper_controller:
+        type: unitree_legged_control/UnitreeJointController
+        joint: jointGripper
+        pid: {p: 300.0, i: 0.0, d: 5.0}
diff --git a/robots/b1z1_description/launch/b1z1_gazebo.launch b/robots/b1z1_description/launch/b1z1_gazebo.launch
new file mode 100644
index 00000000..66071a37
--- /dev/null
+++ b/robots/b1z1_description/launch/b1z1_gazebo.launch
@@ -0,0 +1,73 @@
+<launch>
+    <arg name="wname" default="earth"/>
+    <arg name="rname" default="b1z1"/>
+    <arg name="robot_path" value="(find $(arg rname)_description)"/>
+    <arg name="dollar" value="$"/>
+
+    <arg name="paused" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <arg name="user_debug" default="false"/>
+    <arg name="UnitreeGripperYN" default="true"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find unitree_gazebo)/worlds/$(arg wname).world"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+    <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description"
+           command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
+           DEBUG:=$(arg user_debug) UnitreeGripper:=$(arg UnitreeGripperYN)"/>
+
+    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+    <!-- Set trunk and joint positions at startup -->
+    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
+          args="-urdf -z 0.75 -model $(arg rname)_gazebo -param robot_description -unpause"/>
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
+
+    <!-- load the controllers -->
+    <group unless="$(arg UnitreeGripperYN)">
+        <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+            output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+            Joint01_controller Joint02_controller Joint03_controller
+            Joint04_controller Joint05_controller Joint06_controller
+            FL_hip_controller FL_thigh_controller FL_calf_controller
+            FR_hip_controller FR_thigh_controller FR_calf_controller
+            RL_hip_controller RL_thigh_controller RL_calf_controller
+            RR_hip_controller RR_thigh_controller RR_calf_controller"/>
+    </group>
+
+    <group if="$(arg UnitreeGripperYN)">
+        <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+            output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+            Joint01_controller Joint02_controller Joint03_controller
+            Joint04_controller Joint05_controller Joint06_controller
+            gripper_controller
+            FL_hip_controller FL_thigh_controller FL_calf_controller
+            FR_hip_controller FR_thigh_controller FR_calf_controller
+            RL_hip_controller RL_thigh_controller RL_calf_controller
+            RR_hip_controller RR_thigh_controller RR_calf_controller"/>
+    </group>
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
+    </node>
+
+    <!-- load the parameter unitree_controller -->
+    <include file="$(find unitree_controller)/launch/set_ctrl.launch">
+        <arg name="rname" value="$(arg rname)"/>
+    </include>
+
+</launch>
diff --git a/robots/b1z1_description/launch/b1z1_rviz.launch b/robots/b1z1_description/launch/b1z1_rviz.launch
new file mode 100644
index 00000000..98ae5da3
--- /dev/null
+++ b/robots/b1z1_description/launch/b1z1_rviz.launch
@@ -0,0 +1,18 @@
+<launch>
+
+    <arg name="user_debug" default="false"/>
+    <arg name="UnitreeGripperYN" default="true"/>
+
+    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find b1z1_description)/xacro/robot.xacro'
+            DEBUG:=$(arg user_debug) UnitreeGripper:=$(arg UnitreeGripperYN)"/>
+
+    <node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher"/>
+
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+        <param name="publish_frequency" type="double" value="1000.0"/>
+    </node>
+
+    <node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
+        args="-d $(find b1_description)/launch/check_joint.rviz"/>
+
+</launch>
diff --git a/robots/b1z1_description/package.xml b/robots/b1z1_description/package.xml
new file mode 100644
index 00000000..183ac87c
--- /dev/null
+++ b/robots/b1z1_description/package.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>b1z1_description</name>
+    <version>0.0.0</version>
+    <description>The b1z1_description package</description>
+
+    <maintainer email="b1@unitree.cc">unitree</maintainer>
+    <license>TODO</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+</package>
diff --git a/robots/b1z1_description/xacro/const.xacro b/robots/b1z1_description/xacro/const.xacro
new file mode 100644
index 00000000..b064cd4c
--- /dev/null
+++ b/robots/b1z1_description/xacro/const.xacro
@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<!-- configurable constant parameters for b1z1_description -->
+<robot name="b1z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- include B1 constants -->
+    <xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
+
+    <!-- Z1 mounting point with respect to B1 trunk -->
+    <xacro:property name="arm_offset_x" value="${hip_offset_x}"/>
+    <xacro:property name="arm_offset_y" value="0"/>
+    <xacro:property name="arm_offset_z" value="${trunk_height/2.0 + 0.01}"/>
+    <xacro:property name="arm_offset_r" value="0"/>
+    <xacro:property name="arm_offset_p" value="0"/>
+    <xacro:property name="arm_offset_yaw" value="0"/>
+
+</robot>
diff --git a/robots/b1z1_description/xacro/gazebo.xacro b/robots/b1z1_description/xacro/gazebo.xacro
new file mode 100644
index 00000000..dc9b2ad8
--- /dev/null
+++ b/robots/b1z1_description/xacro/gazebo.xacro
@@ -0,0 +1,279 @@
+<?xml version="1.0"?>
+<robot>
+      <!-- ros_control plugin -->
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/b1z1_gazebo</robotNamespace>
+            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of trunk center. -->
+    <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>10</frequency>
+            <plot>
+                <link>base</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Yellow</material>
+            </plot>
+        </plugin>
+    </gazebo>
+
+    <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
+    <!-- <gazebo>
+        <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
+            <frequency>100</frequency>
+            <plot>
+                <link>FL_foot</link>
+                <pose>0 0 0 0 0 0</pose>
+                <material>Gazebo/Green</material>
+            </plot>
+        </plugin>
+    </gazebo> -->
+
+    <gazebo>
+        <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
+            <bodyName>trunk</bodyName>
+            <topicName>/apply_force/trunk</topicName>
+        </plugin>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <gravity>true</gravity>
+        <sensor name="imu_sensor" type="imu">
+            <always_on>true</always_on>
+            <update_rate>1000</update_rate>
+            <visualize>true</visualize>
+            <topic>__default_topic__</topic>
+            <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+                <topicName>trunk_imu</topicName>
+                <bodyName>imu_link</bodyName>
+                <updateRateHZ>1000.0</updateRateHZ>
+                <gaussianNoise>0.0</gaussianNoise>
+                <xyzOffset>0 0 0</xyzOffset>
+                <rpyOffset>0 0 0</rpyOffset>
+                <frameName>imu_link</frameName>
+            </plugin>
+            <pose>0 0 0 0 0 0</pose>
+        </sensor>
+    </gazebo>
+
+    <!-- Foot contacts. -->
+    <gazebo reference="FR_calf">
+        <sensor name="FR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <sensor name="FL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <sensor name="RR_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <sensor name="RL_foot_contact" type="contact">
+        <update_rate>100</update_rate>
+        <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
+        <contact>
+            <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
+        </contact>
+        </sensor>
+    </gazebo>
+
+    <!-- Visualization of Foot contacts. -->
+    <gazebo reference="FR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>FL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RR_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <visual>
+            <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
+                <topicName>RL_foot_contact</topicName>
+            </plugin>
+        </visual>
+    </gazebo>
+
+    <gazebo reference="base">
+        <material>Gazebo/Green</material>
+        <turnGravityOff>false</turnGravityOff>
+    </gazebo>
+
+    <gazebo reference="trunk">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <gazebo reference="stick_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/White</material>
+    </gazebo>
+
+    <gazebo reference="imu_link">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/Red</material>
+    </gazebo>
+
+    <!-- FL leg -->
+    <gazebo reference="FL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="FL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- FR leg -->
+    <gazebo reference="FR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="FR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="FR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="FR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RL leg -->
+    <gazebo reference="RL_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="RL_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RL_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RL_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- RR leg -->
+    <gazebo reference="RR_hip">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <material>Gazebo/DarkGrey</material>
+    </gazebo>
+    <gazebo reference="RR_thigh">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+    <gazebo reference="RR_calf">
+        <mu1>0.2</mu1>
+        <mu2>0.2</mu2>
+        <self_collide>1</self_collide>
+    </gazebo>
+    <gazebo reference="RR_foot">
+        <mu1>0.6</mu1>
+        <mu2>0.6</mu2>
+        <self_collide>1</self_collide>
+        <material>Gazebo/DarkGrey</material>
+        <kp  value="1000000.0"/>
+        <kd  value="1.0"/>
+    </gazebo>
+
+    <!-- Z1 arm -->
+    <gazebo reference="link04">
+        <self_collide>true</self_collide>
+    </gazebo>
+
+    <gazebo reference="link05">
+        <self_collide>true</self_collide>
+    </gazebo>
+
+    <gazebo reference="link06">
+        <self_collide>true</self_collide>
+    </gazebo>
+
+</robot>
diff --git a/robots/b1z1_description/xacro/robot.xacro b/robots/b1z1_description/xacro/robot.xacro
new file mode 100644
index 00000000..6055ec02
--- /dev/null
+++ b/robots/b1z1_description/xacro/robot.xacro
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+
+<!-- b1z1_description: B1 quadruped mounted with Z1 robotic arm -->
+<robot name="b1z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <!-- define arguments -->
+    <xacro:arg name="DEBUG" default="false"/> <!-- debug mode switch -->
+    <xacro:arg name="UnitreeGripper" default="true"/> <!-- include Z1 end-effector -->
+
+    <!-- include xacro files -->
+    <xacro:include filename="$(find b1_description)/xacro/b1.xacro"/>
+    <xacro:include filename="$(find z1_description)/xacro/z1.xacro"/>
+    <xacro:include filename="$(find b1z1_description)/xacro/const.xacro"/>
+    <xacro:include filename="$(find b1z1_description)/xacro/gazebo.xacro"/>
+
+    <!-- B1 macro: expands to describe B1 quadruped -->
+    <xacro:b1/>
+
+    <!-- Z1 macro: expands to describe Z1 robotic arm -->
+    <xacro:z1 UnitreeGripper="$(arg UnitreeGripper)"/>
+
+    <!-- define joint affixing Z1 to B1 -->
+    <joint name="trunk_arm_joint" type="fixed">
+        <!-- Z1 mounting point wrt B1 trunk; configurable in const.xacro -->
+        <origin rpy="${arm_offset_r} ${arm_offset_p} ${arm_offset_yaw}" xyz="${arm_offset_x} ${arm_offset_y} ${arm_offset_z}"/>
+        <!-- define parent and child links -->
+        <parent link="trunk"/>
+        <child link="link00"/>
+    </joint>
+
+    <!-- debug mode configuration -->
+    <xacro:if value="$(arg DEBUG)">
+        <!-- debug mode hangs up the robot; i.e., statically affixes B1 base to world -->
+        <link name="world"/>
+        <joint name="base_static_joint" type="fixed">
+            <origin rpy="0 0 0" xyz="0 0 0"/>
+            <parent link="world"/>
+            <child link="base"/>
+        </joint>
+    </xacro:if>
+
+</robot>

From 7e00616182aa2e921def3f4e0460968efc94f9a7 Mon Sep 17 00:00:00 2001
From: Paul Blum <paul_d_blum@yahoo.com>
Date: Thu, 2 May 2024 13:29:08 -0400
Subject: [PATCH 4/4] add documentation

---
 robots/b1_description/xacro/b1.xacro       |  1 +
 robots/b1_description/xacro/robot.xacro    | 15 +++++++++------
 robots/b1z1_description/xacro/gazebo.xacro |  5 ++++-
 robots/b1z1_description/xacro/robot.xacro  |  7 ++++---
 robots/z1_description/xacro/robot.xacro    | 14 ++++++++++----
 robots/z1_description/xacro/z1.xacro       |  1 +
 6 files changed, 29 insertions(+), 14 deletions(-)

diff --git a/robots/b1_description/xacro/b1.xacro b/robots/b1_description/xacro/b1.xacro
index 5540992d..d7daaa56 100644
--- a/robots/b1_description/xacro/b1.xacro
+++ b/robots/b1_description/xacro/b1.xacro
@@ -2,6 +2,7 @@
 
 <robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+    <!-- B1 macro: describes Unitree B1 quadruped robot -->
     <xacro:macro name="b1">
 
         <xacro:include filename="$(find b1_description)/xacro/const.xacro"/>
diff --git a/robots/b1_description/xacro/robot.xacro b/robots/b1_description/xacro/robot.xacro
index 70f2aadc..7b2be0bb 100644
--- a/robots/b1_description/xacro/robot.xacro
+++ b/robots/b1_description/xacro/robot.xacro
@@ -1,14 +1,21 @@
 <?xml version="1.0"?>
 
+<!-- b1_description: Unitree B1 quadruped robot -->
 <robot name="b1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:arg name="DEBUG" default="false"/>
+    <!-- define arguments -->
+    <xacro:arg name="DEBUG" default="false"/> <!-- debug mode switch -->
 
+    <!-- include xacro files -->
     <xacro:include filename="$(find b1_description)/xacro/b1.xacro"/>
     <xacro:include filename="$(find b1_description)/xacro/gazebo.xacro"/>
 
-    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <!-- B1 macro: expands to describe B1 quadruped robot -->
+    <xacro:b1/>
+
+    <!-- debug mode configuration -->
     <xacro:if value="$(arg DEBUG)">
+        <!-- statically affix B1 base to world; i.e. "hang up" the robot -->
         <link name="world"/>
         <joint name="base_static_joint" type="fixed">
             <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -17,8 +24,4 @@
         </joint>
     </xacro:if>
 
-    <xacro:b1/>
-
-    <!-- <xacro:stairs stairs="15" xpos="0" ypos="2.0" zpos="0" /> -->
-
 </robot>
diff --git a/robots/b1z1_description/xacro/gazebo.xacro b/robots/b1z1_description/xacro/gazebo.xacro
index dc9b2ad8..4c18e01c 100644
--- a/robots/b1z1_description/xacro/gazebo.xacro
+++ b/robots/b1z1_description/xacro/gazebo.xacro
@@ -1,6 +1,9 @@
 <?xml version="1.0"?>
+
+<!-- Gazebo-specific elements for b1z1_description -->
 <robot>
-      <!-- ros_control plugin -->
+
+    <!-- ros_control plugin -->
     <gazebo>
         <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
             <robotNamespace>/b1z1_gazebo</robotNamespace>
diff --git a/robots/b1z1_description/xacro/robot.xacro b/robots/b1z1_description/xacro/robot.xacro
index 6055ec02..b42f95b3 100644
--- a/robots/b1z1_description/xacro/robot.xacro
+++ b/robots/b1z1_description/xacro/robot.xacro
@@ -13,7 +13,7 @@
     <xacro:include filename="$(find b1z1_description)/xacro/const.xacro"/>
     <xacro:include filename="$(find b1z1_description)/xacro/gazebo.xacro"/>
 
-    <!-- B1 macro: expands to describe B1 quadruped -->
+    <!-- B1 macro: expands to describe B1 quadruped robot -->
     <xacro:b1/>
 
     <!-- Z1 macro: expands to describe Z1 robotic arm -->
@@ -22,7 +22,8 @@
     <!-- define joint affixing Z1 to B1 -->
     <joint name="trunk_arm_joint" type="fixed">
         <!-- Z1 mounting point wrt B1 trunk; configurable in const.xacro -->
-        <origin rpy="${arm_offset_r} ${arm_offset_p} ${arm_offset_yaw}" xyz="${arm_offset_x} ${arm_offset_y} ${arm_offset_z}"/>
+        <origin rpy="${arm_offset_r} ${arm_offset_p} ${arm_offset_yaw}"
+            xyz="${arm_offset_x} ${arm_offset_y} ${arm_offset_z}"/>
         <!-- define parent and child links -->
         <parent link="trunk"/>
         <child link="link00"/>
@@ -30,7 +31,7 @@
 
     <!-- debug mode configuration -->
     <xacro:if value="$(arg DEBUG)">
-        <!-- debug mode hangs up the robot; i.e., statically affixes B1 base to world -->
+        <!-- statically affix B1 base to world; i.e. "hang up" the robot -->
         <link name="world"/>
         <joint name="base_static_joint" type="fixed">
             <origin rpy="0 0 0" xyz="0 0 0"/>
diff --git a/robots/z1_description/xacro/robot.xacro b/robots/z1_description/xacro/robot.xacro
index 184d6244..ac18d224 100644
--- a/robots/z1_description/xacro/robot.xacro
+++ b/robots/z1_description/xacro/robot.xacro
@@ -1,20 +1,26 @@
 <?xml version="1.0"?>
 
+<!-- z1_description: Unitree Z1 robotic arm -->
 <robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <xacro:arg name="UnitreeGripper" default="false"/>
+    <!-- define arguments -->
+    <xacro:arg name="UnitreeGripper" default="false"/> <!-- include default end-effector -->
 
+    <!-- include xacro files -->
     <xacro:include filename="$(find z1_description)/xacro/z1.xacro"/>
     <xacro:include filename="$(find z1_description)/xacro/gazebo.xacro"/>
 
-    <link name="world"/>
+    <!-- Z1 macro: expands to describe Z1 robotic arm -->
+    <xacro:z1 UnitreeGripper="$(arg UnitreeGripper)"/>
 
+    <!-- define world link and statically affix Z1 -->
+    <link name="world"/>
     <joint name="base_static_joint" type="fixed">
+        <!-- Z1 mounting point wrt origin (configurable) -->
         <origin rpy="0 0 0" xyz="0 0 0"/>
+        <!-- define parent and child links -->
         <parent link="world"/>
         <child link="link00"/>
     </joint>
 
-    <xacro:z1 UnitreeGripper="$(arg UnitreeGripper)"/>
-
 </robot>
diff --git a/robots/z1_description/xacro/z1.xacro b/robots/z1_description/xacro/z1.xacro
index 2497df6f..7b61b880 100644
--- a/robots/z1_description/xacro/z1.xacro
+++ b/robots/z1_description/xacro/z1.xacro
@@ -2,6 +2,7 @@
 
 <robot name="z1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+    <!-- Z1 macro: describes Unitree Z1 robotic arm -->
     <xacro:macro name="z1" params="UnitreeGripper:=false">
 
         <xacro:include filename="$(find z1_description)/xacro/const.xacro"/>