SITL: SIM_Rover: add simulation for omni3 mecanum rover

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

libraries/SITL/SIM_Rover.cpp

@@ -21,6 +21,8 @@
 #include <string.h>
 #include <stdio.h>
 
+#include <AP_Math/AP_Math.h>
+
 namespace SITL {
 
 SimRover::SimRover(const char *frame_str) :
@@ -41,11 +43,15 @@ SimRover::SimRover(const char *frame_str) :
     if (vectored_thrust) {
         printf("Vectored Thrust Rover Simulation Started\n");
     }
+
+    omni3 = strstr(frame_str, "omni3mecanum") != nullptr;
+    if (omni3) {
+        printf("Omni3 Mecanum Rover Simulation Started\n");
+    }
+
     lock_step_scheduled = true;
 }
 
-
-
 /*
   return turning circle (diameter) in meters for steering angle proportion in degrees
 */
@@ -93,55 +99,17 @@ float SimRover::calc_lat_accel(float steering_angle, float speed)
  */
 void SimRover::update(const struct sitl_input &input)
 {
-    float steering, throttle;
-
-    // if in skid steering mode the steering and throttle values are used for motor1 and motor2
-    if (skid_steering) {
-        float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
-        float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
-        steering = motor1 - motor2;
-        throttle = 0.5*(motor1 + motor2);
-    } else {
-        steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
-        throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
-
-        // vectored thrust conversion
-        if (vectored_thrust) {
-            const float steering_angle_rad = radians(steering * vectored_angle_max);
-            steering = sinf(steering_angle_rad) * throttle;
-            throttle *= cosf(steering_angle_rad);
-        }
-    }
-
     // how much time has passed?
     float delta_time = frame_time_us * 1.0e-6f;
 
-    // speed in m/s in body frame
+    // update gyro and accel_body according to frame type
-    Vector3f velocity_body = dcm.transposed() * velocity_ef;
+    if (omni3) {
+        update_omni3(input, delta_time);
+    } else {
+        update_ackermann_or_skid(input, delta_time);
+    }
 
-    // speed along x axis, +ve is forward
+    // common to all rovers
-    float speed = velocity_body.x;
-
-    // yaw rate in degrees/s
-    float yaw_rate = calc_yaw_rate(steering, speed);
-
-    // target speed with current throttle
-    float target_speed = throttle * max_speed;
-
-    // linear acceleration in m/s/s - very crude model
-    float accel = max_accel * (target_speed - speed) / max_speed;
-
-    gyro = Vector3f(0,0,radians(yaw_rate));
-
-    // update attitude
-    dcm.rotate(gyro * delta_time);
-    dcm.normalize();
-
-    // accel in body frame due to motor
-    accel_body = Vector3f(accel, 0, 0);
-
-    // add in accel due to direction change
-    accel_body.y += radians(yaw_rate) * speed;
 
     // now in earth frame
     Vector3f accel_earth = dcm * accel_body;
@@ -170,4 +138,126 @@ void SimRover::update(const struct sitl_input &input)
     update_mag_field_bf();
 }
 
