From b2cf8dfa7b0807ae09d2dc9f492dc6799e665772 Mon Sep 17 00:00:00 2001
From: Raffaello Bonghi <raffaello@rnext.it>
Date: Sun, 13 Oct 2024 18:12:40 +0100
Subject: [PATCH] Improve model and mecanum description for simulation

---
 .../launch/description.launch.py              |   8 -
 nanosaur_description/launch/display.launch.py |   9 +-
 nanosaur_description/urdf/base.urdf.xacro     | 162 +++++++++++++
 .../urdf/materials.gazebo.xacro               |  77 ------
 nanosaur_description/urdf/materials.urdf      |  31 ---
 nanosaur_description/urdf/mecanum.urdf.xacro  |  67 ++++++
 nanosaur_description/urdf/nanosaur.urdf.xacro |  15 +-
 .../urdf/omniwheel.urdf.xacro                 | 117 ---------
 .../urdf/properties.urdf.xacro                |  28 +++
 nanosaur_description/urdf/robot.urdf.xacro    | 222 +++++-------------
 .../urdf/utilities.urdf.xacro                 |  64 +++++
 11 files changed, 384 insertions(+), 416 deletions(-)
 create mode 100644 nanosaur_description/urdf/base.urdf.xacro
 delete mode 100644 nanosaur_description/urdf/materials.gazebo.xacro
 delete mode 100644 nanosaur_description/urdf/materials.urdf
 create mode 100644 nanosaur_description/urdf/mecanum.urdf.xacro
 delete mode 100644 nanosaur_description/urdf/omniwheel.urdf.xacro
 create mode 100644 nanosaur_description/urdf/properties.urdf.xacro
 create mode 100644 nanosaur_description/urdf/utilities.urdf.xacro

diff --git a/nanosaur_description/launch/description.launch.py b/nanosaur_description/launch/description.launch.py
index f35fc3e..e5c118e 100644
--- a/nanosaur_description/launch/description.launch.py
+++ b/nanosaur_description/launch/description.launch.py
@@ -43,7 +43,6 @@ def launch_setup(context: LaunchContext, support_package):
     xacro_path = LaunchConfiguration('xacro_path')
     head_type = LaunchConfiguration('head_type')
     flap_type = LaunchConfiguration('flap_type')
-    diff_drive_emulation = LaunchConfiguration('diff_drive_emulation')
     use_nominal_extrinsics = LaunchConfiguration('use_nominal_extrinsics')
 
     robot_state_publisher_node = Node(
@@ -56,7 +55,6 @@ def launch_setup(context: LaunchContext, support_package):
                     'xacro ', xacro_path, ' ',
                     'head_type:=', head_type, ' ',
                     'flap_type:=', flap_type, ' ',
-                    'diff_drive_emulation:=', diff_drive_emulation, ' ',
                     'use_nominal_extrinsics:=', use_nominal_extrinsics, ' ',
                 ])
         }]
@@ -95,11 +93,6 @@ def generate_launch_description():
         default_value='empty',
         description='Flap type to use. Options: empty, LD06.')
 
-    declare_simulation_cmd = DeclareLaunchArgument(
-        name='diff_drive_emulation',
-        default_value='false',
-        description='Enable urdf for differential drive emulation, for simulation.')
-
     declare_use_nominal_extrinsics_cmd = DeclareLaunchArgument(
         name='use_nominal_extrinsics',
         default_value='false',
@@ -109,7 +102,6 @@ def generate_launch_description():
     ld = LaunchDescription()    
     ld.add_action(nanosaur_cmd)
     ld.add_action(declare_model_path_cmd)
-    ld.add_action(declare_simulation_cmd)
     ld.add_action(declare_head_type_cmd)
     ld.add_action(declare_flap_type_cmd)
     ld.add_action(declare_use_nominal_extrinsics_cmd)
diff --git a/nanosaur_description/launch/display.launch.py b/nanosaur_description/launch/display.launch.py
index 1bd89b2..60f560e 100644
--- a/nanosaur_description/launch/display.launch.py
+++ b/nanosaur_description/launch/display.launch.py
@@ -41,7 +41,6 @@ def generate_launch_description():
     gui = LaunchConfiguration('gui')
     head_type = LaunchConfiguration('head_type')
     flap_type = LaunchConfiguration('flap_type')
-    diff_drive_emulation = LaunchConfiguration('diff_drive_emulation')
     rvizconfig = LaunchConfiguration('rvizconfig')
     
     default_rviz_config_path = os.path.join(nanosaur_description_path, 'rviz', 'urdf.rviz')
@@ -56,11 +55,6 @@ def generate_launch_description():
         default_value='empty',
         description='Flap type to use. Options: empty, LD06.')
 
-    declare_simulation_cmd = DeclareLaunchArgument(
-        name='diff_drive_emulation',
-        default_value='false',
-        description='Enable urdf for differential drive emulation, for simulation.')
-
     declare_gui_cmd = DeclareLaunchArgument(
         name='gui',
         default_value='True',
@@ -97,7 +91,7 @@ def generate_launch_description():
     # https://answers.ros.org/question/306935/ros2-include-a-launch-file-from-a-launch-file/
     description_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nanosaur_description_path, '/launch/description.launch.py']),
