说明：该文章用于记录本人对斯坦福Doggo开源代码中的代码框架以及位控的理解，本人所在的实验室已经成功复现Doggo的功能。由于复现的工作由学长完成，所有可能以下有部分代码理解存在偏差，欢迎批评指正。

联系方式：2250017028@qq.com

参考资料：
斯坦福Doggo开源代码
解码斯坦福开源狗DogGo–附硬件清单、源码、图纸、论文

1.代码框架

注：以下的分层是我根据整体代码的函数实现来分，即以主要的函数来进行划分，而非文件划分，具体的文件内容可以参考上面的第二个参考资料

（1）上层

整份代码体现出上层思想的就只有position_control文件中的THD_FUNCTION(PositionControlThread, arg) 函数，它实际上就是一个状态机，根据不同的状态来选择不同的工作参数或者模式。

THD_FUNCTION(PositionControlThread, arg) {
    (void)arg;

    SetODriveCurrentLimits(CURRENT_LIM);
    chThdSleepMilliseconds(100);
    SetODriveCurrentLimits(CURRENT_LIM);

    while(true) {

        struct GaitParams gait_params = state_gait_params[state];

        switch(state) {
            case STOP:
                {
                    LegGain stop_gain = {50, 0.5, 50, 0.5};
                    float y1 = 0.15;
                    float y2 = 0.15;
                    float theta1, gamma1, theta2, gamma2;
                    CartesianToThetaGamma(0.0, y1, 1, theta1, gamma1);
                    CartesianToThetaGamma(0.0, y2, 1, theta2, gamma2);

                    odrv0Interface.SetCoupledPosition(theta2, gamma2, stop_gain);
                    odrv1Interface.SetCoupledPosition(theta1, gamma1, stop_gain);
                    odrv2Interface.SetCoupledPosition(theta1, gamma1, stop_gain);
                    odrv3Interface.SetCoupledPosition(theta2, gamma2, stop_gain);
                }
                break;
            case DANCE:
                gait(gait_params, 0.0, 0.5, 0.0, 0.5, gait_gains);
                break;
            case BOUND:
                gait(gait_params, 0.0, 0.5, 0.5, 0.0, gait_gains);
                break;
            case TROT:
                gait(gait_params, 0.0, 0.5, 0.0, 0.5, gait_gains);
                break;
            case TURN_TROT:
                gait(gait_params, 0.0, 0.5, 0.0, 0.5, gait_gains);
                break;
            case WALK:
                gait(gait_params, 0.0, 0.25, 0.75, 0.5, gait_gains);
                break;
            case PRONK:
                gait(gait_params, 0.0, 0.0, 0.0, 0.0, gait_gains);
                break;
            case JUMP:
                ExecuteJump();
                break;
            case ROTATE:
                {
                float theta,gamma;
                CartesianToThetaGamma(0, 0.24, 1.0, theta, gamma);
                float freq = 0.1;
                float phase = freq * (millis() - rotate_start)/1000.0f;
                theta = (-cos(2*PI * phase) + 1.0f) * 0.5 * 2 * PI;
                CommandAllLegs(theta, gamma, gait_gains);
                }
            case HOP:
                hop(gait_params);
                break;
            case FLIP:
                ExecuteFlip(gait_params);
                break;
            case RESET:
                reset();
                break;
            case TEST:
                test();
                break;
        }

        chThdSleepMicroseconds(1000000/POSITION_CONTROL_FREQ);
    }
}

（2）中层

1. 步态参数表
   stance_height ----站立时身体重心距离地面高度
   down_AMP -----Y轴最低高度
   up_AMP----Y轴最大高度
   flight_percent ----一个周期中，离地时间的占比
   step_length----步长
   FREQ----运动频率（不是控制频率）
   step_diff----两腿的相位差

