#include <Eigen/Dense>
#include "params.h"

// 机器狗参数
double H_X = 0.715;    // (2.2)
double H_Y = 0.25;     // (2.2)
double L_1 = 0.05;     // (2.2)
double L_2 = 0.37;     // (2.2)
double L_3 = 0.40;     // (2.2)
double Mass = 14.4097; // 质量
// 机器狗惯性张量
double I_B_array[9] = {0.110, 0.000,  -0.007, 0.000, 0.893,
                       0.000, -0.007, 0.000,  0.900};

double p_x_offset = -0.018;
double kp = 0.15;

// 控制周期
double DELTA_T_MPC = 0.01; // 10ms
double DELTA_T_OTHERS = 0.064;

// MPC限制条件
double INF = 1e10;
double f_max = 1000.0;
double miu = 0.8;