/* GENERATED by firmware/balance/tools/gen_model.py from build/sim/v2 (python -m sim.robot_v2) and the simulation's
 * controllers. Do not edit: re-run the generator when the model or the gains change (docs/body-firmware.md 8). */
#ifndef GP_MODEL_H
#define GP_MODEL_H

#define GP_MODEL_SOURCE "Gyropup v2 (five-bar), 36.754 kg, model+xml sha256 dd00ab2deb03a069"
#define GP_MODEL_MASS_KG 36.7543f

/* the five-bar (cad/fivebar.py PARAMS), m and rad */
#define GP_FB_D_M 0.15f
#define GP_FB_L1_M 0.15f
#define GP_FB_L2_M 0.25f
#define GP_FB_STOP_FRONT_OUT 0.0523598776f
#define GP_FB_STOP_FRONT_IN -1.69296937f
#define GP_FB_STOP_REAR_OUT -3.19395253f
#define GP_FB_STOP_REAR_IN -1.44862328f
#define GP_H_MIN_M 0.185f /* the controller's height 0 .. 1 (sim/robot_v2.py H_RANGE) */
#define GP_H_MAX_M 0.375f

/* the motors */
#define GP_LEG_PEAK_NM 120.0f
#define GP_LEG_CAP_NM 60.0f
#define GP_LEG_W_PER_V 0.436332313f /* rad/s of no-load speed per volt */
#define GP_WHEEL_R_M 0.127f
#define GP_WHEEL_PEAK_NM 15.0f
#define GP_WHEEL_W_PER_V 0.959931089f
#define GP_WHEEL_KT_NM_PER_A 1.0f
#define GP_WHEEL_POLE_PAIRS 15

/* the robot: masses and the legs' gravity feed-forward ((total - wheels) g / 2, sim/controller_v2.py) */
#define GP_ROBOT_WHEELS_KG 4.236f
#define GP_ROBOT_LEG_FF_N 159.502262f

/* sim/controller.py Gains */
#define GP_G_YAW_KP_PER_NM 0.25f
#define GP_G_YAW_SHARE 0.3f
#define GP_G_HEIGHT_RATE 0.6f
#define GP_G_SPEED_MARGIN 0.5f
#define GP_G_ACCEL 1.5f
#define GP_G_ROLL_RATE 4.0f
#define GP_G_ROLL_MAX 0.45f
/* LQR weights q = (40.0, 600.0, 30.0, 20.0), r = 2.0 */

/* sim/controller_v2.py LegGains: the robot's virtual legs */
#define GP_LEG_KP_L 9000.0f
#define GP_LEG_KD_L 350.0f
#define GP_LEG_KP_TH 450.0f
#define GP_LEG_KD_TH 14.0f

/* the leg rig profile: sim/leg_control.py Gains, sim/leg_rig.py run()'s tuck and feed-forward at the design mass */
#define GP_RIG_KP_L 3000.0f
#define GP_RIG_KD_L 250.0f
#define GP_RIG_KP_TH 150.0f
#define GP_RIG_KD_TH 6.0f
#define GP_TUCK_KP_L 3000.0f
#define GP_TUCK_KD_L 260.0f
#define GP_RIG_HALF_MASS_KG 18.75f
#define GP_RIG_LEG_FF_N 152.242577f

/* the centre-of-mass model (robot frame: x forward, z up, origin midway between the hip axes) */
#define GP_COM_BASE_KG 24.1709f
#define GP_COM_BASE_X 0.00185379841f
#define GP_COM_BASE_Z 0.0411912806f
/* checked against MuJoCo's pendulum angle at four placed heights: worst 2.57e-07 rad */

#define GP_LQR_N 21
#ifdef GP_MODEL_TABLES
/* each leg link: mass (kg) and centre (m) in its frame (x along the link): upper_f, lower_f, upper_r, lower_r */
static const float gp_com_link[4][3] = {
    {0.6477f, 0.05513f, 6e-05f}, /* upper_f */
    {0.7165f, 0.15396f, 0.0f}, /* lower_f */
    {0.6477f, 0.05513f, 6e-05f}, /* upper_r */
    {2.1618f, 0.2138f, 0.0f}, /* lower_r */
};
/* LQR gains K (u = -K [x - x_ref, theta, v - v_cmd, theta_dot], u the two wheels' torque, N m) by height 0..1;
 * the pendulum at each: body mass (kg), centre height over the axle (m), inertia (kg m^2) */