// {stance_height, down_AMP, up_AMP, flight_percent (proportion), step_length, FREQ，step_diff}
struct GaitParams state_gait_params[] = {
    //{s.h, d.a., u.a., f.p., s.l., fr., s.d.}
    {NAN, NAN, NAN, NAN, NAN, NAN, NAN}, // STOP
    {0.17, 0.04, 0.06, 0.35, 0.15, 2.0, 0.0}, // TROT
    {0.17, 0.04, 0.06, 0.35, 0.0, 2.0, 0.0}, // BOUND
    {0.15, 0.00, 0.06, 0.25, 0.0, 1.5, 0.0}, // WALK
    {0.12, 0.05, 0.0, 0.75, 0.0, 1.0, 0.0}, // PRONK
    {NAN, NAN, NAN, NAN, NAN, NAN, NAN}, // JUMP
    {0.15, 0.05, 0.05, 0.35, 0.0, 1.5, 0.0}, // DANCE
    {0.15, 0.05, 0.05, 0.2, 0.0, 1.0, 0.0}, // HOP
    {NAN, NAN, NAN, NAN, NAN, 1.0, NAN}, // TEST
    {NAN, NAN, NAN, NAN, NAN, NAN, NAN}, // ROTATE
    {0.15, 0.07, 0.06, 0.2, 0.0, 1.0, 0.0}, // FLIP
    {0.17, 0.04, 0.06, 0.35, 0.1, 2.0, 0.06}, // TURN_TROT
    {NAN, NAN, NAN, NAN, NAN, NAN, NAN} // RESET
};

2. 步态执行流程
   主要函数是position_control文件中的gait 函数

void gait(struct GaitParams params,
                float leg0_offset, float leg1_offset,
                float leg2_offset, float leg3_offset,
                struct LegGain gains) {

    struct GaitParams paramsR = params;
    struct GaitParams paramsL = params;
    paramsR.step_length -= params.step_diff;
    paramsL.step_length += params.step_diff;

    if (!IsValidGaitParams(paramsR) || !IsValidGaitParams(paramsL) || !IsValidLegGain(gains)) {
        return;
    }

    float t = millis()/1000.0;

    const float leg0_direction = -1.0;
    CoupledMoveLeg(odrv0Interface, t, paramsL, leg0_offset, leg0_direction, gains,
        global_debug_values.odrv0.sp_theta, global_debug_values.odrv0.sp_gamma);

    const float leg1_direction = -1.0;
    CoupledMoveLeg(odrv1Interface, t, paramsL, leg1_offset, leg1_direction, gains,
        global_debug_values.odrv1.sp_theta, global_debug_values.odrv1.sp_gamma);

    const float leg2_direction = 1.0;
    CoupledMoveLeg(odrv2Interface, t, paramsR, leg2_offset, leg2_direction, gains,
        global_debug_values.odrv2.sp_theta, global_debug_values.odrv2.sp_gamma);

    const float leg3_direction = 1.0;
    CoupledMoveLeg(odrv3Interface, t, paramsR, leg3_offset, leg3_direction, gains,
        global_debug_values.odrv3.sp_theta, global_debug_values.odrv3.sp_gamma);
}

流程的思维导图如下：

函数具体实现就不作分析了，根据函数名即可知道各个函数的具体作用。

（3）底层

主要包括以下几个部分

uart ----串口
usb_serial ----USB串口
Odrive ----电机驱动
imu ----陀螺仪，用于获取身体姿态

对于不同的主控以及外设的型号，底层的驱动代码都不尽相同，所以这里就不展开对底层代码的分析。在复现的时候，最好是根据使用的各个外设型号，去编写对应的驱动代码。

2.步态实现分析

说明：这里只分析跳跃(jump)和后空翻(backfilp)两种步态，因为只有这两种步态需要不断调整步态参数

（1）jump

整个流程十分清晰
腿：准备->缩腿->伸腿->恢复
狗：站立->趴下->跳起->站立

