cheetah-software程序分析之 ./common/FootStepPlanner

前言

由于目前没有找到对MIT Mini Cheetah源码的解读而多是介绍仿真步骤，因此我将逐步的对其进行分析，其中的不足之处和不理解之处请多多提出，以共同进步。

CMakeLists.txt

先上代码

include_directories("./")
include_directories("../../common/include/")
file(GLOB_RECURSE sources
        "./*.cpp")

add_library(footstep_planner SHARED ${sources})
target_link_libraries(footstep_planner biomimetics)

代码很简单，就是包含当前目录和上层 的include目录，生成一个名为footstep_planner的动态链接库。

GraphSearch.h

该程序模块的主要作用是构建每个time step的质心状态轨迹，包括机器人在该时间的位置，速度和转角。对于footstepplanner的程序来说，我觉得它应该还有使用论文中 ¹ 提出的落足点选择的功能，但是找遍了全部的程序，依然看不见这几个公式的出现。。。。

接下来就一起看看程序是如何实现计算状态轨迹的：

/* 该部分是用来计算机体的轨迹 ，主要函数为buildInputTrajectory();*/

#ifndef CHEETAH_SOFTWARE_GRAPHSEARCH_H
#define CHEETAH_SOFTWARE_GRAPHSEARCH_H

#include <vector>
#include "cppTypes.h"

//接触状态结构体
struct ContactState {
  union {
    bool contact[4];
    struct {
      bool fr, fl, rr, rl;
    };
  };

  ContactState(bool _fr, bool _fl, bool _rr, bool _rl) {
    fr = _fr;
    fl = _fl;
    rr = _rr;
    rl = _rl;
  }

  ContactState() { }
};

//默认步态，trot，standing
struct DefaultGaits {
  std::vector<ContactState> trotting, standing;
};
//输入轨迹状态，应该是当前的基座状态，包含p,v,theta
struct InputTrajectoryState {
  Vec2<float> p;
  Vec2<float> v;
  float theta;
};

//足底状态，位置p，接触状态contact，状态时间stateTime
struct FootplanFootState {
  Vec2<float> p;
  bool contact;
  float stateTime;
};

//整体状态，t应该是周期，基坐标位置pBase，四个足底状态feet[4]
struct FootplanState {
  float t;
  Vec2<float> pBase;
  FootplanFootState feet[4];
};

//状态缓冲，nodesVisited,最大容量maxMenory
struct FootplanStats {
  u64 nodesVisited;
  u64 maxMemory;
  
  FootplanStats() {
  //两个参数归零
    reset();
  }

  void reset() {
    nodesVisited = 0;
    maxMemory = 0;
  }
};
//基座期望目标位置
struct FootplanGoal {
  Vec2<float> goalPos;
};

using FootplanStateCost = float (*)(FootplanState&, FootplanGoal&);
using FootplanTransitionCost = float (*)(FootplanState&, FootplanState&, FootplanGoal&);

namespace FootplanCosts {
  //计算当前基座位置到目标基座位置的距离
  float distanceToGoal(FootplanState& state, FootplanGoal& goal);
}

//  cheetah._bodyLength = 0.19 * 2;
//  cheetah._bodyWidth = 0.049 * 2;

class FootstepPlanner {
public:
  FootstepPlanner(bool verbose);
  void reset();
  //根据输入构造轨迹，输入步态周期duration,单位时间dt，输入轨迹状态x0，角速度omega
  void buildInputTrajectory(float duration, float dt, InputTrajectoryState x0, float omega);
  void planFixedEvenGait(std::vector<ContactState>& gait, float gait_period);
  std::vector<InputTrajectoryState>& getInitialTrajectory() {
    return _inputTrajectory;
  }

  void addCost(FootplanStateCost cost) {
    _stateCosts.push_back(cost);
  }

  void addCost(FootplanTransitionCost cost) {
    _transitionCosts.push_back(cost);
  }

  FootplanGoal& getGoal() {
    return _goal;
  }

  DefaultGaits defaults;
private:
  bool _verbose;
  FootplanStats _stats;
  FootplanGoal _goal;

  std::vector<FootplanStateCost> _stateCosts;
  std::vector<FootplanTransitionCost> _transitionCosts;
  std::vector<InputTrajectoryState> _inputTrajectory;
};


#endif //CHEETAH_SOFTWARE_GRAPHSEARCH_H

可以看到，.h文件对必要的结构体做了定义，虽然有对足底状态的结构体定义，但是并没有对这部分数据的计算。

GraphSearch.cpp

.cpp文件最主要的就是buildInputTrajectory函数，该函数按照时间步去计算对应时间下的质心状态。

#include "GraphSearch.h"
#include "Math/orientation_tools.h"

float FootplanCosts::distanceToGoal(FootplanState &state, FootplanGoal &goal) {
  Vec2<float> dp = state.pBase - goal.goalPos;
  return dp.norm();//向量范数，即距离
}

FootstepPlanner::FootstepPlanner(bool verbose) : _verbose(verbose) {
  _stats.reset();
  defaults.trotting = {{true, false, false, true},
                       {false, true, true, false}};
  defaults.standing = {{true, true, true, true}};
}

void FootstepPlanner::reset() {
  _stats.reset();
  _stateCosts.clear();
  _transitionCosts.clear();
}


void FootstepPlanner::buildInputTrajectory(float duration, float dt, InputTrajectoryState x0, float omega) {
  if(_verbose) {
    printf("Input trajectory with %d steps\n", (int)(duration / dt));
  }
  _inputTrajectory.clear();
  _inputTrajectory.reserve(duration / dt);

  Vec3<float> velocity(x0.v[0], x0.v[1], 0.f);
  Vec3<float> position(x0.p[0], x0.p[1], 0.f);
  float theta = x0.theta;
  float t = 0;
  for(uint32_t i = 0; i < (duration / dt); i++) {
    Vec3<float> vRot = ori::coordinateRotation(ori::CoordinateAxis::Z, theta).transpose() * velocity;

    _inputTrajectory.push_back({{position[0], position[1]}, {vRot[0], vRot[1]}, theta}); //给出周期内每个time step的轨迹信息

    position += vRot * dt;
    t += dt;
    theta += omega * dt;
  }
}

void FootstepPlanner::planFixedEvenGait(std::vector<ContactState> &gait, float gait_period) {
  (void)gait;
  (void)gait_period;
}

总结

FootStepPlanner部分主要功能就是计算质心的状态轨迹，按照相关论文中的描述，个人认为该部分是应该包括落足点的选择的。在仿真过程中可以看到的是通过手柄对机器人进行控制，其速度改变的同时似乎对应的步长变长了，说明整体的程序中是有这么一个落足点选择函数的，但是我没有找到，还望多多交流。

---

1. Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control ↩︎