From f48a17a4b30601ebb11e2f9e3e678fec8a73ebdc Mon Sep 17 00:00:00 2001
From: Stefano Colli <45536733+StefanoColli@users.noreply.github.com>
Date: Mon, 16 Dec 2024 14:59:25 +0100
Subject: [PATCH] Extract gimbal model  (#70)

* gimbal: extract gimbal model for reusability

* Increase pitch joint limit
---
 .../meshes/cgo3_camera_remeshed_v1.stl        | Bin
 .../cgo3_horizontal_arm_remeshed_v1.stl       | Bin
 .../meshes/cgo3_mount_remeshed_v1.stl         | Bin
 .../meshes/cgo3_vertical_arm_remeshed_v1.stl  | Bin
 models/gimbal/model.config                    |  11 +
 models/gimbal/model.sdf                       | 357 +++++++++++++++++
 models/x500_gimbal/model.sdf                  | 379 +-----------------
 7 files changed, 375 insertions(+), 372 deletions(-)
 rename models/{x500_gimbal => gimbal}/meshes/cgo3_camera_remeshed_v1.stl (100%)
 rename models/{x500_gimbal => gimbal}/meshes/cgo3_horizontal_arm_remeshed_v1.stl (100%)
 rename models/{x500_gimbal => gimbal}/meshes/cgo3_mount_remeshed_v1.stl (100%)
 rename models/{x500_gimbal => gimbal}/meshes/cgo3_vertical_arm_remeshed_v1.stl (100%)
 create mode 100644 models/gimbal/model.config
 create mode 100644 models/gimbal/model.sdf

diff --git a/models/x500_gimbal/meshes/cgo3_camera_remeshed_v1.stl b/models/gimbal/meshes/cgo3_camera_remeshed_v1.stl
similarity index 100%
rename from models/x500_gimbal/meshes/cgo3_camera_remeshed_v1.stl
rename to models/gimbal/meshes/cgo3_camera_remeshed_v1.stl
diff --git a/models/x500_gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl b/models/gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl
similarity index 100%
rename from models/x500_gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl
rename to models/gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl
diff --git a/models/x500_gimbal/meshes/cgo3_mount_remeshed_v1.stl b/models/gimbal/meshes/cgo3_mount_remeshed_v1.stl
similarity index 100%
rename from models/x500_gimbal/meshes/cgo3_mount_remeshed_v1.stl
rename to models/gimbal/meshes/cgo3_mount_remeshed_v1.stl
diff --git a/models/x500_gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl b/models/gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl
similarity index 100%
rename from models/x500_gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl
rename to models/gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl
diff --git a/models/gimbal/model.config b/models/gimbal/model.config
new file mode 100644
index 0000000..ef6f19b
--- /dev/null
+++ b/models/gimbal/model.config
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<model>
+  <name>gimbal</name>
+  <version>1.0</version>
+  <sdf version="1.9">model.sdf</sdf>
+  <author>
+    <name>Stefano Colli</name>
+    <email>stefano@auterion.com</email>
+  </author>
+  <description>Gimbal with camera model</description>
+</model>
diff --git a/models/gimbal/model.sdf b/models/gimbal/model.sdf
new file mode 100644
index 0000000..46d2ce8
--- /dev/null
+++ b/models/gimbal/model.sdf
@@ -0,0 +1,357 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <model name='gimbal'>
+    <self_collide>false</self_collide>
+    <static>false</static>
+
+  <!-- Gimbal mount point -->
+    <link name="cgo3_mount_link">
+      <inertial>
+        <pose>-0.02552 0 -0.08136 0 0 0</pose>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0</iyz>
+          <izz>0.001</izz>
+        </inertia>
+      </inertial>
+      <visual name="cgo3_mount_visual">
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://gimbal/meshes/cgo3_mount_remeshed_v1.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175  1.0</ambient>
+          <diffuse>.175 .175 .175  1.0</diffuse>
+          <specular>.175 .175 .175  1.0</specular>
+        </material>
+      </visual>
+    </link>
+
+  <!-- Gimbal vertical arm -->
+    <link name="cgo3_vertical_arm_link">
+      <inertial>
+        <pose>0 0 -0.1283 0 0 0</pose>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0</iyz>
+          <izz>0.001</izz>
+        </inertia>
+      </inertial>
+      <visual name='cgo3_vertical_arm_visual'>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175  1.0</ambient>
+          <diffuse>.175 .175 .175  1.0</diffuse>
+          <specular>.175 .175 .175  1.0</specular>
+        </material>
+      </visual>
+    </link>
+
+  <!-- Yaw joint -->
+    <joint name='cgo3_vertical_arm_joint' type='revolute'>
+      <child>cgo3_vertical_arm_link</child>
+      <parent>cgo3_mount_link</parent>
+      <pose>-0.02552 0 0 0 0 0</pose>
+      <axis>
+        <xyz>0 0 -1</xyz>
+        <limit>
+          <lower>-1e+16</lower>
+          <upper>1e+16</upper>
+          <effort>100</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+          <limit>
+            <cfm>0.1</cfm>
+            <erp>0.2</erp>
+          </limit>
+        </ode>
+      </physics>
+    </joint>
+
+   <!-- Gimbal horizontal arm -->
+    <link name="cgo3_horizontal_arm_link">
+      <inertial>
+        <pose>-0.0213 0 -0.162 0 0 0</pose>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0</iyz>
+          <izz>0.001</izz>
+        </inertia>
+      </inertial>
+      <visual name='cgo3_horizontal_arm_visual'>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175  1.0</ambient>
+          <diffuse>.175 .175 .175  1.0</diffuse>
+          <specular>.175 .175 .175  1.0</specular>
+        </material>
+      </visual>
+    </link>
+
+  <!-- Roll joint -->
+    <joint name='cgo3_horizontal_arm_joint' type='revolute'>
+      <child>cgo3_horizontal_arm_link</child>
+      <parent>cgo3_vertical_arm_link</parent>
+      <pose>0 0 -0.1619 0 0 0</pose>
+      <axis>
+        <xyz>-1 0 0</xyz>
+        <limit>
+          <lower>-0.785398</lower>
+          <upper>0.785398</upper>
+          <effort>100</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+          <limit>
+            <cfm>0.1</cfm>
+            <erp>0.2</erp>
+          </limit>
+        </ode>
+      </physics>
+    </joint>
+
+   <!-- Camera -->
+    <link name="camera_link">
+      <inertial>
+        <pose>-0.0412 0 -0.162 0 0 0</pose>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.001</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.001</iyy>
+          <iyz>0</iyz>
+          <izz>0.001</izz>
+        </inertia>
+      </inertial>
+      <collision name='cgo3_camera_collision'>
+        <pose>-0.0412 0 -0.162 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.035</radius>
+          </sphere>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+            </ode>
+          </friction>
+          <contact>
+            <ode>
+              <kp>1e+8</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+        </surface>
+      </collision>
+      <visual name='cgo3_camera_visual'>
+        <geometry>
+          <mesh>
+            <scale>0.001 0.001 0.001</scale>
+            <uri>model://gimbal/meshes/cgo3_camera_remeshed_v1.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <ambient>.175 .175 .175  1.0</ambient>
+          <diffuse>.175 .175 .175  1.0</diffuse>
+          <specular>.175 .175 .175  1.0</specular>
+        </material>
+      </visual>
+
+      <sensor name="camera_imu" type="imu">
+        <always_on>1</always_on>
+        <update_rate>250</update_rate>
+        <pose>-0.0412 0 -0.162 0 0 0</pose>
+        <imu>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00018665</stddev>
+                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00018665</stddev>
+                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00018665</stddev>
+                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
+              </noise>
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00186</stddev>
+                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00186</stddev>
+                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0</mean>
+                <stddev>0.00186</stddev>
+                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
+                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
+              </noise>
+            </z>
+          </linear_acceleration>
+        </imu>
+      </sensor>
+
+      <sensor name="camera" type="camera">
+        <pose>-0.074 0 -0.162 0 0 3.14</pose>
+        <camera>
+          <horizontal_fov>2.0</horizontal_fov>
+          <image>
+            <format>R8G8B8</format>
+             <format>R8G8B8</format>
+            <width>1280</width>
+            <height>720</height>
+          </image>
+          <clip>
+            <near>0.05</near>
+            <far>15000</far>
+          </clip>
+        </camera>
+        <always_on>1</always_on>
+        <update_rate>30</update_rate>
+        <visualize>true</visualize>
+        <topic>camera</topic>
+      </sensor>
+    </link>
+
+  <!-- Pitch joint -->
+    <joint name='cgo3_camera_joint' type='revolute'>
+      <child>camera_link</child>
+      <parent>cgo3_horizontal_arm_link</parent>
+      <pose>-0.0412 0 -0.162 0 0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <limit>
+          <lower>-2.35619</lower>
+          <upper>0.7854</upper>
+          <effort>100</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <damping>0.1</damping>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <implicit_spring_damper>1</implicit_spring_damper>
+          <limit>
+            <cfm>0.1</cfm>
+            <erp>0.2</erp>
+          </limit>
+        </ode>
+      </physics>
+    </joint>
+
+  <!-- Plugins -->
+
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>cgo3_horizontal_arm_joint</joint_name>
+      <sub_topic>command/gimbal_roll</sub_topic>
+      <p_gain>0.8</p_gain>
+      <i_gain>0.035</i_gain>
+      <d_gain>0.02</d_gain>
+      <i_max>0</i_max>
+      <i_min>0</i_min>
+      <cmd_max>0.3</cmd_max>
+      <cmd_min>-0.3</cmd_min>
+    </plugin>
+
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>cgo3_camera_joint</joint_name>
+      <sub_topic>command/gimbal_pitch</sub_topic>
+      <p_gain>0.8</p_gain>
+      <i_gain>0.01245</i_gain>
+      <d_gain>0.015</d_gain>
+      <i_max>0</i_max>
+      <i_min>0</i_min>
+      <cmd_max>0.3</cmd_max>
+      <cmd_min>-0.3</cmd_min>
+    </plugin>
+
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>cgo3_vertical_arm_joint</joint_name>
+      <sub_topic>command/gimbal_yaw</sub_topic>
+      <p_gain>0.3</p_gain>
+      <i_gain>0.01245</i_gain>
+      <d_gain>0.015</d_gain>
+      <i_max>0</i_max>
+      <i_min>0</i_min>
+      <cmd_max>0.3</cmd_max>
+      <cmd_min>-0.3</cmd_min>
+    </plugin>
+
+  </model>
+</sdf>
diff --git a/models/x500_gimbal/model.sdf b/models/x500_gimbal/model.sdf
index 14386e1..7353b6a 100644
--- a/models/x500_gimbal/model.sdf
+++ b/models/x500_gimbal/model.sdf
@@ -2,380 +2,15 @@
 <sdf version='1.9'>
   <model name='x500_gimbal'>
     <include merge='true'>
-      <uri>x500</uri>
+      <uri>model://x500</uri>
     </include>
-
-   <!-- Gimbal mount point -->
-    <link name="cgo3_mount_link">
-      <inertial>
-        <pose>0 0 0.18 0 0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name="cgo3_mount_visual">
-        <pose>0.03 0 0.262 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://x500_gimbal/meshes/cgo3_mount_remeshed_v1.stl</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175  1.0</ambient>
-          <diffuse>.175 .175 .175  1.0</diffuse>
-          <specular>.175 .175 .175  1.0</specular>
-        </material>
-      </visual>
-    </link>
-    <joint name="cgo3_mount_joint" type='revolute'>
-      <child>cgo3_mount_link</child>
+    <include merge='true'>
+      <uri>model://gimbal</uri>
+      <pose>0 0 0.26 0 0 3.14</pose>
+    </include>
+    <joint name="GimbalAttachJoint" type="fixed">
       <parent>base_link</parent>
-      <pose>0 0 0 0 0 0</pose>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>100</effort>
-          <velocity>-1</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-        </ode>
-      </physics>
-    </joint>
-
-   <!-- Gimbal vertical arm -->
-    <link name="cgo3_vertical_arm_link">
-      <inertial>
-        <pose>0.05 0 0.13 0 0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='cgo3_vertical_arm_visual'>
-	      <pose>0.03 0 0.262 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://x500_gimbal/meshes/cgo3_vertical_arm_remeshed_v1.stl</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175  1.0</ambient>
-          <diffuse>.175 .175 .175  1.0</diffuse>
-          <specular>.175 .175 .175  1.0</specular>
-        </material>
-      </visual>
-    </link>
-    <joint name='cgo3_vertical_arm_joint' type='revolute'>
-      <child>cgo3_vertical_arm_link</child>
-      <parent>cgo3_mount_link</parent>
-      <pose>0.005 0 0 0 0 0</pose>
-      <axis>
-        <xyz>0 0 -1</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-          <effort>100</effort>
-          <velocity>-1</velocity>
-        </limit>
-        <dynamics>
-          <damping>0.1</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <limit>
-            <!-- testing soft limits -->
-            <cfm>0.1</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-   <!-- Gimbal horizontal arm -->
-    <link name="cgo3_horizontal_arm_link">
-      <inertial>
-        <pose>0 0 0.1 0 0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='cgo3_horizontal_arm_visual'>
-        <pose>0.03 0 0.262 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://x500_gimbal/meshes/cgo3_horizontal_arm_remeshed_v1.stl</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175  1.0</ambient>
-          <diffuse>.175 .175 .175  1.0</diffuse>
-          <specular>.175 .175 .175  1.0</specular>
-        </material>
-      </visual>
-    </link>
-    <joint name='cgo3_horizontal_arm_joint' type='revolute'>
-      <child>cgo3_horizontal_arm_link</child>
-      <parent>cgo3_vertical_arm_link</parent>
-      <pose>0 0 0.1 0 0 0</pose>
-      <axis>
-        <xyz>-1 0 0</xyz>
-        <limit>
-          <lower>-0.785398</lower>
-          <upper>0.785398</upper>
-          <effort>100</effort>
-          <velocity>-1</velocity>
-        </limit>
-        <dynamics>
-          <damping>0.1</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <limit>
-            <cfm>0.1</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-   <!-- Camera -->
-    <link name="camera_link">
-      <pose>0 0 0 0 0 3.141592</pose>
-      <inertial>
-        <!-- place holder -->
-        <pose>0.015 0 0.1 0 0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <collision name='cgo3_camera_collision'>
-        <pose>0.015 0 0.1 0 0 0</pose>
-        <geometry>
-          <sphere>
-            <radius>0.035</radius>
-          </sphere>
-        </geometry>
-        <surface>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-            </ode>
-          </friction>
-          <contact>
-            <ode>
-              <kp>1e+8</kp>
-              <kd>1</kd>
-              <max_vel>0.01</max_vel>
-              <min_depth>0.001</min_depth>
-            </ode>
-          </contact>
-        </surface>
-      </collision>
-      <visual name='cgo3_camera_visual'>
-        <pose>-0.03 0 0.262 0 0 3.141592</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://x500_gimbal/meshes/cgo3_camera_remeshed_v1.stl</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175  1.0</ambient>
-          <diffuse>.175 .175 .175  1.0</diffuse>
-          <specular>.175 .175 .175  1.0</specular>
-        </material>
-      </visual>
-
-      <sensor name="camera_imu" type="imu">
-        <always_on>1</always_on>
-        <update_rate>250</update_rate>
-        <imu>
-          <angular_velocity>
-            <x>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00018665</stddev>
-                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
-              </noise>
-            </x>
-            <y>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00018665</stddev>
-                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
-              </noise>
-            </y>
-            <z>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00018665</stddev>
-                <dynamic_bias_stddev>3.8785e-05</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>1000</dynamic_bias_correlation_time>
-              </noise>
-            </z>
-          </angular_velocity>
-          <linear_acceleration>
-            <x>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00186</stddev>
-                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
-              </noise>
-            </x>
-            <y>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00186</stddev>
-                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
-              </noise>
-            </y>
-            <z>
-              <noise type="gaussian">
-                <mean>0</mean>
-                <stddev>0.00186</stddev>
-                <dynamic_bias_stddev>0.006</dynamic_bias_stddev>
-                <dynamic_bias_correlation_time>300</dynamic_bias_correlation_time>
-              </noise>
-            </z>
-          </linear_acceleration>
-        </imu>
-      </sensor>
-
-      <sensor name="camera" type="camera">
-        <pose>0 0 0.1 0 0 0</pose>
-        <camera>
-          <horizontal_fov>2.0</horizontal_fov>
-          <image>
-            <format>R8G8B8</format>
-             <format>R8G8B8</format>
-            <width>1280</width>
-            <height>720</height>
-          </image>
-          <clip>
-            <near>0.05</near>
-            <far>15000</far>
-          </clip>
-        </camera>
-        <always_on>1</always_on>
-        <update_rate>10</update_rate>
-        <visualize>true</visualize>
-        <topic>camera</topic>
-      </sensor>
-    </link>
-    <joint name='cgo3_camera_joint' type='revolute'>
-      <child>camera_link</child>
-      <parent>cgo3_horizontal_arm_link</parent>
-      <pose>0.01 0 0.1 0 0 0</pose>
-      <axis>
-        <xyz>0 -1 0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>0.7854</upper>
-          <effort>100</effort>
-          <velocity>-1</velocity>
-        </limit>
-        <dynamics>
-          <damping>0.1</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <limit>
-            <!-- testing soft limits -->
-            <cfm>0.1</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
+      <child>cgo3_mount_link</child>
     </joint>
-
-   <!-- Plugins -->
-
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>cgo3_horizontal_arm_joint</joint_name>
-      <sub_topic>command/gimbal_roll</sub_topic>
-      <p_gain>0.8</p_gain>
-      <i_gain>0.035</i_gain>
-      <d_gain>0.02</d_gain>
-      <i_max>0</i_max>
-      <i_min>0</i_min>
-      <cmd_max>0.3</cmd_max>
-      <cmd_min>-0.3</cmd_min>
-    </plugin>
-
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>cgo3_camera_joint</joint_name>
-      <sub_topic>command/gimbal_pitch</sub_topic>
-      <p_gain>0.8</p_gain>
-      <i_gain>0.01245</i_gain>
-      <d_gain>0.015</d_gain>
-      <i_max>0</i_max>
-      <i_min>0</i_min>
-      <cmd_max>0.3</cmd_max>
-      <cmd_min>-0.3</cmd_min>
-    </plugin>
-
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>cgo3_vertical_arm_joint</joint_name>
-      <sub_topic>command/gimbal_yaw</sub_topic>
-      <p_gain>0.3</p_gain>
-      <i_gain>0.01245</i_gain>
-      <d_gain>0.015</d_gain>
-      <i_max>0</i_max>
-      <i_min>0</i_min>
-      <cmd_max>0.3</cmd_max>
-      <cmd_min>-0.3</cmd_min>
-    </plugin>
-
   </model>
 </sdf>