-        launch_arguments={'head_type': head_type, 'flap_type': flap_type, 'diff_drive_emulation': diff_drive_emulation}.items()
+        launch_arguments={'head_type': head_type, 'flap_type': flap_type}.items()
         )
 
     # Define LaunchDescription variable and return it
@@ -105,7 +99,6 @@ def generate_launch_description():
     
     ld.add_action(declare_head_type_cmd)
     ld.add_action(declare_flap_type_cmd)
-    ld.add_action(declare_simulation_cmd)
     ld.add_action(declare_gui_cmd)
     ld.add_action(declare_rvizconfig_cmd)
     ld.add_action(description_launch)
diff --git a/nanosaur_description/urdf/base.urdf.xacro b/nanosaur_description/urdf/base.urdf.xacro
new file mode 100644
index 0000000..991267c
--- /dev/null
+++ b/nanosaur_description/urdf/base.urdf.xacro
@@ -0,0 +1,162 @@
+<?xml version="1.0"?>
+<robot name="nanosaur_base" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:macro name="nanosaur_base" params="base_name length width height mass">
+
+        <link name="${base_name}">
+            <collision>
+                <origin xyz="${nanosaur_body_offset_x} 0 ${height / 2 - nanosaur_wheel_offset_z}" rpy="0 0 0"/>
+                <geometry>
+                    <box size="${length} ${width} ${height}" />
+                </geometry>
+            </collision>
+
+            <xacro:inertial_box m="${mass}" x="${length}" y="${width}" z="${height}">
+                <origin xyz="${nanosaur_body_offset_x} 0 ${height / 2 - nanosaur_wheel_offset_z}" rpy="0 0 0"/>
+            </xacro:inertial_box>
+        </link>
+
+        <joint name="nansosaur_chassis_joint" type="fixed">
+            <origin xyz="0 0 ${0.042 - nanosaur_wheel_offset_z}" rpy="0 0 0"/>
+            <parent link="${base_name}"/>
+            <child link="nanosaur_chassis"/>
+        </joint>
+
+        <link name="nanosaur_chassis">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry><mesh filename="${path_meshes}/nanosaur_base.stl" scale="0.001 0.001 0.001"/></geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <joint name="nanosaur_cover_batteries_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="nanosaur_chassis"/>
+            <child link="nanosaur_cover_batteries"/>
+        </joint>
+
+        <link name="nanosaur_cover_batteries">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry><mesh filename="${path_meshes}/nanosaur_cover_batteries.stl" scale="0.001 0.001 0.001"/></geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <joint name="nanosaur_cover_electronics_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="nanosaur_chassis"/>
+            <child link="nanosaur_cover_electronics"/>
+        </joint>
+
+        <link name="nanosaur_cover_electronics">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+                <geometry><mesh filename="${path_meshes}/nanosaur_cover_electronics.stl" scale="0.001 0.001 0.001"/></geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <joint name="NVIDIA_jetson_joint" type="fixed">
+            <origin xyz="0.056 0.0078 0.0085" rpy="0 0 0"/>
+            <parent link="nanosaur_chassis"/>
+            <child link="NVIDIA_jetson"/>
+        </joint>
+
+        <link name="NVIDIA_jetson">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 ${M_PI}" />
+                <geometry><mesh filename="${path_meshes}/parts/NVIDIA_Jetson_Orin_Nano_DevKit.stl"/></geometry>
+                <material name="DarkGrey" />
+            </visual>
+        </link>
+
+        <joint name="motor2040_joint" type="fixed">
+            <origin xyz="0 0 -0.035" rpy="0 ${M_PI} 0"/>
+            <parent link="nanosaur_chassis"/>
+            <child link="motor2040"/>
+        </joint>
+
+        <link name="motor2040">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry><mesh filename="${path_meshes}/parts/motor2040.stl"/></geometry>
+                <material name="DarkGrey" />
+            </visual>
+        </link>
+
+
+        <joint name="nanosaur_PCB_electronics_joint" type="fixed">
+            <origin xyz="-0.046 -0.015 0.0225" rpy="0 0 0" />
+            <parent link="nanosaur_chassis"/>
+            <child link="nanosaur_PCB_electronics"/>
+        </joint>
+
+        <link name="nanosaur_PCB_electronics">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry><mesh filename="${path_meshes}/parts/nanosaur_PCB_electronics.stl"/></geometry>
+                <material name="DarkGrey" />
+            </visual>
+        </link>
+
+        <joint name="nanosaur_PCB_oleds_joint" type="fixed">
+            <origin xyz="0.053 0 0.037" rpy="0 ${M_PI / 5.5} 0" />
+            <parent link="nanosaur_chassis"/>
+            <child link="nanosaur_PCB_oleds"/>
+        </joint>
+
+        <link name="nanosaur_PCB_oleds">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 ${M_PI / 2}" />
+                <geometry><mesh filename="${path_meshes}/parts/nanosaur_PCB_oleds.stl"/></geometry>
+                <material name="DarkGrey" />
+            </visual>
+        </link>
+
+        <joint name="display_left_joint" type="fixed">
+            <origin xyz="-0.0122 0.021 0.003" rpy="0 0 0" />
+            <parent link="nanosaur_PCB_oleds"/>
+            <child link="display_left"/>
+        </joint>
+
+        <link name="display_left">
+            <inertial>
+                <mass value="0.01"/>
+                <origin rpy="0 0 0" xyz="0 0 0"/>
+                <inertia ixx="1.6666666666666668e-07" ixy="0" ixz="0" iyy="1.6666666666666668e-07" iyz="0" izz="1.6666666666666668e-07"/>
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}" />
+                <geometry>
+                    <mesh filename="${path_meshes}/parts/OLED_Module.stl"/>
+                </geometry>
+                <material name="Black" />
+            </visual>
+        </link>
+
+        <joint name="display_right_joint" type="fixed">
+            <origin xyz="-0.0122 -0.021 0.003" rpy="0 0 0" />
+            <parent link="nanosaur_PCB_oleds"/>
+            <child link="display_right"/>
+        </joint>
+
+        <link name="display_right">
+            <inertial>
+                <mass value="0.01"/>
+                <origin rpy=" 0 0 0" xyz="0 0 0"/>
+                <inertia ixx="1.6666666666666668e-07" ixy="0" ixz="0" iyy="1.6666666666666668e-07" iyz="0" izz="1.6666666666666668e-07"/>
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}" />
+                <geometry>
+                    <mesh filename="${path_meshes}/parts/OLED_Module.stl"/>
+                </geometry>
+                <material name="Black" />
+            </visual>
+        </link>
+
+    </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/materials.gazebo.xacro b/nanosaur_description/urdf/materials.gazebo.xacro
deleted file mode 100644
index 28f2c2f..0000000
--- a/nanosaur_description/urdf/materials.gazebo.xacro
+++ /dev/null
@@ -1,77 +0,0 @@
-<?xml version="1.0"?>
-<robot name="nanosaur_gazebo_materials"
-    xmlns:xacro="http://ros.org/wiki/xacro">
-
-    <!-- Gazebo color reference https://bitbucket.org/osrf/gazebo/src/default/media/materials/scripts/gazebo.material?fileviewer=file-view-default
-    Gazebo http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials
-    Ignition https://gazebosim.org/api/gazebo/3.7/migrationsdf.html
-    -->
-
-    <gazebo reference="base_front">
-        <material>Gazebo/Green</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.0 0.5 0.0 1.0</ambient>
-                <diffuse>0.0 0.5 0.0 1.0</diffuse>
-                <specular>0.0 0.5 0.0 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-    <gazebo reference="base_rear">
-        <material>Gazebo/Green</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.0 0.5 0.0 1.0</ambient>
-                <diffuse>0.0 0.5 0.0 1.0</diffuse>
-                <specular>0.0 0.5 0.0 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-    <gazebo reference="motor_board">
-        <material>Gazebo/DarkGrey</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.2 0.2 0.2 1.0</ambient>
-                <diffuse>0.2 0.2 0.2 1.0</diffuse>
-                <specular>0.2 0.2 0.2 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-    <gazebo reference="power_bank">
-        <material>Gazebo/DarkGrey</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.2 0.2 0.2 1.0</ambient>
-                <diffuse>0.2 0.2 0.2 1.0</diffuse>
-                <specular>0.2 0.2 0.2 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-    <gazebo reference="pb_holder">
-        <material>Gazebo/Green</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.0 0.5 0.0 1.0</ambient>
-                <diffuse>0.0 0.5 0.0 1.0</diffuse>
-                <specular>0.0 0.5 0.0 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-    <gazebo reference="NVIDIA_jetson">
-        <material>Gazebo/DarkGrey</material>
-        <!-- Ignition color reference -->
-        <visual>
-            <material>
-                <ambient>0.2 0.2 0.2 1.0</ambient>
-                <diffuse>0.2 0.2 0.2 1.0</diffuse>
-                <specular>0.2 0.2 0.2 1.0</specular>
-            </material>
-        </visual>
-    </gazebo>
-
-</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/materials.urdf b/nanosaur_description/urdf/materials.urdf
deleted file mode 100644
index 711fcfe..0000000
--- a/nanosaur_description/urdf/materials.urdf
+++ /dev/null
@@ -1,31 +0,0 @@
-<robot name="materials">
-
-  <material name="Black">
-    <color rgba="0.1 0.1 0.1 1.0"/>
-  </material>
-
-  <material name="Green">
-    <color rgba="0.0 0.8 0.0 1.0"/>
-  </material>
-
-  <material name="aluminum">
-    <color rgba="0.5 0.5 0.5 1.0"/>
-  </material>
-
-  <material name="DarkGrey">
-    <color rgba="0.3 0.3 0.3 1.0"/>
-  </material>
-
-  <material name="GunMetalGrey">
-    <color rgba="0.46 0.5 0.56 1.0"/>
-  </material>
-
-  <material name="Gold">
-    <color rgba="1 1 0.752941176470588 1" />
-  </material>
-
-  <material name="zedm_mat">
-    <color rgba="0 0 0 0.9"/>
-  </material>
-
-</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/mecanum.urdf.xacro b/nanosaur_description/urdf/mecanum.urdf.xacro
new file mode 100644
index 0000000..3bfef1d
--- /dev/null
+++ b/nanosaur_description/urdf/mecanum.urdf.xacro
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot name="mecanum_wheel" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:macro name="mecanum_wheel" params="position side parent radius length xyz mass mu">
+
+        <!-- Define orientation -->
+        <xacro:if value="${side == 'left'}">
+            <xacro:property name="orientation" value="-1"/>
+        </xacro:if>
+        <xacro:if value="${side == 'right'}">
+            <xacro:property name="orientation" value="1"/>
+        </xacro:if>
+
+        <link name="${position}_${side}_mecanum_link">
+            <visual>
+                <origin xyz="0 ${orientation * (length / 2 - 0.002)} 0" rpy="${orientation * M_PI/2} 0 0" />
+                <geometry>
+                    <mesh filename="${path_meshes}/parts/mecanum_cover.stl"/>
+                </geometry>
+                <material name="Black" />
+            </visual>
+
+            <collision>
+                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0"/>
+                <geometry>
+                    <cylinder radius="${radius}" length="${length}"/>
+                </geometry>
+            </collision>
+
+            <xacro:inertial_cylinder m="${mass}" r="${radius}" l="${length}">
+                <origin xyz="0 0 0" rpy="0 0 0"/>
+            </xacro:inertial_cylinder>
+        </link>
+
+        <gazebo reference="${position}_${side}_mecanum_link">
+            <mu1>${mu}</mu1>
+            <mu2>${mu}</mu2>
+        </gazebo>
+
+        <joint name="${position}_${side}_mecanum_joint" type="continuous">
+            <axis xyz="0 1 0" />
+            <parent link="${parent}"/>
+            <child link="${position}_${side}_mecanum_link"/>
+            <origin xyz="${xyz}" rpy="0 0 0"/>
+        </joint>
+
+        <!-- Visual effect miniwheels in green -->
+        <link name="${position}_${side}_mecanum_wheels_link">
+            <visual>
+                <origin xyz="0 ${orientation * (length / 2 - 0.002)} 0" rpy="${orientation * M_PI/2} 0 0" />
+                <geometry>
+                    <mesh filename="${path_meshes}/parts/mecanum_wheels.stl"/>
+                </geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <joint name="${position}_${side}_mecanum_wheels_link_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="${position}_${side}_mecanum_link"/>
+            <child link="${position}_${side}_mecanum_wheels_link"/>
+            <axis xyz="0 1 0" />
+        </joint>
+
+    </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/nanosaur.urdf.xacro b/nanosaur_description/urdf/nanosaur.urdf.xacro
index 9ab6ebc..b97ea7f 100644
--- a/nanosaur_description/urdf/nanosaur.urdf.xacro
+++ b/nanosaur_description/urdf/nanosaur.urdf.xacro
@@ -4,27 +4,14 @@
     <!-- nanosaur parts -->
     <xacro:include filename="$(find nanosaur_description)/urdf/robot.urdf.xacro" />
 
