From 4354072d34c99b5b30fedc0c8ab2411d93c6a733 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 17 Jul 2024 16:13:40 +0100 Subject: [PATCH] SITL: SIM_Rover: add simulation for omni3 mecanum rover Signed-off-by: Rhys Mainwaring --- libraries/SITL/SIM_Rover.cpp | 142 ++++++++++++++++++++++++++++------- libraries/SITL/SIM_Rover.h | 9 +++ 2 files changed, 125 insertions(+), 26 deletions(-) diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index c1a7afb608f445..3182700dd9ad04 100644 --- a/libraries/SITL/SIM_Rover.cpp +++ b/libraries/SITL/SIM_Rover.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace SITL { SimRover::SimRover(const char *frame_str) : @@ -41,10 +43,14 @@ SimRover::SimRover(const char *frame_str) : if (vectored_thrust) { printf("Vectored Thrust Rover Simulation Started\n"); } - lock_step_scheduled = true; -} + 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 @@ -92,6 +98,50 @@ float SimRover::calc_lat_accel(float steering_angle, float speed) update the rover simulation by one time step */ void SimRover::update(const struct sitl_input &input) +{ + // how much time has passed? + float delta_time = frame_time_us * 1.0e-6f; + + // update gyro and accel_body according to frame type + if (omni3) { + update_omni3(input, delta_time); + } else { + update_ackermann_or_skid(input, delta_time); + } + + // common to all rovers + + // now in earth frame + Vector3f accel_earth = dcm * accel_body; + accel_earth += Vector3f(0, 0, GRAVITY_MSS); + + // we are on the ground, so our vertical accel is zero + accel_earth.z = 0; + + // work out acceleration as seen by the accelerometers. It sees the kinematic + // acceleration (ie. real movement), plus gravity + accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); + + // new velocity vector + velocity_ef += accel_earth * delta_time; + + // new position vector + position += (velocity_ef * delta_time).todouble(); + + update_external_payload(input); + + // update lat/lon/altitude + update_position(); + time_advance(); + + // update magnetic field + 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; @@ -113,9 +163,6 @@ void SimRover::update(const struct sitl_input &input) } } - // how much time has passed? - float delta_time = frame_time_us * 1.0e-6f; - // speed in m/s in body frame Vector3f velocity_body = dcm.transposed() * velocity_ef; @@ -137,37 +184,80 @@ void SimRover::update(const struct sitl_input &input) dcm.rotate(gyro * delta_time); dcm.normalize(); - // accel in body frame due to motor + // 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; +} - // now in earth frame - Vector3f accel_earth = dcm * accel_body; - accel_earth += Vector3f(0, 0, GRAVITY_MSS); - - // we are on the ground, so our vertical accel is zero - accel_earth.z = 0; - - // work out acceleration as seen by the accelerometers. It sees the kinematic - // acceleration (ie. real movement), plus gravity - accel_body = dcm.transposed() * (accel_earth + Vector3f(0, 0, -GRAVITY_MSS)); +/* + 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; - // new velocity vector - velocity_ef += accel_earth * delta_time; + // speed in m/s in body frame + Vector3f velocity_body = dcm.transposed() * velocity_ef; - // new position vector - position += (velocity_ef * delta_time).todouble(); + // 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; - update_external_payload(input); + gyro = Vector3f(0, 0, twist.z); - // update lat/lon/altitude - update_position(); - time_advance(); + // update attitude + dcm.rotate(gyro * delta_time); + dcm.normalize(); - // update magnetic field - update_mag_field_bf(); + // accel in body frame due to motors (excluding gravity) + accel_body = Vector3f(accel_x, accel_y, 0); } } // namespace SITL diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 42009a48a177f7..113bc55dfc1a82 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/libraries/SITL/SIM_Rover.h @@ -51,6 +51,15 @@ class SimRover : public Aircraft { 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);