From feae46c2f9c88891c488dda3e08b1377a5c52b26 Mon Sep 17 00:00:00 2001
From: Henrique-BO <henrique.barrosoliveira@usp.br>
Date: Sun, 24 Sep 2023 17:45:57 +0200
Subject: [PATCH 1/3] Add pendulum for ApplyForceTorque and MouseDrag demo

Signed-off-by: Henrique-BO <henrique.barrosoliveira@usp.br>
---
 harmonic_demo/Pendulum/model.config |  16 ++++
 harmonic_demo/Pendulum/model.sdf    | 120 ++++++++++++++++++++++++++++
 harmonic_demo/harmonic.sdf          |  23 +++++-
 3 files changed, 158 insertions(+), 1 deletion(-)
 create mode 100644 harmonic_demo/Pendulum/model.config
 create mode 100644 harmonic_demo/Pendulum/model.sdf

diff --git a/harmonic_demo/Pendulum/model.config b/harmonic_demo/Pendulum/model.config
new file mode 100644
index 0000000..c7ac412
--- /dev/null
+++ b/harmonic_demo/Pendulum/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<model>
+  <name>Pendulum</name>
+  <version>1.0</version>
+  <sdf version="1.11">model.sdf</sdf>
+
+  <author>
+    <name>Henrique Barros Oliveira</name>
+    <email>henrique.barrosoliveira@usp.br</email>
+  </author>
+
+  <description>
+    Pendulum for demonstrating ApplyForceTorque and MouseDrag plugins
+  </description>
+
+</model>
\ No newline at end of file
diff --git a/harmonic_demo/Pendulum/model.sdf b/harmonic_demo/Pendulum/model.sdf
new file mode 100644
index 0000000..a6702d8
--- /dev/null
+++ b/harmonic_demo/Pendulum/model.sdf
@@ -0,0 +1,120 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.11">
+  <model name="pendulum">
+    <link name="base">
+      <inertial auto="true"/>
+      <visual name="vis_plate_on_ground">
+        <pose>0 0 0.01 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.2 0.1 0.1</diffuse>
+          <specular>0.2 0.2 0.2</specular>
+        </material>
+      </visual>
+      <visual name="vis_pole">
+        <pose>-0.05 0 0.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.4</size>
+          </box>
+        </geometry>
+        <material>
+          <diffuse>0.2 0.1 0.1</diffuse>
+          <specular>0.2 0.2 0.2</specular>
+        </material>
+      </visual>
+      <collision name="col_plate_on_ground">
+        <density>1</density>
+        <pose>0 0 0.01 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.2</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+      </collision>
+      <collision name="col_pole">
+        <density>1000</density>
+        <pose>-0.05 0 0.2 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.4</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- upper link, length 1, IC -90 degrees -->
+    <link name="upper_link">
+      <!-- <pose>0 0 0.55 -1.5708 0 0</pose> -->
+      <pose>0 0 0.35 3.1415 0 0</pose>
+      <self_collide>0</self_collide>
+      <inertial auto="true"/>
+      <visual name="vis_upper_joint">
+        <pose>0.04 0 0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.02</radius>
+            <length>0.2</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+        </material>
+      </visual>
+      <visual name="vis_cylinder">
+        <pose>0.1 0 0.15 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.02</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <diffuse>0.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+        </material>
+      </visual>
+      <collision name="col_upper_joint">
+        <density>1000</density>
+        <pose>-0.01 0 0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.02</radius>
+            <length>0.15</length>
+          </cylinder>
+        </geometry>
+      </collision>
+      <collision name="col_cylinder">
+        <density>1000</density>
+        <pose>0 0 0.15 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.02</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- pin joint for upper link, at origin of upper link -->
+    <joint name="upper_joint" type="revolute">
+      <parent>base</parent>
+      <child>upper_link</child>
+      <axis>
+        <xyz>1.0 0 0</xyz>
+      </axis>
+    </joint>
+
+    <joint name="fix_to_world" type="fixed">
+      <parent>world</parent>
+      <child>base</child>
+    </joint>
+  </model>
+</sdf>
\ No newline at end of file
diff --git a/harmonic_demo/harmonic.sdf b/harmonic_demo/harmonic.sdf
index 1a80dd2..0fa8797 100644
--- a/harmonic_demo/harmonic.sdf
+++ b/harmonic_demo/harmonic.sdf
@@ -246,6 +246,23 @@
           <property type="string" key="state">docked_collapsed</property>
         </gz-gui>
       </plugin>