-    <!-- Enable simulation mode.
-         Add extra wheel and center sprocket for differential drive simulation -->
-    <xacro:arg name="diff_drive_emulation" default="false" />
     <!-- camera definitions -->
     <xacro:arg name="head_type" default="empty" /> <!--options: empty, realsense, zedmini -->
     <xacro:arg name="flap_type" default="empty" /> <!--options: empty, LD06 -->
     <xacro:arg name="use_nominal_extrinsics" default="false" /> <!-- ONLY FOR REALSENSE -->
 
-     <!-- nanosaur definition -->
-     <link name="base_link" />
-
-     <joint name="nanosaur_base_joint" type="fixed">
-          <origin xyz="0 0 0.05" rpy="0 0 0" />
-          <parent link="base_link"/>
-          <child link="nanosaur_base"/>
-     </joint>
-
     <!-- nanosaur robot -->
     <xacro:nanosaur head_type="$(arg head_type)"
                     flap_type="$(arg flap_type)"
-                    use_nominal_extrinsics="$(arg use_nominal_extrinsics)"
-                    diff_drive_emulation="$(arg diff_drive_emulation)" />
+                    use_nominal_extrinsics="$(arg use_nominal_extrinsics)" />
 
 </robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/omniwheel.urdf.xacro b/nanosaur_description/urdf/omniwheel.urdf.xacro
