From 4179dd6d606927e12642d31d1e86bddc66e7b1a0 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 11 Aug 2023 13:35:25 +0000
Subject: [PATCH] Fix inertias by introducing a macro

---
 .../urdf/test_pendulum_effort.xacro.urdf      | 84 +++++++++----------
 1 file changed, 42 insertions(+), 42 deletions(-)

diff --git a/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf
index a428fd32..9c78a9ba 100644
--- a/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf
+++ b/gazebo_ros2_control_demos/urdf/test_pendulum_effort.xacro.urdf
@@ -1,10 +1,36 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="cartopole">
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="pendulum on cart">
+
+  <xacro:macro name="quadr_block" params="mass width height name">
+    <link name="${name}">
+      <collision>
+        <geometry>
+          <box size="${width} ${width} ${height}"/>
+        </geometry>
+        <origin xyz="0 0 ${height/2}"/>
+      </collision>
+      <visual>
+        <geometry>
+          <box size="${width} ${width} ${height}"/>
+        </geometry>
+        <origin xyz="0 0 ${height/2}"/>
+        <material name="blue">
+          <color rgba="0 0 .8 1"/>
+        </material>
+      </visual>
+      <inertial>
+        <origin xyz="0 0 ${height/2}" rpy="0 0 0"/>
+        <mass value="${mass}"/>
+        <inertia
+          ixx="${mass / 12.0 * (width*width + height*height)}" ixy="0.0" ixz="0.0"
+          iyy="${mass / 12.0 * (height*height + width*width)}" iyz="0.0"
+          izz="${mass / 12.0 * (width*width + width*width)}"/>
+      </inertial>
+    </link>
+  </xacro:macro>
+
   <link name="world"/>
+
   <link name="slideBar">
     <visual>
       <geometry>
@@ -20,59 +46,33 @@
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
-  <link name="cart">
-    <visual>
-      <geometry>
-        <box size="0.5 0.5 0.2"/>
-      </geometry>
-      <origin xyz="0 0 0"/>
-      <material name="blue">
-        <color rgba="0 0 .8 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <mass value="10"/>
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-    </inertial>
-  </link>
 
-  <link name="pendulum">
-    <visual>
-      <geometry>
-        <box size="0.2 0.2 1.0"/>
-      </geometry>
-      <origin xyz="0 0 -0.4"/>
-      <material name="blue">
-        <color rgba="0 0 .8 1"/>
-      </material>
-    </visual>
-    <inertial>
-      <mass value="1"/>
-      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
-      <origin xyz="0 0 -0.4"/>
-    </inertial>
-  </link>
+  <xacro:quadr_block name="cart" mass="10" width="0.5" height="0.2"/>
+
+  <xacro:quadr_block name="pendulum" mass="1" width="0.2" height="1.0"/>
 
   <joint name="world_to_base" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 1"/>
     <parent link="world"/>
     <child link="slideBar"/>
   </joint>
+
   <joint name="slider_to_cart" type="prismatic">
     <axis xyz="1 0 0"/>
     <origin xyz="0.0 0.0 0.0"/>
     <parent link="slideBar"/>
     <child link="cart"/>
     <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
-    <dynamics damping="0.0" friction="0.0"/>
+    <dynamics damping="0.3" friction="0.0"/>
   </joint>
+
   <joint name="cart_to_pendulum" type="revolute">
     <axis xyz="0 1 0"/>
-    <origin xyz="0.0 0.35 0.0"/>
+    <origin xyz="0.0 0.35 0.0" rpy="0 0 0"/>
     <parent link="cart"/>
     <child link="pendulum"/>
-    <limit effort="1000.0" lower="-10000" upper="10000" velocity="30"/>
-    <dynamics damping="0.1" friction="0.1"/>
+    <limit effort="10000.0" lower="-100000" upper="100000" velocity="100000"/>
+    <dynamics damping="0.0" friction="0.0"/>
   </joint>
 
   <ros2_control name="GazeboSystem" type="system">
@@ -93,7 +93,7 @@
     <joint name="cart_to_pendulum">
       <state_interface name="position">
         <!-- this does not work if no command interface is set -->
-        <param name="initial_value">1.57</param>
+        <!-- <param name="initial_value">1.57</param> -->
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>