+      
+      <!-- Apply Force and Torque -->
+      <plugin filename="ApplyForceTorque">
+        <gz-gui>
+          <property type="string" key="state">docked_collapsed</property>
+        </gz-gui>
+      </plugin>
+
+      <!-- Mouse Drag -->
+      <plugin filename="MouseDrag">
+        <gz-gui>
+          <property type="string" key="state">docked_collapsed</property>
+        </gz-gui>
+
+        <rotation_stiffness>50</rotation_stiffness>
+        <position_stiffness>200</position_stiffness>
+      </plugin>
 
     </gui>
 
@@ -443,6 +460,11 @@
       <name>Office Chair</name>
       <uri>Office Chair</uri>
     </include>
+    <include>
+      <pose>1.85 -5.05 7.82 0 0 0</pose>
+      <name>Pendulum</name>
+      <uri>Pendulum</uri>
+    </include>
     <include>
       <name>Pendulum_sculpture</name>
       <pose>3.43 -3.82 7.64 0 0 1.57</pose>
@@ -496,4 +518,3 @@
     </model>
   </world>
 </sdf>
-

From 8f30e378d1ef6fab1752696a373ba174e6c52910 Mon Sep 17 00:00:00 2001
From: Henrique-BO <henrique.barrosoliveira@usp.br>
Date: Sun, 24 Sep 2023 17:50:51 +0200
Subject: [PATCH 2/3] Add newlines

Signed-off-by: Henrique-BO <henrique.barrosoliveira@usp.br>
---
 harmonic_demo/Pendulum/model.config | 2 +-
 harmonic_demo/Pendulum/model.sdf    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/harmonic_demo/Pendulum/model.config b/harmonic_demo/Pendulum/model.config
index c7ac412..083253d 100644
--- a/harmonic_demo/Pendulum/model.config
+++ b/harmonic_demo/Pendulum/model.config
@@ -13,4 +13,4 @@
     Pendulum for demonstrating ApplyForceTorque and MouseDrag plugins
   </description>
 
-</model>
\ No newline at end of file
+</model>
diff --git a/harmonic_demo/Pendulum/model.sdf b/harmonic_demo/Pendulum/model.sdf
index a6702d8..5ee21c6 100644
--- a/harmonic_demo/Pendulum/model.sdf
+++ b/harmonic_demo/Pendulum/model.sdf
@@ -117,4 +117,4 @@
       <child>base</child>
     </joint>
   </model>
-</sdf>
\ No newline at end of file
+</sdf>

From 8f311786fed23447aaf29762e8c89950904fba2c Mon Sep 17 00:00:00 2001
From: Henrique-BO <henrique.barrosoliveira@usp.br>
Date: Mon, 25 Sep 2023 23:02:55 +0200
Subject: [PATCH 3/3] Fix pendulum collision

Signed-off-by: Henrique-BO <henrique.barrosoliveira@usp.br>
---
 harmonic_demo/Pendulum/model.sdf | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/harmonic_demo/Pendulum/model.sdf b/harmonic_demo/Pendulum/model.sdf
index 5ee21c6..5389c3d 100644
--- a/harmonic_demo/Pendulum/model.sdf
+++ b/harmonic_demo/Pendulum/model.sdf
@@ -33,7 +33,7 @@
         <pose>0 0 0.01 0 0 0</pose>
         <geometry>
           <cylinder>
-            <radius>0.2</radius>
+            <radius>0.1</radius>
             <length>0.02</length>
           </cylinder>
         </geometry>
@@ -51,7 +51,6 @@
 
     <!-- upper link, length 1, IC -90 degrees -->
     <link name="upper_link">
-      <!-- <pose>0 0 0.55 -1.5708 0 0</pose> -->
       <pose>0 0 0.35 3.1415 0 0</pose>
       <self_collide>0</self_collide>
       <inertial auto="true"/>
@@ -83,17 +82,17 @@
       </visual>
       <collision name="col_upper_joint">
         <density>1000</density>
-        <pose>-0.01 0 0 0 1.5708 0</pose>
+        <pose>0.04 0 0 0 1.5708 0</pose>
         <geometry>
           <cylinder>
             <radius>0.02</radius>
-            <length>0.15</length>
+            <length>0.2</length>
           </cylinder>
         </geometry>
       </collision>
       <collision name="col_cylinder">
         <density>1000</density>
-        <pose>0 0 0.15 0 0 0</pose>
+        <pose>0.1 0 0.15 0 0 0</pose>
         <geometry>
           <cylinder>
             <radius>0.02</radius>