cheetah

编程入门行业动态更新时间:2024-10-16 16:01:54

cheetah

Cheetah-Software 程序分析之 ./common/FootStepPlanner

前言
CMakeLists.txt
GraphSearch.h
GraphSearch.cpp
总结

前言

由于目前没有找到对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，角速度omegavoid 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;
}