From a10ded747a80dcd1cb9db6f04f8b45f8733761d3 Mon Sep 17 00:00:00 2001
From: Ilua Uraev <ur.narmak@gmail.com>
Date: Tue, 3 Jun 2025 00:02:23 +0300
Subject: [PATCH] feat: add table and initial position params

---
 urdf/aubo_i5.urdf.xacro | 12 ++++---
 urdf/ros2_control.xacro | 51 ++++++++++++++++++++++----
 world/world.sdf         | 80 ++++++++++++++++++++++++++++++++++++++++-
 3 files changed, 130 insertions(+), 13 deletions(-)

diff --git a/urdf/aubo_i5.urdf.xacro b/urdf/aubo_i5.urdf.xacro
index 93c177f..d099f34 100644
--- a/urdf/aubo_i5.urdf.xacro
+++ b/urdf/aubo_i5.urdf.xacro
@@ -11,6 +11,7 @@
   <xacro:include filename="$(find dh_ag95_description)/urdf/dh_ag95_macro.urdf.xacro" />
   <xacro:include filename="$(find dh_ag145_description)/urdf/dh_ag145_macro.urdf.xacro" />
   <xacro:include filename="$(find aubo_description)/urdf/sensors.xacro" />
+  <xacro:include filename="$(find simple_gz_gripper)/urdf/simple_gripper_macro.xacro"/>
 
   <xacro:property name="hardware" value="$(arg hardware)"/>
   <xacro:property name="robot_ip" value="$(arg robot_ip)"/>
@@ -242,11 +243,12 @@
     <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
   </joint>
 
-  <!-- <xacro:if value="${with_gripper=='true'}"> -->
-    <xacro:dh_ag145_gripper prefix="" parent="wrist3_Link">
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-    </xacro:dh_ag145_gripper>
-  <!-- </xacro:if> -->
+    <!-- <xacro:dh_ag145_gripper prefix="" parent="wrist3_Link"> -->
+    <!--   <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> -->
+    <!-- </xacro:dh_ag145_gripper> -->
+  <xacro:simple_parallel_gripper prefix="" parent="wrist3_Link">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 1.57" />
+  </xacro:simple_parallel_gripper>
 
 
   <xacro:rgbd parent="wrist3_Link" tf_prefix="">
diff --git a/urdf/ros2_control.xacro b/urdf/ros2_control.xacro
index 34ba3ff..2827d67 100644
--- a/urdf/ros2_control.xacro
+++ b/urdf/ros2_control.xacro
@@ -4,7 +4,7 @@
     <ros2_control name="aubo_hardware_interface" type="system">
       <hardware>
         <xacro:if value="${hardware=='gazebo'}">
-        <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </xacro:if>
         <xacro:if value="${hardware=='mock'}">
           <plugin>mock_components/GenericSystem</plugin>
@@ -19,7 +19,9 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <state_interface name="velocity" />
       </joint>
 
@@ -28,7 +30,14 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <xacro:if value="${hardware=='gazebo'}">
+          <state_interface name="position">
+            <param name="initial_value">0.5</param>
+          </state_interface>
+        </xacro:if>
+        <xacro:if value="${hardware=='real' or hardware=='mock'}">
+          <state_interface name="position" />
+        </xacro:if>
         <state_interface name="velocity" />
       </joint>
 
@@ -37,7 +46,14 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <xacro:if value="${hardware=='gazebo'}">
+          <state_interface name="position">
+            <param name="initial_value">1.75</param>
+          </state_interface>
+        </xacro:if>
+        <xacro:if value="${hardware=='real' or hardware=='mock'}">
+          <state_interface name="position" />
+        </xacro:if>
         <state_interface name="velocity" />
       </joint>
 
@@ -45,7 +61,14 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <xacro:if value="${hardware=='gazebo'}">
+          <state_interface name="position">
+            <param name="initial_value">0.0</param>
+          </state_interface>
+        </xacro:if>
+        <xacro:if value="${hardware=='real' or hardware=='mock'}">
+          <state_interface name="position" />
+        </xacro:if>
         <state_interface name="velocity" />
       </joint>
 
@@ -53,7 +76,14 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <xacro:if value="${hardware=='gazebo'}">
+          <state_interface name="position">
+            <param name="initial_value">1.57</param>
+          </state_interface>
+        </xacro:if>
+        <xacro:if value="${hardware=='real' or hardware=='mock'}">
+          <state_interface name="position" />
+        </xacro:if>
         <state_interface name="velocity" />
       </joint>
 
@@ -61,7 +91,14 @@
         <command_interface name="position" />
         <command_interface name="velocity" />
 
-        <state_interface name="position" />
+        <xacro:if value="${hardware=='gazebo'}">
+          <state_interface name="position">
+            <param name="initial_value">0.0</param>
+          </state_interface>
+        </xacro:if>
+        <xacro:if value="${hardware=='real' or hardware=='mock'}">
+          <state_interface name="position" />
+        </xacro:if>
         <state_interface name="velocity" />
       </joint>
 
diff --git a/world/world.sdf b/world/world.sdf
index 1ec27bd..1f4c132 100644
--- a/world/world.sdf
+++ b/world/world.sdf
@@ -1,4 +1,4 @@
-<?xml version="1.0" ?>
+<?xml version="1.0"?>
 <sdf version="1.6">
   <world name="aubo-world">
     <physics name="1ms" type="dart">
@@ -66,5 +66,83 @@
       </link>
     </model>
 
+    <model name="table">
+      <static>true</static>
+      <pose>0.5 0 0.1 0 0 0</pose>
+      <link name="cylinder_link">
+        <inertial auto="true" />
+        <collision name="table_collision">
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.2</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="table_visual">
+          <geometry>
+            <box>
+              <size>0.3 0.3 0.2</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.6 0.3 0.3 1</ambient>
+            <diffuse>0.6 0.3 0.3 1</diffuse>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <model name="simple_cylinder">
+      <pose>0.5 0 0.252 0 0 0</pose>
+      <link name="cylinder_link">
+        <inertial>
+          <mass>0.05</mass>
+          <pose>0 0 0 0 0 0</pose>
+          <inertia>
+            <ixx>3.2083e-05</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>3.2083e-05</iyy>
+            <iyz>0</iyz>
+            <izz>1.0e-05</izz>
+          </inertia>
+        </inertial>
+
+        <collision name="cylinder_collision">
+          <geometry>
+            <cylinder>
+              <radius>0.03</radius>
+              <length>0.1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <contact>
+              <ode>
+                <min_depth>0.001</min_depth>
+                <max_vel>0.01</max_vel>
+              </ode>
+            </contact>
+            <friction>
+              <ode>
+                <mu>10.0</mu>
+                <mu2>10.0</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+
+        <visual name="cylinder_visual">
+          <geometry>
+            <cylinder>
+              <radius>0.03</radius>
+              <length>0.1</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.3 0.3 0.6 1</ambient>
+            <diffuse>0.3 0.3 0.6 1</diffuse>
+          </material>
+        </visual>
+      </link>
+    </model>
   </world>
 </sdf>