deleted file mode 100644
index 77c01b9..0000000
--- a/nanosaur_description/urdf/omniwheel.urdf.xacro
+++ /dev/null
@@ -1,117 +0,0 @@
-<?xml version="1.0"?>
-<robot name="omniwheel"
-    xmlns:xacro="http://ros.org/wiki/xacro">
-
-
-    <xacro:macro name="omniwheel" params="parent type position xyz">
-
-        <!-- Define orientation -->
-        <xacro:if value="${type == 'left'}">
-            <xacro:property name="orientation" value="-1"/>
-        </xacro:if>
-        <xacro:if value="${type == 'right'}">
-            <xacro:property name="orientation" value="1"/>
-        </xacro:if>
-        <!-- Define orientation -->
-        <xacro:if value="${position == 'front'}">
-            <xacro:property name="motor_orientation" value="1"/>
-            <xacro:property name="fdir1_direction" value="-1"/>
-        </xacro:if>
-        <xacro:if value="${position == 'rear'}">
-            <xacro:property name="motor_orientation" value="0"/>
-            <xacro:property name="fdir1_direction" value="1"/>
-        </xacro:if>
-        <!--radius definition-->
-        <xacro:property name="omni_wheel_mass" value="0.015"/>
-        <xacro:property name="omni_wheel_radius" value="${0.037 / 2}"/>
-        <xacro:property name="omni_wheel_height" value="0.0226"/>
-
-        <xacro:macro name="inertial_cylinder" params="m r h">
-        <inertia  ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
-                    iyy="${m*r*r/2}" iyz = "0"
-                    izz="${m*(3*r*r+h*h)/12}" />
-        </xacro:macro>
-
-        <xacro:macro name="inertial_sphere" params="m r">
-        <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0"
-                    iyy="${(2/5) * m * (r*r)}" iyz="0.0"
-                izz="${(2/5) * m * (r*r)}" />
-        </xacro:macro>  
-
-        <joint name="motor_${type}_${position}_joint" type="fixed">
-            <origin xyz="${xyz}" rpy="${orientation * M_PI/2} 0 0" />
-            <parent link="${parent}"/>
-            <child link="motor_${type}_${position}"/>
-        </joint>
-
-        <link name="motor_${type}_${position}">
-            <visual>
-                <origin xyz="0 0 -0.024" rpy="0 0 ${motor_orientation * M_PI}" />
-                <geometry>
-                    <mesh filename="${path_meshes}/parts/motor_block.stl"/>
-                </geometry>
-                <material name="Gold" />
-            </visual>
-        </link>
-
-        <joint name="omniwheel_${type}_${position}_joint" type="continuous">
-            <origin xyz="0 0 ${omni_wheel_height / 2}" rpy="0 0 0" />
-            <parent link="motor_${type}_${position}"/>
-            <child link="omniwheel_${type}_${position}"/>
-            <axis xyz="0 0 1" />
-            <limit effort="1000" velocity="1000"/>
-        </joint>
-
-        <link name="omniwheel_${type}_${position}">
-            <visual>
-                <origin xyz="0 0 -${omni_wheel_radius / 2}" rpy="0 0 0" />
-                <geometry>
-                    <mesh filename="${path_meshes}/parts/mecanum_cover.stl"/>
-                </geometry>
-                <material name="Black" />
-            </visual>
-            <!-- Collision cylinder -->
-            <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry>
-                    <!--<sphere radius="${omni_wheel_radius}" />-->
-                    <cylinder length="${omni_wheel_height}" radius="${omni_wheel_radius}"/>
-                </geometry>
-                <material name="Black" />
-            </collision>
-            <!-- Inertia -->
-            <inertial>
-                <mass value="${omni_wheel_mass}"/>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <!--<xacro:inertial_sphere  m="${omni_wheel_mass}" r="${omni_wheel_radius}" />-->
-                <xacro:inertial_cylinder  m="${omni_wheel_mass}" r="${omni_wheel_radius}" h="${omni_wheel_height}" />
-            </inertial>
-        </link>
-
-        <joint name="omniwheel_${type}_${position}_wheels_joint" type="fixed">
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <parent link="omniwheel_${type}_${position}"/>
-            <child link="omniwheel_${type}_${position}_wheels"/>
-            <axis xyz="0 0 1" />
-        </joint>
-
-        <link name="omniwheel_${type}_${position}_wheels">
-            <visual>
-                <origin xyz="0 0 -${omni_wheel_radius / 2}" rpy="0 0 0" />
-                <geometry>
-                    <mesh filename="${path_meshes}/parts/mecanum_wheels.stl"/>
-                </geometry>
-                <material name="Green" />
-            </visual>
-        </link>
-
-        <!-- Reference https://github.com/gazebosim/gz-sim/issues/2261 -->
-        <gazebo reference="omniwheel_${type}_${position}">
-            <mu1 value="0.01"/>
-            <mu2 value="0.01"/>
-        </gazebo>
-
-
-    </xacro:macro>
-
-</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/properties.urdf.xacro b/nanosaur_description/urdf/properties.urdf.xacro
new file mode 100644
index 0000000..5777ab0
--- /dev/null
+++ b/nanosaur_description/urdf/properties.urdf.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<robot name="nanosaur_properties" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <!-- Global properties -->
+    <xacro:property name="M_PI" value="3.14159"/>
+    <!--https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1284 -->
+    <!--I cannot use the solution below for remote machines-->
+    <!--<xacro:property name="path_meshes" value="file://$(find nanosaur_description)/meshes"/>-->
+    <!--<xacro:property name="path_meshes" value="package://nanosaur_description/meshes"/>-->
+
+    <xacro:property name="nanosaur_mass" value="0.5"/>
+    <xacro:property name="nanosaur_width" value="0.123"/>
+    <xacro:property name="nanosaur_length" value="0.182"/>
+    <xacro:property name="nanosaur_height" value="0.103"/>
+
+    <xacro:property name="nanosaur_body_offset_x" value="0.015"/>
+    <xacro:property name="nanosaur_wheel_offset_z" value="0.007"/>
+    <!-- Longitudinal distance between front and back wheels, in meters. -->
+    <xacro:property name="nanosaur_wheelbase" value="0.1"/>
+    <!-- Lateral distance between left and right wheels, in meters. -->
+    <xacro:property name="nanosaur_wheel_separation" value="0.101"/>
+
+    <xacro:property name="mecanum_mass" value="0.015"/>
+    <xacro:property name="mecanum_radius" value="${0.037 / 2}"/>
+    <xacro:property name="mecanum_length" value="0.024"/>
+    <xacro:property name="mecanum_friction" value="0.1"/>
+
+</robot>
\ No newline at end of file
diff --git a/nanosaur_description/urdf/robot.urdf.xacro b/nanosaur_description/urdf/robot.urdf.xacro
index 8d764f8..27f495a 100644
--- a/nanosaur_description/urdf/robot.urdf.xacro
+++ b/nanosaur_description/urdf/robot.urdf.xacro
@@ -1,188 +1,88 @@
 <?xml version="1.0"?>
 <robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
 
