diff --git a/libraries/SITL/SIM_Rover.cpp b/libraries/SITL/SIM_Rover.cpp index c1a7afb608..3182700dd9 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,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 - Vector3f velocity_body = dcm.transposed() * velocity_ef; + // update gyro and accel_body according to frame type + if (omni3) { + update_omni3(input, delta_time); + } else { + update_ackermann_or_skid(input, delta_time); + } - // 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 - accel_body = Vector3f(accel, 0, 0); - - // add in accel due to direction change - accel_body.y += radians(yaw_rate) * speed; + // common to all rovers // 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 diff --git a/libraries/SITL/SIM_Rover.h b/libraries/SITL/SIM_Rover.h index 42009a48a1..113bc55dfc 100644 --- a/libraries/SITL/SIM_Rover.h +++ b/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);