From ef3f9c6f9fcec6b6ae123dbdc8a0518d93c96f08 Mon Sep 17 00:00:00 2001
From: Luis Camero <lcamero@clearpathrobotics.com>
Date: Mon, 16 Oct 2023 14:42:56 -0400
Subject: [PATCH] Inertia fix

---
 .../diffbot/urdf/diffbot_description.urdf.xacro      | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro b/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro
index 03aa3497f..263be605e 100644
--- a/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro
+++ b/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro
@@ -72,9 +72,9 @@
         <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
         <mass value="${wheel_mass}"/>
         <inertia
-          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
-          iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
-          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
+          ixx="${wheel_mass / 12.0 * (3*wheel_radius*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
+          iyy="${wheel_mass / 12.0 * (3*wheel_radius*wheel_radius + wheel_len*wheel_len)}" iyz="0.0"
+          izz="${wheel_mass / 2.0 * wheel_radius*wheel_radius}"/>
         </inertial>
       </link>
 
@@ -107,9 +107,9 @@
         <origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
         <mass value="${wheel_mass}"/>
         <inertia
-          ixx="${wheel_mass / 12.0 * (3*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
-          iyy="${wheel_mass / 12.0 * wheel_radius*wheel_radius}" iyz="0.0"
-          izz="${wheel_mass / 12.0 * wheel_radius*wheel_radius}"/>
+          ixx="${wheel_mass / 12.0 * (3*wheel_radius*wheel_radius + wheel_len*wheel_len)}" ixy="0.0" ixz="0.0"
+          iyy="${wheel_mass / 12.0 * (3*wheel_radius*wheel_radius + wheel_len*wheel_len)}" iyz="0.0"
+          izz="${wheel_mass / 2.0 * wheel_radius*wheel_radius}"/>
         </inertial>
       </link>