-    <!-- materials -->
-    <xacro:include filename="$(find nanosaur_description)/urdf/materials.urdf" />
-    <xacro:include filename="$(find nanosaur_description)/urdf/materials.gazebo.xacro" />
+    <!-- utilities -->
+    <xacro:include filename="$(find nanosaur_description)/urdf/utilities.urdf.xacro" />
     <!-- nanosaur parts -->
-    <xacro:include filename="$(find nanosaur_description)/urdf/omniwheel.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/properties.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/base.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/mecanum.urdf.xacro" />
     <xacro:include filename="$(find nanosaur_description)/urdf/flap/flap.urdf.xacro" />
     <xacro:include filename="$(find nanosaur_description)/urdf/flap/flap_LD06.urdf.xacro" />
     <xacro:include filename="$(find nanosaur_description)/urdf/head/head.urdf.xacro" />
     <xacro:include filename="$(find nanosaur_description)/urdf/head/head_realsense.urdf.xacro" />
-    <!-- Global properties -->
-    <xacro:property name="M_PI" value="3.14159"/>
-    <!--https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1284 -->
-    <!--I cannot use the solution below for remote machines-->
-    <!--<xacro:property name="path_meshes" value="file://$(find nanosaur_description)/meshes"/>-->
-    <!--<xacro:property name="path_meshes" value="package://nanosaur_description/meshes"/>-->
 
