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 1/3] 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>

From fb111da6bcd68dc4a804a3898842af13e0b7a2df Mon Sep 17 00:00:00 2001
From: Ilua Uraev <ur.narmak@gmail.com>
Date: Tue, 3 Jun 2025 15:50:01 +0300
Subject: [PATCH 2/3] feat: add second camera

---
 config/gz_bridge.yaml   | 7 ++++++-
 urdf/aubo_i5.urdf.xacro | 8 ++++++--
 urdf/sensors.xacro      | 4 ++--
 3 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/config/gz_bridge.yaml b/config/gz_bridge.yaml
index f0cf516..6924d6c 100644
--- a/config/gz_bridge.yaml
+++ b/config/gz_bridge.yaml
@@ -3,7 +3,12 @@
   gz_type_name: "gz.msgs.Clock"
   direction: GZ_TO_ROS
 
-- topic_name: "/rgbd_camera/image"
+- topic_name: "/inner_rgbd_camera/image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- topic_name: "/outer_rgbd_camera/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
diff --git a/urdf/aubo_i5.urdf.xacro b/urdf/aubo_i5.urdf.xacro
index d099f34..87eec1e 100644
--- a/urdf/aubo_i5.urdf.xacro
+++ b/urdf/aubo_i5.urdf.xacro
@@ -251,8 +251,12 @@
   </xacro:simple_parallel_gripper>
 
 
-  <xacro:rgbd parent="wrist3_Link" tf_prefix="">
-    <origin xyz="0.0 0.05 0.04" rpy="0.0 -1.47 -1.57" />
+  <xacro:rgbd parent="wrist3_Link" tf_prefix="inner_">
+    <origin xyz="0.0 0.05 0.0" rpy="0.0 -1.47 -1.57" />
+  </xacro:rgbd>
+
+  <xacro:rgbd parent="base_link" tf_prefix="outer_">
+    <origin xyz="0.62179261445999146 -0.83430188894271851 0.85204130411148071" rpy="0.00 0.608 1.934" />
   </xacro:rgbd>
 
   <xacro:aubo_ros2_control hardware="${hardware}" robot_ip="${robot_ip}" />
diff --git a/urdf/sensors.xacro b/urdf/sensors.xacro
index 54fb9ac..fec0b7f 100644
--- a/urdf/sensors.xacro
+++ b/urdf/sensors.xacro
@@ -30,8 +30,8 @@
         <camera>
           <horizontal_fov>1.047</horizontal_fov>
           <image>
-            <width>1920</width>
-            <height>1080</height>
+            <width>640</width>
+            <height>480</height>
           </image>
           <clip>
             <near>0.1</near>

From 5d162fbaeca5cf9d942a112beeedb4fc4b0c6390 Mon Sep 17 00:00:00 2001
From: Ilua Uraev <ur.narmak@gmail.com>
Date: Tue, 3 Jun 2025 15:50:18 +0300
Subject: [PATCH 3/3] refactor: update cylinder size in scene

---
 world/world.sdf | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/world/world.sdf b/world/world.sdf
index 1f4c132..6fda47d 100644
--- a/world/world.sdf
+++ b/world/world.sdf
@@ -110,8 +110,8 @@
         <collision name="cylinder_collision">
           <geometry>
             <cylinder>
-              <radius>0.03</radius>
-              <length>0.1</length>
+              <radius>0.01</radius>
+              <length>0.05</length>
             </cylinder>
           </geometry>
           <surface>
@@ -133,8 +133,8 @@
         <visual name="cylinder_visual">
           <geometry>
             <cylinder>
-              <radius>0.03</radius>
-              <length>0.1</length>
+              <radius>0.01</radius>
+              <length>0.05</length>
             </cylinder>
           </geometry>
           <material>