PX4-Autopilot/Tools/simulation/gz/models/gimbal/model.sdf

<?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">
        <gz_frame_id>camera_link</gz_frame_id>
        <always_on>1</always_on>
        <update_rate>250</update_rate>
        <pose>-0.0412 0 -0.162 0 0 3.14</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">
        <gz_frame_id>camera_link</gz_frame_id>
        <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>
      </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>