-    <xacro:macro name="nanosaur" params="head_type flap_type use_nominal_extrinsics diff_drive_emulation mimic_enable:=true path_meshes:=package://nanosaur_description/meshes">
+    <xacro:macro name="nanosaur" params="head_type flap_type use_nominal_extrinsics path_meshes:=package://nanosaur_description/meshes">
 
-        <link name="nanosaur_base">
-            <inertial>
-                <mass value="0.453"/>
-                <origin rpy=" 0 0 0" xyz="0.0 0 0.0"/>
-                <inertia ixx="982653.79909" ixy="-2069.061" ixz="193876.45537"
-                        iyy="1888714.1125" iyz="-4847.17947"
-                        izz="2199329.1679"/>
-            </inertial>
-            <visual>
-                <geometry><mesh filename="${path_meshes}/nanosaur_base.stl" scale="0.001 0.001 0.001"/></geometry>
-                <material name="Green" />
-            </visual>
-            <collision>
-                <origin xyz="0.01 0 0.005" rpy="0 0 0"/>
-                <geometry>
-                    <box size="0.17 0.075 0.09"/>
-                </geometry>
-                <material name="Green" />
-            </collision>
-        </link>
+        <link name="base_link"/>
 
-        <joint name="nanosaur_cover_batteries_joint" type="fixed">
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <parent link="nanosaur_base"/>
-            <child link="nanosaur_cover_batteries"/>
+        <joint name="chassis_to_base" type="fixed">
+            <parent link="base_link"/>
+            <child link="nanosaur_base"/>
+            <origin xyz="0 0 ${mecanum_radius}" rpy="0 0 0"/>
         </joint>
 
