Orbit: add rover model

- Use Imu sensors with custom gravity
- Adjust earth mass to give surface accel of 10 m/s^2
- Add rover model

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Author: srmainwaring

src/GravityPlugin.cc

@@ -63,6 +63,9 @@ class GravityPlugin::Impl
   /// \brief Flag set to true if the plugin is initialised.
   public: bool initialised{false};
 
+  /// \brief Power law exponent. Default is inverse square.
+  public: double gravity_expo{-2.0};
+
   /// \brief Universal gravitational constant (N m^2 / kg^2).
   public: static constexpr double G{6.67430E-11};
 };
@@ -98,9 +101,9 @@ void GravityPlugin::Configure(
   }
 
   // parameters
-  if (_sdf->HasElement("source"))
+  if (_sdf->HasElement("gravity_source"))
   {
-    this->impl->sourceName = _sdf->Get<std::string>("source");
+    this->impl->sourceName = _sdf->Get<std::string>("gravity_source");
   }
   else
   {
@@ -109,6 +112,11 @@ void GravityPlugin::Configure(
     return;
   }
 
+  if (_sdf->HasElement("gravity_expo"))
+  {
+    this->impl->gravity_expo = _sdf->Get<double>("gravity_expo");
+  }
+
   if (_sdf->HasElement("debug"))
   {
     this->impl->debug = _sdf->Get<bool>("debug");
@@ -207,10 +215,10 @@ void GravityPlugin::PreUpdate(
 
     // inverse radius
     double one_over_r = 1.0 / r;
-    double one_over_r2 = 1.0 / (r * r);
+    double r_expo = pow(r, this->impl->gravity_expo);
 
     // magnitude of force
-    auto f = 1.0 * this->impl->G * mass_C * mass_B * one_over_r2;
+    auto f = 1.0 * this->impl->G * mass_C * mass_B * r_expo;
 
     // direction of force
     auto direction = -1.0 * one_over_r * p_CB_W;

worlds/earth.sdf

@@ -22,12 +22,15 @@
     </plugin>
     <plugin filename="gz-sim-imu-system"
       name="gz::sim::systems::Imu">
+      <gravity_source>earth</gravity_source>
+      <gravity_expo>-2.0</gravity_expo>
     </plugin>
 
     <plugin filename="GravityPlugin"
       name="GravityPlugin">
       <debug>0</debug>
-      <source>earth</source>
+      <gravity_source>earth</gravity_source>
+      <gravity_expo>-2.0</gravity_expo>
     </plugin>
 
     <!-- disable planar gravity -->
@@ -53,7 +56,7 @@
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
-    <model name="enu_axes">
+    <!-- <model name="enu_axes">
       <static>1</static>
       <link name="link">
         <visual name="r">
@@ -105,34 +108,55 @@
           </material>
         </visual>
       </link>
-    </model>
+    </model> -->
 
     <!-- source of gravity -->
     <model name="earth">
       <static>0</static>
       <link name="earth">
         <gravity>0</gravity>
+        <!-- expo = -2.0 -->
         <inertial>
-          <mass>5.972E14</mass>
+          <mass>1.50E+13</mass>
           <inertia>
-            <ixx>2.38880E+16</ixx>
+            <ixx>5.99314E+14</ixx>
             <ixy>0</ixy>
             <ixz>0</ixz>
-            <iyy>2.38880E+16</iyy>
+            <iyy>5.99314E+14</iyy>
             <iyz>0</iyz>
-            <izz>2.38880E+16</izz>
+            <izz>5.99314E+14</izz>
           </inertia>
         </inertial>
+        <!-- expo = -0.5 -->
+        <!-- <inertial>
+          <mass>4.74E+11</mass>
+          <inertia>
+            <ixx>1.89520E+13</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1.89520E+13</iyy>
+            <iyz>0</iyz>
+            <izz>1.89520E+13</izz>
+          </inertia>
+        </inertial> -->
         <collision name="collision">
-          <pose degrees="true">0 0 0 90 0 0</pose>
+          <pose degrees="true">0 0 0 90 0 -90</pose>
           <geometry>
             <sphere>
               <radius>10</radius>
             </sphere>
           </geometry>
-        </collision>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>1.0</mu2>
+              </ode>
+            </friction>
+          </surface>
+          </collision>
         <visual name="visual">
-          <pose degrees="true">0 0 0 90 0 0</pose>
+          <pose degrees="true">0 0 0 90 0 -90</pose>
           <geometry>
             <sphere>
               <radius>10</radius>
@@ -154,7 +178,7 @@
 
       <plugin filename="gz-sim-velocity-control-system"
         name="gz::sim::systems::VelocityControl">
-        <initial_angular>0 0 0.1</initial_angular>
+        <initial_angular>0 0 0.0</initial_angular>
       </plugin>
 
     </model>
@@ -201,7 +225,7 @@
       <plugin filename="InitialVelocityPlugin"
         name="InitialVelocityPlugin">
         <debug>0</debug>
-        <linear>0 51.55 0</linear>
+        <linear>0 8.164965809 0</linear>
         <link>base_link</link>
         <p_gain>50</p_gain>
         <i_gain>1</i_gain>
@@ -255,7 +279,7 @@
       <plugin filename="InitialVelocityPlugin"
         name="InitialVelocityPlugin">
         <debug>0</debug>
-        <linear>-44.64 0 0</linear>
+        <linear>-7.071067812 0 0</linear>
         <link>base_link</link>
         <p_gain>50</p_gain>
         <i_gain>1</i_gain>
@@ -309,7 +333,7 @@
       <plugin filename="InitialVelocityPlugin"
         name="InitialVelocityPlugin">
         <debug>0</debug>
-        <linear>0 65.0 0</linear>
+        <linear>0 10.0 0</linear>
         <link>base_link</link>
         <p_gain>50</p_gain>
         <i_gain>1</i_gain>
@@ -321,5 +345,32 @@
       </plugin>
     </model>
 
+    <!-- <include>
+      <uri>model://iris_with_gimbal</uri>
+      <pose degrees="true">0 0 10.3 0 0 90</pose>
+    </include> -->
+
+
+    <!--
+      Earth uses ECEF frame
+        X - in plane of equator through centre and meridian 0
+        Y - in plane of equator through centre and meridian 90
+        Z - through centre and north pole 
+
+      Transforms to local NED for different starting positions
+      <10 0 0 90 0 90>
+      lat 0, lon 0, hdg 90
+      NED
+        X_ned =  Z_ecef
+        Y_ned =  Y_ecef
+        Z_ned = -X_ecef
+      <gazeboXYZToNED degrees="true">10 0 0 0 -90 0</gazeboXYZToNED>
+    -->
+
+    <include>
+      <uri>model://omni4rover</uri>
+      <pose degrees="true">10.3 0 0 90 -90 90</pose>
+    </include>
+
   </world>
 </sdf>