From 7d4be5179468c23a3130f8a51ed21592d6c7736a Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 10 May 2024 19:58:33 +0000
Subject: [PATCH] Make position and effort version of pendulum equal

---
 .../urdf/test_pendulum_position.xacro.urdf    | 78 ++++++++++---------
 1 file changed, 41 insertions(+), 37 deletions(-)

diff --git a/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf b/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf
index 44fa9744..63e7f830 100644
--- a/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf
+++ b/gz_ros2_control_demos/urdf/test_pendulum_position.xacro.urdf
@@ -1,6 +1,36 @@
-<?xml version="1.0" ?>
-<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 0"/>
+      </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>
@@ -16,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 ${pi} 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.1" friction="0.0"/>
   </joint>
 
   <ros2_control name="GazeboSystem" type="system">