Port #104 to ros2

- Sync physics settings for ros2 - Remove IR lock params - Add MOT_PWM params - Default to iris with gimbal - Update zephyr parachute example - Update 3DoF gimbal URIs to use ament packages - Switch iris to use the 3DoF gimbal Signed-off-by: Rhys Mainwaring <[email protected]>
ArduPilot · Oct 30, 2024 · 610035b · 610035b
1 parent 41d29c8
commit 610035b
Show file tree

Hide file tree

Showing 8 changed files with 147 additions and 78 deletions.
diff --git a/config/gazebo-iris-gimbal.parm b/config/gazebo-iris-gimbal.parm
@@ -2,17 +2,18 @@
 FRAME_CLASS	     1
 FRAME_TYPE	     1
 
-# IRLOCK FEATURE
-RC8_OPTION       39
-PLND_ENABLED     1
-PLND_TYPE        3
+# Match servo out for motors
+MOT_PWM_MIN      1100
+MOT_PWM_MAX      1900
 
-# SONAR FOR IRLOCK
-SIM_SONAR_SCALE  10
-RNGFND1_TYPE     1
-RNGFND1_SCALING  10
-RNGFND1_PIN      0
-RNGFND1_MAX_CM   5000
+# Gimbal on mount 1 has 3 DOF
+MNT1_TYPE        1           # Servo
+MNT1_PITCH_MAX   45
+MNT1_PITCH_MIN   -135
+MNT1_ROLL_MAX    30
+MNT1_ROLL_MIN    -30
+MNT1_YAW_MAX     160
+MNT1_YAW_MIN     -160
 
 # Gimbal RC in
 RC6_MAX          1900

diff --git a/models/gimbal_small_1d/model.sdf b/models/gimbal_small_1d/model.sdf
@@ -140,11 +140,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0 0 0.02 0 0 0</pose>

diff --git a/models/gimbal_small_2d/model.sdf b/models/gimbal_small_2d/model.sdf
@@ -76,11 +76,11 @@
       <axis>
         <xyz>0 0 1</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.01 0 -0.04 0 0 0</pose>
@@ -176,11 +176,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0 0 0.02 0 0 0</pose>

diff --git a/models/gimbal_small_3d/model.sdf b/models/gimbal_small_3d/model.sdf
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <sdf version="1.9">
   <model name="gimbal_small_3d">
-    <pose>0 0 0.18 0 0 0</pose>
-    <link name="base_link">
+    <!-- <pose>0 0 0.18 0 0 0</pose> -->
+    <link name="gimbal_link">
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -14,10 +14,10 @@
           <izz>0.0001</izz>
         </inertia>
       </inertial>
-      <visual name="base_visual">
+      <visual name="gimbal_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/base_plate.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/base_plate.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -26,7 +26,7 @@
           <specular>0.01 0.01 0.01 1.0</specular>
         </material>
       </visual>
-      <collision name="base_collision">
+      <collision name="gimbal_collision">
         <pose>0.01 0.075 -0.025 0 0 0</pose>
         <geometry>
           <box>
@@ -51,7 +51,7 @@
       <visual name="yaw_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/yaw_arm.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -63,22 +63,22 @@
       <collision name="yaw_collision">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/yaw_arm.dae</uri>
           </mesh>
         </geometry>
       </collision>
     </link>
     <joint name="yaw_joint" type="revolute">
-      <parent>base_link</parent>
+      <parent>gimbal_link</parent>
       <child>yaw_link</child>
       <axis>
         <xyz>0 1 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.0105 0.065 -0.002 0 0 0</pose>
@@ -99,7 +99,7 @@
       <visual name="roll_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/roll_arm.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -111,7 +111,7 @@
       <collision name="roll_collision">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/roll_arm.dae</uri>
           </mesh>
         </geometry>
       </collision>
@@ -122,11 +122,11 @@
       <axis>
         <xyz>0 0 1</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.0099 0.002 -0.05 0 0 0</pose>
@@ -148,7 +148,7 @@
         <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/camera_enclosure.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -161,7 +161,7 @@
         <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/camera_enclosure.dae</uri>
           </mesh>
         </geometry>
       </collision>