-        <link name="nanosaur_cover_batteries">
-            <visual>
-                <geometry><mesh filename="${path_meshes}/nanosaur_cover_batteries.stl" scale="0.001 0.001 0.001"/></geometry>
-                <material name="Green" />
-            </visual>
-        </link>
-
-        <joint name="nanosaur_cover_electronics_joint" type="fixed">
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <parent link="nanosaur_base"/>
-            <child link="nanosaur_cover_electronics"/>
-        </joint>
-
-        <link name="nanosaur_cover_electronics">
-            <visual>
-                <geometry><mesh filename="${path_meshes}/nanosaur_cover_electronics.stl" scale="0.001 0.001 0.001"/></geometry>
-                <material name="Green" />
-            </visual>
-        </link>
-
-        <joint name="NVIDIA_jetson_joint" type="fixed">
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <parent link="nanosaur_base"/>
-            <child link="NVIDIA_jetson"/>
-        </joint>
-
-        <link name="NVIDIA_jetson">
-            <visual>
-                <origin xyz="0.056 0.0078 0.0085" rpy="0 0 ${M_PI}" />
-                <geometry><mesh filename="${path_meshes}/parts/NVIDIA_Jetson_Orin_Nano_DevKit.stl"/></geometry>
-                <material name="DarkGrey" />
-            </visual>
-        </link>
-
-        <joint name="motor2040_joint" type="fixed">
-            <origin xyz="0 0 -0.035" rpy="0 ${M_PI} 0" />
-            <parent link="nanosaur_base"/>
-            <child link="motor2040"/>
-        </joint>
-
-        <link name="motor2040">
-            <visual>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry><mesh filename="${path_meshes}/parts/motor2040.stl"/></geometry>
-                <material name="DarkGrey" />
-            </visual>
-        </link>
-
-        <joint name="nanosaur_PCB_electronics_joint" type="fixed">
-            <origin xyz="-0.046 -0.015 0.0225" rpy="0 0 0" />
-            <parent link="nanosaur_base"/>
-            <child link="nanosaur_PCB_electronics"/>
-        </joint>
-
-        <link name="nanosaur_PCB_electronics">
-            <visual>
-                <origin xyz="0 0 0" rpy="0 0 0" />
-                <geometry><mesh filename="${path_meshes}/parts/nanosaur_PCB_electronics.stl"/></geometry>
-                <material name="DarkGrey" />
-            </visual>
-        </link>
-
-        <joint name="nanosaur_PCB_oleds_joint" type="fixed">
-            <origin xyz="0.053 0 0.037" rpy="0 ${M_PI / 5.5} 0" />
-            <parent link="nanosaur_base"/>
-            <child link="nanosaur_PCB_oleds"/>
-        </joint>
-
-        <link name="nanosaur_PCB_oleds">
-            <visual>
-                <origin xyz="0 0 0" rpy="0 0 ${M_PI / 2}" />
-                <geometry><mesh filename="${path_meshes}/parts/nanosaur_PCB_oleds.stl"/></geometry>
-                <material name="DarkGrey" />
-            </visual>
-        </link>
-
-        <joint name="display_left_joint" type="fixed">
-            <origin xyz="-0.0122 0.021 0.003" rpy="0 0 0" />
-            <parent link="nanosaur_PCB_oleds"/>
-            <child link="display_left"/>
-        </joint>
-
-        <link name="display_left">
-            <inertial>
-                <mass value="0.01"/>
-                <origin rpy="0 0 0" xyz="0 0 0"/>
-                <inertia ixx="1.6666666666666668e-07" ixy="0" ixz="0" iyy="1.6666666666666668e-07" iyz="0" izz="1.6666666666666668e-07"/>
-            </inertial>
-            <visual>
-                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}" />
-                <geometry>
-                    <mesh filename="${path_meshes}/parts/OLED_Module.stl"/>
-                </geometry>
-                <material name="Black" />
-            </visual>
-        </link>
-
-        <joint name="display_right_joint" type="fixed">
-            <origin xyz="-0.0122 -0.021 0.003" rpy="0 0 0" />
-            <parent link="nanosaur_PCB_oleds"/>
-            <child link="display_right"/>
-        </joint>
-
-        <link name="display_right">
-            <inertial>
-                <mass value="0.01"/>
-                <origin rpy=" 0 0 0" xyz="0 0 0"/>
-                <inertia ixx="1.6666666666666668e-07" ixy="0" ixz="0" iyy="1.6666666666666668e-07" iyz="0" izz="1.6666666666666668e-07"/>
-            </inertial>
-            <visual>
-                <origin xyz="0 0 0" rpy="0 0 ${M_PI/2}" />
-                <geometry>
-                    <mesh filename="${path_meshes}/parts/OLED_Module.stl"/>
-                </geometry>
-                <material name="Black" />
-            </visual>
-        </link>
-
-        <xacro:omniwheel parent="nanosaur_base" type="left" position="front" xyz="0.05 0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="left" position="rear" xyz="-0.05 0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="right" position="front" xyz="0.05 -0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="right" position="rear" xyz="-0.05 -0.041 -0.035"/>
+        <xacro:nanosaur_base 
+                base_name="nanosaur_base"
+                length="${nanosaur_length}"
+                width="${nanosaur_width}"
+                height="${nanosaur_height}"
+                mass="${nanosaur_mass}" />
+
+        <xacro:mecanum_wheel
+                position="front"
+                side="left"
+                parent="nanosaur_base"
+                radius="${mecanum_radius}"
+                length="${mecanum_length}"
+                xyz="${nanosaur_wheelbase / 2} ${nanosaur_wheel_separation / 2} 0"
+                mass="${mecanum_mass}"
+                mu="${mecanum_friction}" />
+
+        <xacro:mecanum_wheel
+                position="front"
+                side="right"
+                parent="nanosaur_base"
+                radius="${mecanum_radius}"
+                length="${mecanum_length}"
+                xyz="${nanosaur_wheelbase / 2} -${nanosaur_wheel_separation / 2} 0"
+                mass="${mecanum_mass}"
+                mu="${mecanum_friction}" />
+
+        <xacro:mecanum_wheel
+                position="rear"
+                side="left"
+                parent="nanosaur_base"
+                radius="${mecanum_radius}"
+                length="${mecanum_length}"
+                xyz="-${nanosaur_wheelbase / 2} ${nanosaur_wheel_separation / 2} 0"
+                mass="${mecanum_mass}"
+                mu="${mecanum_friction}" />
+
+        <xacro:mecanum_wheel
+                position="rear"
+                side="right"
+                parent="nanosaur_base"
+                radius="${mecanum_radius}"
+                length="${mecanum_length}"
+                xyz="-${nanosaur_wheelbase / 2} -${nanosaur_wheel_separation / 2} 0"
+                mass="${mecanum_mass}"
+                mu="${mecanum_friction}" />   
 
         <!-- Nanosaur heads -->
         <xacro:if value="${head_type == 'empty'}">