static const float gp_lqr_k[GP_LQR_N][4] = {
    {-4.47213595f, -71.7155142f, -9.45991023f, -11.4829022f}, /* h 0.00: 32.52 kg, l 0.1859 m, I 0.9489 */
    {-4.47213595f, -72.7937736f, -9.44010063f, -11.6191896f}, /* h 0.05: 32.52 kg, l 0.1937 m, I 0.9647 */
    {-4.47213595f, -73.8349876f, -9.42256831f, -11.7578231f}, /* h 0.10: 32.52 kg, l 0.2015 m, I 0.9810 */
    {-4.47213595f, -74.8413932f, -9.40707674f, -11.8987301f}, /* h 0.15: 32.52 kg, l 0.2093 m, I 0.9979 */
    {-4.47213595f, -75.8150701f, -9.39341939f, -12.0418303f}, /* h 0.20: 32.52 kg, l 0.2171 m, I 1.0154 */
    {-4.47213595f, -76.7579475f, -9.38141525f, -12.1870371f}, /* h 0.25: 32.52 kg, l 0.2250 m, I 1.0335 */
    {-4.47213595f, -77.6718134f, -9.37090503f, -12.3342597f}, /* h 0.30: 32.52 kg, l 0.2328 m, I 1.0521 */
    {-4.47213595f, -78.5583235f, -9.36174795f, -12.4834041f}, /* h 0.35: 32.52 kg, l 0.2406 m, I 1.0713 */
    {-4.47213595f, -79.4190109f, -9.35381913f, -12.6343739f}, /* h 0.40: 32.52 kg, l 0.2485 m, I 1.0911 */
    {-4.47213595f, -80.2552953f, -9.3470073f, -12.7870707f}, /* h 0.45: 32.52 kg, l 0.2563 m, I 1.1114 */
    {-4.47213595f, -81.0684914f, -9.34121289f, -12.9413941f}, /* h 0.50: 32.52 kg, l 0.2642 m, I 1.1323 */
    {-4.47213595f, -81.8598174f, -9.33634637f, -13.0972411f}, /* h 0.55: 32.52 kg, l 0.2721 m, I 1.1538 */
    {-4.47213595f, -82.6304014f, -9.33232682f, -13.2545058f}, /* h 0.60: 32.52 kg, l 0.2799 m, I 1.1758 */
    {-4.47213595f, -83.3812886f, -9.32908055f, -13.4130769f}, /* h 0.65: 32.52 kg, l 0.2878 m, I 1.1983 */
    {-4.47213595f, -84.1134457f, -9.3265399f, -13.5728355f}, /* h 0.70: 32.52 kg, l 0.2957 m, I 1.2213 */
    {-4.47213595f, -84.8277656f, -9.32464194f, -13.7336502f}, /* h 0.75: 32.52 kg, l 0.3036 m, I 1.2449 */
    {-4.47213595f, -85.525069f, -9.32332695f, -13.8953693f}, /* h 0.80: 32.52 kg, l 0.3115 m, I 1.2689 */
    {-4.47213595f, -86.2061035f, -9.32253638f, -14.0578058f}, /* h 0.85: 32.52 kg, l 0.3194 m, I 1.2932 */
    {-4.47213595f, -86.871536f, -9.32220945f, -14.2207077f}, /* h 0.90: 32.52 kg, l 0.3273 m, I 1.3180 */
    {-4.47213595f, -87.5219303f, -9.32227603f, -14.3836906f}, /* h 0.95: 32.52 kg, l 0.3352 m, I 1.3429 */
    {-4.47213595f, -88.1576765f, -9.32263731f, -14.5460444f}, /* h 1.00: 32.52 kg, l 0.3432 m, I 1.3679 */
};
#endif

#endif