@@ -227,11 +227,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.045 0.0021 0.0199 0 0 0</pose>

diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf
@@ -7,9 +7,9 @@
     </include>
 
     <include merge="true">
-      <uri>package://ardupilot_gazebo/models/gimbal_small_2d</uri>
+      <uri>package://ardupilot_gazebo/models/gimbal_small_3d</uri>
       <name>gimbal</name>
-      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>    
+      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>
     </include>
 
     <joint name="gimbal_joint" type="revolute">
@@ -194,21 +194,6 @@
       <joint_name>rotor_3_joint</joint_name>
     </plugin>
 
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>roll_joint</joint_name>
-      <topic>/gimbal/cmd_roll</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>tilt_joint</joint_name>
-      <topic>/gimbal/cmd_tilt</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-
     <plugin filename="gz-sim-linearbatteryplugin-system"
       name="gz::sim::systems::LinearBatteryPlugin">
       <battery_name>lipo_3500mAh</battery_name>
@@ -224,6 +209,7 @@
 
     <plugin name="ArduPilotPlugin"
       filename="ArduPilotPlugin">
+      <!-- Port settings -->
       <fdm_addr>127.0.0.1</fdm_addr>
       <fdm_port_in>9002</fdm_port_in>
       <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
@@ -238,7 +224,6 @@
       <!-- Sensors -->
       <imuName>imu_link::imu_sensor</imuName>
 
-      <!-- Control channels -->
       <!--
           incoming control command [0, 1]
           so offset it by 0 to get [0, 1]
@@ -318,29 +303,65 @@
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
 
-      <control channel="4">
+      <!-- roll range is -30 to +30 deg -->
+      <control channel="8">
         <jointName>roll_joint</jointName>
-        <multiplier>3.14159265</multiplier>
+        <multiplier>1.047197551196</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
         <servo_max>1900</servo_max>
         <type>COMMAND</type>
         <cmd_topic>/gimbal/cmd_roll</cmd_topic>
-        <p_gain>3</p_gain>
+        <p_gain>2</p_gain>
+      </control>
+
+      <!-- pitch range is -135 to +45 deg -->
+      <control channel="9">
+        <jointName>pitch_joint</jointName>
+        <multiplier>-3.14159265</multiplier>
+        <offset>-0.75</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>COMMAND</type>
+        <cmd_topic>/gimbal/cmd_pitch</cmd_topic>
+        <p_gain>2</p_gain>
       </control>
 
-      <control channel="5">
-        <jointName>tilt_joint</jointName>
-        <multiplier>3.14159265</multiplier>
+      <!-- yaw range is -160 to +160 deg -->
+      <control channel="10">
+        <jointName>yaw_joint</jointName>
+        <multiplier>-5.5850536</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
         <servo_max>1900</servo_max>
         <type>COMMAND</type>
-        <cmd_topic>/gimbal/cmd_tilt</cmd_topic>
-        <p_gain>3</p_gain>
+        <cmd_topic>/gimbal/cmd_yaw</cmd_topic>
+        <p_gain>2</p_gain>
       </control>
 
     </plugin>
 
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>roll_joint</joint_name>
+      <topic>/gimbal/cmd_roll</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>pitch_joint</joint_name>
+      <topic>/gimbal/cmd_pitch</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>yaw_joint</joint_name>
+      <topic>/gimbal/cmd_yaw</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+
   </model>
 </sdf>
diff --git a/worlds/iris_runway.sdf b/worlds/iris_runway.sdf
@@ -116,7 +116,7 @@
     </include>
 
     <include>
-      <uri>model://iris_with_ardupilot</uri>
+      <uri>model://iris_with_gimbal</uri>
       <pose degrees="true">0 0 0.195 0 0 90</pose>
     </include>
 

diff --git a/worlds/iris_warehouse.sdf b/worlds/iris_warehouse.sdf
@@ -250,7 +250,7 @@
 
     <include>
         <pose>-6 0 0.25 0 0 0</pose>
-        <uri>model://iris_with_ardupilot</uri>
+        <uri>model://iris_with_gimbal</uri>
     </include>
 
     <include>