-            <xacro:nanosaur_head parent="nanosaur_base" />
+            <xacro:nanosaur_head parent="nanosaur_chassis" />
         </xacro:if>
         <xacro:if value="${head_type == 'realsense'}">
-            <xacro:nanosaur_head_realsense parent="nanosaur_base" name="camera" use_nominal_extrinsics="${use_nominal_extrinsics}" />
+            <xacro:nanosaur_head_realsense parent="nanosaur_chassis" name="camera" use_nominal_extrinsics="${use_nominal_extrinsics}" />
         </xacro:if>
 
         <!-- Nanosaur flap -->
         <xacro:if value="${flap_type == 'empty'}">
-            <xacro:nanosaur_flap parent="nanosaur_base" />
+            <xacro:nanosaur_flap parent="nanosaur_chassis" />
         </xacro:if>
         <xacro:if value="${flap_type == 'LD06'}">
-            <xacro:nanosaur_flap_LD06 parent="nanosaur_base" />
+            <xacro:nanosaur_flap_LD06 parent="nanosaur_chassis" />
         </xacro:if>
 
     </xacro:macro>
diff --git a/nanosaur_description/urdf/utilities.urdf.xacro b/nanosaur_description/urdf/utilities.urdf.xacro
new file mode 100644
index 0000000..c7242c4
--- /dev/null
+++ b/nanosaur_description/urdf/utilities.urdf.xacro
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<robot name="utilities" xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <material name="Black">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+    </material>
+
+    <material name="Green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+    </material>
+
+    <material name="aluminum">
+    <color rgba="0.5 0.5 0.5 1.0"/>
+    </material>
+
+    <material name="DarkGrey">
+    <color rgba="0.3 0.3 0.3 1.0"/>
+    </material>
+
+    <material name="GunMetalGrey">
+    <color rgba="0.46 0.5 0.56 1.0"/>
+    </material>
+
+    <material name="Gold">
+    <color rgba="1 1 0.752941176470588 1" />
+    </material>
+
+    <material name="zedm_mat">
+    <color rgba="0 0 0 0.9"/>
+    </material>
+
+    <xacro:macro name="inertial_sphere" params="m r *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${m}" />
+            <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0"
+                    iyy="${(2/5) * m * (r*r)}" iyz="0.0"
+                    izz="${(2/5) * m * (r*r)}" />
+        </inertial>
+    </xacro:macro>  
+
+
+    <xacro:macro name="inertial_box" params="m x y z *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${m}" />
+            <inertia ixx="${(1/12) * m * (y*y+z*z)}" ixy="0.0" ixz="0.0"
+                    iyy="${(1/12) * m * (x*x+z*z)}" iyz="0.0"
+                    izz="${(1/12) * m * (x*x+y*y)}" />
+        </inertial>
+    </xacro:macro>
+
+
+    <xacro:macro name="inertial_cylinder" params="m r l *origin">
+        <inertial>
+            <xacro:insert_block name="origin"/>
+            <mass value="${m}" />
+            <inertia ixx="${(1/12) * m * (3*r*r + l*l)}" ixy="0.0" ixz="0.0"
+                    iyy="${(1/12) * m * (3*r*r + l*l)}" iyz="0.0"
+                    izz="${(1/2) * m * (r*r)}" />
+        </inertial>
+    </xacro:macro>
+
+</robot>
\ No newline at end of file