+/*
+  update the ackermann or skid rover simulation by one time step
+ */
+void SimRover::update_ackermann_or_skid(const struct sitl_input &input, float delta_time)
+{
+    float steering, throttle;
+
+    // if in skid steering mode the steering and throttle values are used for motor1 and motor2
+    if (skid_steering) {
+        float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+        float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+        steering = motor1 - motor2;
+        throttle = 0.5*(motor1 + motor2);
+    } else {
+        steering = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+        throttle = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+
+        // vectored thrust conversion
+        if (vectored_thrust) {
+            const float steering_angle_rad = radians(steering * vectored_angle_max);
+            steering = sinf(steering_angle_rad) * throttle;
+            throttle *= cosf(steering_angle_rad);
+        }
+    }
+
+    // speed in m/s in body frame
+    Vector3f velocity_body = dcm.transposed() * velocity_ef;
+
+    // speed along x axis, +ve is forward
+    float speed = velocity_body.x;
+
+    // yaw rate in degrees/s
+    float yaw_rate = calc_yaw_rate(steering, speed);
+
+    // target speed with current throttle
+    float target_speed = throttle * max_speed;
+
+    // linear acceleration in m/s/s - very crude model
+    float accel = max_accel * (target_speed - speed) / max_speed;
+
+    gyro = Vector3f(0,0,radians(yaw_rate));
+
+    // update attitude
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    // accel in body frame due to motor (excluding gravity)
+    accel_body = Vector3f(accel, 0, 0);
+
+    // add in accel due to direction change
+    accel_body.y += radians(yaw_rate) * speed;
+}
+
+/*
+  update the omni3 rover simulation by one time step
+ */
+void SimRover::update_omni3(const struct sitl_input &input, float delta_time)
+{
+    // in omni3 mode the first three servos are motor speeds
+    float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
+    float motor2 = 2*((input.servos[1]-1000)/1000.0f - 0.5f);
+    float motor3 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
+
+    // use forward kinematics to calculate body frame velocity
+    Vector3f wheel_ang_vel(
+      motor1 * omni3_wheel_max_ang_vel,
+      motor2 * omni3_wheel_max_ang_vel,
+      motor3 * omni3_wheel_max_ang_vel
+    );
+
+    // derivation of forward kinematics for an Omni3Mecanum rover
+    // A. Gfrerrer. "Geometry and kinematics of the Mecanum wheel",
+    // Computer Aided Geometric Design 25 (2008) 784–791.
+    // Retrieved from https://www.geometrie.tugraz.at/gfrerrer/publications/MecanumWheel.pdf.
+    //
+    // the frame is equilateral triangle
+    //
+    // d[i] = 0.18 m is distance from frame centre to each wheel 
+    // r_w = 0.04725 m is the wheel radius.
+    // delta = radians(-45) is angle of the roller to the direction of forward rotation
+    // alpha[i] is the angle the wheel axis is rotated about the body z-axis
+    // c[i] = cos(alpha[i] + delta)
+    // s[i] = sin(alpha[i] + delta)
+    //
+    // wheel  d[i]  alpha[i]  a_x[i]   a_y[i]      c[i]      s[i]
+    //     1  0.18   1.04719    0.09  0.15588  0.965925  0.258819
+    //     2  0.18   3.14159   -0.18  0.0     -0.707106  0.707106
+    //     3  0.18   5.23598    0.09 -0.15588 -0.258819 -0.965925
+    //
+    //  k = 1/(r_w * sin(delta)) = -29.930445 is a scale factor
+    //
+    // inverse kinematic matrix
+    // M[i, 0] = k * c[i]
+    // M[i, 1] = k * s[i]
+    // M[i, 2] = k * (a_x[i] s[i] - a_y[i] c[i])
+    //
+    // forward kinematics matrix: Minv = M^-1
+    constexpr Matrix3f Minv(
+      -0.0215149, 0.01575, 0.0057649,
+      -0.0057649, -0.01575, 0.0215149,
+      0.0875, 0.0875, 0.0875);
+
+    // twist - this is the target linear and angular velocity
+    Vector3f twist = Minv * wheel_ang_vel;
+
+    // speed in m/s in body frame
+    Vector3f velocity_body = dcm.transposed() * velocity_ef;
+
+    // linear acceleration in m/s/s - very crude model
+    float accel_x = omni3_max_accel * (twist.x - velocity_body.x) / omni3_max_speed;
+    float accel_y = omni3_max_accel * (twist.y - velocity_body.y) / omni3_max_speed;
+
+    gyro = Vector3f(0, 0, twist.z);
+
+    // update attitude
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    // accel in body frame due to motors (excluding gravity)
+    accel_body = Vector3f(accel_x, accel_y, 0);
+}
+
 } // namespace SITL

libraries/SITL/SIM_Rover.h

@@ -51,6 +51,15 @@ private:
     float vectored_angle_max = 90.0f;       // maximum angle (in degrees) to which thrust can be turned
     float vectored_turn_rate_max = 90.0f;   // maximum turn rate (in deg/sec) with full throttle angled at 90deg
 
+    // omni3 Mecanum related members
+    bool omni3;                             // true if vehicle is omni-directional with 3 Mecanum wheels
+    float omni3_max_speed = 2.3625f;        // omni vehicle's maximum forward speed in m/s
+    float omni3_max_accel = 1.0f;           // omni vehicle's maximum forward acceleration in m/s/s
+    float omni3_wheel_max_ang_vel = 50.0f;  // omni vehicle's wheel maximum angular velocity in rad/s
+
+    void update_ackermann_or_skid(const struct sitl_input &input, float delta_time);
+    void update_omni3(const struct sitl_input &input, float delta_time);
+
     float turn_circle(float steering) const;
     float calc_yaw_rate(float steering, float speed);
     float calc_lat_accel(float steering_angle, float speed);