/**
* Drives the ODrives in an open-loop, position-control sinTrajectory.
*/
void ExecuteJump() {
    const float prep_time = 0.5f; // Duration before jumping [s]
    const float launch_time = 0.8f ; // Duration before retracting the leg [s]
    const float fall_time = 1.0f; //Duration after retracting leg to go back to normal behavior [s]
    const float stance_height = 0.081f; // Desired leg extension before the jump [m]
    const float jump_extension = 0.249f; // Maximum leg extension in [m]
    const float fall_extension = 0.13f; // Desired leg extension during fall [m]

    float t = millis()/1000.0f - start_time_; // Seconds since jump was commanded

    if (t < prep_time) {
        float x = 0;
        float y = stance_height;
        float theta,gamma;
        CartesianToThetaGamma(x, y, 1.0, theta, gamma);

        // Use gains with small stiffness and lots of damping
        struct LegGain gains = {50, 1.0, 50, 1.0};
        CommandAllLegs(theta,gamma,gains);
        // Serial << "Prep: +" << t << "s, y: " << y;
    } else if (t >= prep_time && t < prep_time + launch_time) {
        float x = 0;
        float y = jump_extension;
        float theta, gamma;
        CartesianToThetaGamma(x, y, 1.0, theta, gamma);

        // Use high stiffness and low damping to execute the jump
        struct LegGain gains = {240, 0.5, 240, 0.2};
        CommandAllLegs(theta, gamma, gains);
        // Serial << "Jump: +" << t << "s, y: " << y;
    } else if (t >= prep_time + launch_time && t < prep_time + launch_time + fall_time) {
        float x = 0;
        float y = fall_extension;
        float theta,gamma;
        CartesianToThetaGamma(x, y, 1.0, theta, gamma);

        // Use low stiffness and lots of damping to handle the fall
        struct LegGain gains = {50, 1.0, 50, 1.0};

        CommandAllLegs(theta, gamma, gains);
        // Serial << "Retract: +" << t << "s, y: " << y;
    } else {
        state = STOP;
        Serial.println("Jump Complete.");
    }
    // Serial << '\n';
}

注意：

1. 弹跳与x轴无关，所以这里只考虑y轴的运动。
2. jump_extension 和 fall_extension这两个变量表示的是弹跳和下落的起始高度，由于y轴以下为正，所以前者大于后者

（2）backflip

后空翻的整个流程需要结合上面的动图来看。翻转主要是利用了狗腿伸缩产生的力作为弹跳力，在空中时，每只腿都保持与身体垂直，这个时候电机产生的扭矩也会带动身体运动。

void ExecuteFlip(struct GaitParams params) {
    const float prep_time = 0.6f; // Duration before jumping [s]
    const float launch_time = 0.1f; // Duration before retracting the leg [s]

    struct LegGain rear_gains = {120,1,80,1};
    float rear_up_len = 0.08;
    float rear_down_len = 0.24;

    float t = millis()/1000.0f - flip_start_time_;
    float pitch = global_debug_values.imu.pitch;

    float theta=0,gamma=0;

    // Preparing to jump
    // 1.准备状态,各腿与身体垂直
    if (t < prep_time) {
        float y_back = rear_up_len;
        CommandLegsThetaY(pitch, y_back, rear_gains, BACK);
		
        float y_front = params.stance_height - params.up_amp;
        CommandLegsThetaY(pitch, y_front, gait_gains, FRONT);

    // Push off with front feet
    // 2.前腿降低，准备起跳
    } else if (t >= prep_time && t < prep_time + launch_time) {
        float y_back = rear_up_len;
        CommandLegsThetaY(pitch, y_back, rear_gains, BACK);
		
        float y_front = params.stance_height + params.down_amp;
        CommandLegsThetaY(pitch, y_front, gait_gains, FRONT);

    // Rotate front legs to catch
    // 3.前腿上升，产生弹跳力
} else if (pitch < 90.0*M_PI/180.0) {

        float y_back = rear_up_len;
        CommandLegsThetaY(pitch, y_back, rear_gains, BACK);
        
        float y_front = params.stance_height;
        CommandLegsThetaY(-pitch, y_front, gait_gains, FRONT);

    // Push off with back feet, keep rotating front legs to catch
    // 4.身体与地面垂直后，后腿降低，维持旋转状态
    } else  if (pitch < 130.0*M_PI/180.0) {
        float y_back = rear_down_len;
        CommandLegsThetaY(pitch, y_back, gait_gains, BACK);

        float y_front = params.stance_height;
        CommandLegsThetaY(-pitch, y_front, gait_gains, FRONT);
    // 5.后腿上升，产生弹跳力，使之继续旋转
    } else if (pitch < 180.0*M_PI/180.0){
        float y_back = params.stance_height;
        CommandLegsThetaY(pitch, y_back, gait_gains, BACK);

        float y_front = params.stance_height;
        CommandLegsThetaY(-pitch, y_front, gait_gains, FRONT);
    // 6.各腿保持与身体垂直，直至落地
    } else {
        struct LegGain landing_gains = {120,1,50,1.5};
        float y_back = params.stance_height;
        CommandLegsThetaY(pitch, y_back, landing_gains, BACK);

        float y_front = params.stance_height;
        CommandLegsThetaY(pitch - 2*M_PI, y_front, landing_gains, FRONT);
    }
}