towr code阅读

1. Introduction

towr是非常优美的足式机器人规划代码，通过阅读towr重要的几个迭代版本的代码深入了解。

2 v0.1

第一代的版本，foot的位置是提前给定的，只对COG的trajectory进行优化。

2.1 cost

• 公式
  仅仅只考虑加速度，
  $${\int }_0^T{\ddot{x}}^2(t)dt+v=q^{T}Gq+v$$

其中，
$$q={\left[\begin{array}{cccc}a& b& c& d\end{array}\right]}^T$$

• 代码

// 使用六阶多项式进行表示，有XY两个维度，需要优化的参数数目，用std::vecXd 表示
int n_coeff = splines.size() * kCoeffCount * kDim2d;
// 1e-12 < EndFCost < 1e-8, 初始值要非零
cf->M = EandFCost * Eigen::MatrixXd::Identity(cf->M.rows(), cf->M.cols()); // 初始化q和G的数据结构// 修改权重矩阵,将权重矩阵G进行分块，每一段占据一块，通过索引定位要对应的权重：X、Y、分段
for (const SplineInfo& s : splines) {std::array<double, 10> t_span = cache_exponents<10>(s.duration_);for (int dim = X; dim <= Y; dim++) {const int a  = var_index(s.id_, dim, A);const int b  = var_index(s.id_, dim, B);const int c  = var_index(s.id_, dim, C);const int d  = var_index(s.id_, dim, D);// for explanation of values see M.Kalakrishnan et al., page 248// "Learning, Planning and Control for Quadruped Robots over challenging// Terrain", IJRR, 2010// exact spline            cf->M(a, a) = 400.0 / 7.0      * t_span[7] * weight[dim];}
}
//具体每块的权重
cf->M(a, a) = 400.0 / 7.0      * t_span[7] * weight[dim];
cf->M(a, b) = 40.0             * t_span[6] * weight[dim];
cf->M(a, c) = 120.0 / 5.0      * t_span[5] * weight[dim];
cf->M(a, d) = 10.0             * t_span[4] * weight[dim];
cf->M(b, b) = 144.0 / 5.0      * t_span[5] * weight[dim];
cf->M(b, c) = 18.0             * t_span[4] * weight[dim];
cf->M(b, d) = 8.0              * t_span[3] * weight[dim];
cf->M(c, c) = 12.0             * t_span[3] * weight[dim];
cf->M(c, d) = 6.0              * t_span[2] * weight[dim];
cf->M(d, d) = 4.0              * t_span[1] * weight[dim];// mirrow values over diagonal to fill bottom left triangle
for (int c = 0; c < cf->M.cols(); ++c)for (int r = c + 1; r < cf->M.rows(); ++r)cf->M(r, c) = cf->M(c, r);

总结：标准QP类型，不需要提供Hessian 矩阵

2.2 equality constraints

• Continuity and Goal Constraints
  在QP问题上，表示为$$M\ast coeff+v=0$$
  
• 位置、速度和加速度的约束
  $$M{\left[\begin{array}{cccccc}a& b& c& d& e& f\end{array}\right]}^T+v=0$$
• 初始和终止状态约束code

// 计算constraints的数目
int coeff = splines.size() * kCoeffCount * kDim2d; // total number of all spline coefficients
int constraints = 0;
constraints += 2*4; // init {x,y} * {pos, vel, acc, jerk}
constraints += 2*3; // end  {x,y} * {pos, vel, acc, jerk}
constraints += (splines.size() - 1) * kDim2d * 4; // junctions {pos, vel, acc, jerk}
MatVecPtr ec(new MatVec(coeff, constraints, constraints));
// 满足初始状态
// positions at t = 0, M * V = cog
int f = var_index(0, dim, F);
ec->M(f, i) = 1.0;   // 权重
ec->v(i++) = -start_cog(dim);
// velocity set to zero
int e = var_index(0, dim, E);
ec->M(e, i) = 1.0;
ec->v(i++) = -kVelStart(dim);
// acceleration set to zero
int d = var_index(0, dim, D);
ec->M(d, i) = 2.0;
ec->v(i++) = -kAccStart(dim);
// jerk set to zero
int c = var_index(0, dim, C);
ec->M(c, i) = 6.0;
ec->v(i++) = -kJerkStart(dim); 

终止状态的约束

 ec->M(last_spline + A, i) = t_duration[5];ec->M(last_spline + B, i) = t_duration[4];ec->M(last_spline + C, i) = t_duration[3];ec->M(last_spline + D, i) = t_duration[2];ec->M(last_spline + E, i) = t_duration[1];ec->M(last_spline + F, i) = 1;ec->v(i++) = -end_cog(dim);// velocityec->M(last_spline + A, i) = 5 * t_duration[4];ec->M(last_spline + B, i) = 4 * t_duration[3];ec->M(last_spline + C, i) = 3 * t_duration[2];ec->M(last_spline + D, i) = 2* t_duration[1];ec->M(last_spline + E, i) = 1;ec->v(i++) = -kVelEnd(dim);       // accelerationsec->M(last_spline + A, i) = 20 * t_duration[3];ec->M(last_spline + B, i) = 12 * t_duration[2];ec->M(last_spline + C, i) = 6 * t_duration[1];ec->M(last_spline + D, i) = 2;

• 连接点约束
  初始点t=0; t_end = t_duration
  

int curr_spline = var_index(s, dim, A);
int next_spline = var_index(s+1, dim, A);// position of current spline at t_duration
ec->M(curr_spline + A, i) = t_duration[5];
ec->M(curr_spline + B, i) = t_duration[4];
ec->M(curr_spline + C, i) = t_duration[3];
ec->M(curr_spline + D, i) = t_duration[2];
ec->M(curr_spline + E, i) = t_duration[1];
ec->M(curr_spline + F, i) = 1;
// ...minus position of next spline at t=0...
ec->M(next_spline + F, i) = -1.0;
// ...must be zero
ec->v(i++) = 0.0;// velocity
ec->M(curr_spline + A, i) = 5 * t_duration[4];
ec->M(curr_spline + B, i) = 4 * t_duration[3];
ec->M(curr_spline + C, i) = 3 * t_duration[2];
ec->M(curr_spline + D, i) = 2 * t_duration[1];
ec->M(curr_spline + E, i) = 1;
ec->M(next_spline + E, i) = -1.0;
ec->v(i++) = 0.0;// acceleration
ec->M(curr_spline + A, i) = 20 * t_duration[3];
ec->M(curr_spline + B, i) = 12 * t_duration[2];
ec->M(curr_spline + C, i) = 6  * t_duration[1];
ec->M(curr_spline + D, i) = 2;
ec->M(next_spline + D, i) = -2.0;
ec->v(i++) = 0.0;// jerk (derivative of acceleration)
ec->M(curr_spline + A, i) = 60 * t_duration[2];
ec->M(curr_spline + B, i) = 24 * t_duration[1];
ec->M(curr_spline + C, i) = 6;
ec->M(next_spline + C, i) = -6.0;
ec->v(i++) = 0.0;  

2.3 inequality constraints

• 质心在zmp安全范围内
  将四组其中三个脚组成边缘
  
  ZMP的作用位置
  
  zmp到边缘的距离满足要求
  
  
• code

// 优化参数
int coeff = splines.size() * kCoeffCount * kDim2d;// calculate number of inequality constraints
double t_total = 0.0;
for (const SplineInfo& s : splines) t_total += s.duration_;int points = ceil(t_total / kDt);  // 离散化位置点
int constraints = points * 3; // 3 triangle side constraints per point，满足3个边缘要求
MatVecPtr ineq(new MatVec(coeff, constraints, constraints)); //不等式约束 Mq+v<=0// calculate zmp 
for (double time(0.0); time < s.duration_; time += kDt) {std::array<double,6> t = cache_exponents<6>(time);// one constraint per line of support trianglefor (SuppTriangle::TrLine l: lines) {double z_acc = 0.0; // TODO: calculate z_acc based on foothold heightconst int x = var_index(s.id_, X, A);const int y = var_index(s.id_, Y, A);ineq->M(x + A, c) = l.coeff.p * (t[5] - h/(g+z_acc) * 20.0 * t[3]);ineq->M(x + B, c) = l.coeff.p * (t[4] - h/(g+z_acc) * 12.0 * t[2]);ineq->M(x + C, c) = l.coeff.p * (t[3] - h/(g+z_acc) *  6.0 * t[1]);ineq->M(x + D, c) = l.coeff.p * (t[2] - h/(g+z_acc) *  2.0);ineq->M(x + E, c) = l.coeff.p *  t[1];ineq->M(x + F, c) = l.coeff.p *  1;ineq->M(y + A, c) = l.coeff.q * (t[5] - h/(g+z_acc) * 20.0 * t[3]);ineq->M(y + B, c) = l.coeff.q * (t[4] - h/(g+z_acc) * 12.0 * t[2]);ineq->M(y + C, c) = l.coeff.q * (t[3] - h/(g+z_acc) *  6.0 * t[1]);ineq->M(y + D, c) = l.coeff.q * (t[2] - h/(g+z_acc) *  2.0);ineq->M(y + E, c) = l.coeff.q *  t[1];ineq->M(y + F, c) = l.coeff.q *  1;ineq->v[c] = l.coeff.r - l.s_margin;++c;

2.4 参数配置

初始状态：

所有步态

步态的时序
注意交叉的时候需要留出时间给cog进行调整

DEBUG xpp.zmp.zmpoptimizer : 
Spline: id= 0:	duration=0.60	four_leg_supp=0	step=0
Spline: id= 1:	duration=0.60	four_leg_supp=0	step=1
Spline: id= 2:	duration=0.20	four_leg_supp=1	step=2
Spline: id= 3:	duration=0.60	four_leg_supp=0	step=2
Spline: id= 4:	duration=0.60	four_leg_supp=0	step=3
Spline: id= 5:	duration=0.20	four_leg_supp=1	step=4
Spline: id= 6:	duration=0.60	four_leg_supp=0	step=4
Spline: id= 7:	duration=0.60	four_leg_supp=0	step=5
Spline: id= 8:	duration=0.20	four_leg_supp=1	step=6
Spline: id= 9:	duration=0.60	four_leg_supp=0	step=6
Spline: id= 10:	duration=0.60	four_leg_supp=0	step=7
Spline: id= 11:	duration=0.20	four_leg_supp=1	step=8

2.5 数据结构

2.6 注意事项

在LH,LF,RH, RF, (LF,RH)切换的时候，机器人是四个腿全放在地面的，所以不会形成triangle
suppTriangles 的数量和splines的数量并不一致

SuppTriangles  SuppTriangle::FromFootholds(LegDataMap<Foothold> stance,const Footholds& steps,const MarginValues& margins, LegDataMap<Foothold>& last_stance)
{SuppTriangles tr;ArrayF3 non_swing;for (std::size_t s=0; s<steps.size(); s++) {LegID swingleg = steps[s].leg;// extract the 3 non-swinglegs from stanceint i = 0;for (LegID l : LegIDArray) if (stance[l].leg != swingleg)non_swing[i++] = stance[l];tr.push_back(SuppTriangle(non_swing, margins));stance[swingleg] = steps[s]; // update current stance with last step}last_stance = stance;::xpp::utils::logger_helpers::print_triangles(tr, log_);::xpp::utils::logger_helpers::PrintTriaglesMatlabInfo(tr, log_matlab_);return tr;   
}

但是在计算triangle的时候，这里没有考虑four-support的情况,four-support 不用考虑平衡

for (const SplineInfo& s : splines) {LOG4CXX_DEBUG(log_, "Calc inequality constaints of spline " << s.id_ << " of " << splines.size() << ", duration=" << std::setprecision(3) << s.duration_ << ", step=" << s.step_);// no constraints in 4ls phaseif (s.four_leg_supp_) continue;// TODO: insert support polygon constraints here instead of just // allowing the ZMP to be anywhere// cache lines of support triangle of current spline for efficiencySuppTriangle::TrLines3 lines = supp_triangles[s.step_].CalcLines();  // 仍然使用上一次swing的triangle

2.7 代码的技巧

3 v0.2

第二版中最主要的改变是将6阶spline中只优化其中四个参数，提高了运算速度。

3.1 优化器设置

所要优化的参数变量

3.1.1 cost

仍然采用二阶积分，计算cost.

    for (const SplineInfo& s : spline_infos_) {std::array<double, 10> t_span = cache_exponents<10>(s.duration_);for (int dim = X; dim <= Y; dim++) {const int a  = var_index(s.id_, dim, A);const int b  = var_index(s.id_, dim, B);const int c  = var_index(s.id_, dim, C);const int d  = var_index(s.id_, dim, D);// for explanation of values see M.Kalakrishnan et al., page 248// "Learning, Planning and Control for Quadruped Robots over challenging// Terrain", IJRR, 2010// exact spline            cf->M(a, a) = 400.0 / 7.0      * t_span[7] * weight[dim];cf->M(a, b) = 40.0             * t_span[6] * weight[dim];cf->M(a, c) = 120.0 / 5.0      * t_span[5] * weight[dim];cf->M(a, d) = 10.0             * t_span[4] * weight[dim];cf->M(b, b) = 144.0 / 5.0      * t_span[5] * weight[dim];cf->M(b, c) = 18.0             * t_span[4] * weight[dim];cf->M(b, d) = 8.0              * t_span[3] * weight[dim];cf->M(c, c) = 12.0             * t_span[3] * weight[dim];cf->M(c, d) = 6.0              * t_span[2] * weight[dim];cf->M(d, d) = 4.0              * t_span[1] * weight[dim];  // mirrow values over diagonal to fill bottom left trianglefor (int c = 0; c < cf->M.cols(); ++c) for (int r = c+1; r < cf->M.rows(); ++r)cf->M(r, c) = cf->M(c, r);          }}

3.1.2 equality constraints

连接点只考虑加速度和速度
初始对应的速度和位置通过参数给定start_p 和start_v进行导入

constraints += kDim2d*2;                         // init {x,y} * {acc, jerk} pos, vel implied
constraints += kDim2d*3;                         // end  {x,y} * {pos, vel, acc}
constraints += (spline_infos_.size()-1) * kDim2d * 2;  // junctions {acc,jerk} since pos, vel  implied

• 初始状态,不对初始位置速度进行限制了
  $$f(x)=A{t}^5+B{t}^4+C{t}^3+D{t}^2+Et+F$$
  只对加速度和jerk进行限制
  $$\left[\begin{array}{cccc}0& 0& 0& 2.0\\ 0& 0& 6& 0\end{array}\right]\left[\begin{array}{c}A\\ B\\ C\\ D\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right]$$
  代码：

// acceleration set to zero
int d = var_index(0, dim, D);
ec->M(d, i) = 2.0;
ec->v(i++) = -kAccStart(dim);
// jerk set to zero
int c = var_index(0, dim, C);
ec->M(c, i) = 6.0;
ec->v(i++) = -kJerkStart(dim); 

• 终止状态
  var_index(int spline, int dim, int coeff) 对应结果
  $$n=spline\ast kDim2d\ast coeff+dim\ast coeff+coeff$$
  DescribeEByPrev的计算效果
  $$\begin{array}{rl}Ek& =\left[\begin{array}{cccccccccccccc}5t_0^4& 4t_0^3& 3t_0^2& 2t_0& ...& 5t_i^4& 4t_i^3& 3t_i^2& 2t_i& ...& 5t_{n-1}^4& 4t_{n-1}^3& 3t_{n-1}^2& 2t_{n-1}\end{array}\right]\\ nondependent& =startcogv\end{array}$$
  考虑到隐含关系
  $$\begin{array}{rl}5A_i{t}_i^4+4B_i{t}_i^3+3C_i{t}_i^2+2D_i{t}_i+E_i& =E_{i+1}\\ Coeff\ast Ek& =\sum _i^{n-1}{E}_{i+1}-E_i=E_n-E_0\\ nondependent& =E_0\\ Coeff\ast Ek+nondependent& =E_n\end{array}$$
  DescribeFByPrev的计算效果
  $$\begin{array}{rl}Fk& =\left[\begin{array}{ccccc}5t_0^4+{\sum }_0^{k}5t_0^4{t}_i& 4t_0^3+{\sum }_0^{k}4t_0^3{t}_i& ...& 3t_0^2+{\sum }_0^{k}3t_0^2{t}_i& 2t_0+{\sum }_0^{k}2t_0{t}_i\\ 5t_1^4+{\sum }_1^{k}5t_1^4{t}_i& 4t_1^3+{\sum }_1^{k}4t_1^3{t}_i& ...& 3t_1^2+{\sum }_1^{k}3t_1^2{t}_i& 2t_1+{\sum }_1^{k}2t_1{t}_i\end{array}\right]\\ nondependent& =startcogv\ast \sum t_i+startcogp\end{array}$$
  考虑到位置隐含关系
  $$\begin{array}{rl}{A}_i{t}_i^5+B_i{t}_i^4+C_i{t}_i^3+D_i{t}_i^2+E_i{t}_i+F_i& =F_{i+1}\\ \sum _0^{i-1}(5A_j{t}_j^4{t}_i+4B_j{t}_j^3{t}_i+3D_j{t}_j^2{t}_i+2D_j{t}_j{t}_i)& =\sum _0^{i-1}(E_{j+1}-E_j)t_i=(E_i-E_0)t_i\\ Coeff\ast Fk& =\sum _i^{n-1}(F_{i+1}-F_i-E_i{t}_i+E_i{t}_i-E_0{t}_i)=F_n-F_0-E_0{t}_{sum}\\ nondependent& =E_0{t}_{sum}+F_0\\ Coeff\ast Fk+nondependent& =F_n\end{array}$$

借助DescribeEByPrev和DescribeFByPrev，同样可以计算pos和velocity对应的数值

• 对于终止位置
  $$\begin{array}{rl}(Coeff\ast Ek+nondependente)\ast t_n+& \\ (Coeff\ast Fk+nondependentf)+(en{d}_P-E_n{t}_n-F_n)& =\\ & =E_n\ast t_n+F_n+(en{d}_P-E_n{t}_n-F_n)\\ & =en{d}_P\end{array}$$
• 对于终止速度
  $$\begin{array}{rl}(Coeff\ast Ek+nondependente)+& \\ (en{d}_V-E_n)& =\\ & =E_n+(en{d}_v-E_n)\\ & =en{d}_V\end{array}$$
• 对于加速度不需要借助DescribeFByPrev和DescribeEByPrev

// 2. Final conditions
int K = spline_infos_.back().id_; // id of last spline
int last_spline = var_index(K, dim, A);
std::array<double,6> t_duration = cache_exponents<6>(spline_infos_.back().duration_);// calculate e and f coefficients from previous values
Eigen::VectorXd Ek(coeff); Ek.setZero();
Eigen::VectorXd Fk(coeff); Fk.setZero();
double non_dependent_e, non_dependent_f;
DescribeEByPrev(K, dim, start_cog_v(dim), Ek, non_dependent_e);
DescribeFByPrev(K, dim, start_cog_v(dim), start_cog_p(dim), Fk, non_dependent_f);// position 
ec->M(last_spline + A, i) = t_duration[5];
ec->M(last_spline + B, i) = t_duration[4];
ec->M(last_spline + C, i) = t_duration[3];
ec->M(last_spline + D, i) = t_duration[2];
ec->M.col(i) += Ek*t_duration[1];
ec->M.col(i) += Fk;ec->v(i)     += non_dependent_e*t_duration[1] + non_dependent_f;
ec->v(i++) = -end_cog(dim);// velocity
ec->M(last_spline + A, i) = 5 * t_duration[4];
ec->M(last_spline + B, i) = 4 * t_duration[3];
ec->M(last_spline + C, i) = 3 * t_duration[2];
ec->M(last_spline + D, i) = 2* t_duration[1];
ec->M.col(i) += Ek;ec->v(i)     += non_dependent_e;
ec->v(i++)   += -kVelEnd(dim);       // accelerations
ec->M(last_spline + A, i) = 20 * t_duration[3];
ec->M(last_spline + B, i) = 12 * t_duration[2];
ec->M(last_spline + C, i) = 6 * t_duration[1];
ec->M(last_spline + D, i) = 2;ec->v(i++) = -kAccEnd(dim); 

• 连接点状态
  连接点状态因为没有位置和速度约束和v0.1的一样

for (SplineInfoVec::size_type s = 0; s < spline_infos_.size() - 1; ++s)
{std::array<double,6> t_duration = cache_exponents<6>(spline_infos_.at(s).duration_);for (int dim = X; dim <= Y; dim++) {int curr_spline = var_index(s, dim, A);int next_spline = var_index(s + 1, dim, A);// accelerationec->M(curr_spline + A, i) = 20 * t_duration[3];ec->M(curr_spline + B, i) = 12 * t_duration[2];ec->M(curr_spline + C, i) = 6  * t_duration[1];ec->M(curr_spline + D, i) = 2;ec->M(next_spline + D, i) = -2.0;ec->v(i++) = 0.0;// jerk (derivative of acceleration)ec->M(curr_spline + A, i) = 60 * t_duration[2];ec->M(curr_spline + B, i) = 24 * t_duration[1];ec->M(curr_spline + C, i) = 6;ec->M(next_spline + C, i) = -6.0;ec->v(i++) = 0.0;}
}

3.1.3 inequality constraints

ZMP的作用位置

zmp到边缘的距离满足要求

$$\begin{array}{rl}{Z}_x\ast l.p+Z_y\ast l_q+r-m& >0\\ z& =po{s}_t-E_i\ast t-F_i+(Coeff\ast EK+E_0)\ast t+\\ & (Coeff\ast FK+nondeF)-\frac{h}{g_{zacc}}\ast ac{c}_t\end{array}$$

for (double i=0; i < std::floor(s.duration_/kDt); ++i) {double time = i*kDt;std::array<double,6> t = cache_exponents<6>(time);// one constraint per line of support trianglefor (SuppTriangle::TrLine l: lines) {// the zero moment point must always lay on one side of triangle side:// p*x_zmp + q*y_zmp + r > stability_margin// with: x_zmp = x_pos - height/(g+z_acc) * x_acc//       x_pos = at^5 +   bt^4 +  ct^3 + dt*2 + et + f//       x_acc = 20at^3 + 12bt^2 + 6ct   + 2ddouble z_acc = 0.0; // TODO: calculate z_acc based on foothold heightfor (int dim=X; dim<=Y; dim++) {double lc = (dim==X) ? l.coeff.p : l.coeff.q;// calculate e and f coefficients from previous valuesEigen::VectorXd Ek(coeff); Ek.setZero();Eigen::VectorXd Fk(coeff); Fk.setZero();double non_dependent_e, non_dependent_f;DescribeEByPrev(k, dim, start_cog_v(dim), Ek, non_dependent_e);DescribeFByPrev(k, dim, start_cog_v(dim), start_cog_p(dim), Fk, non_dependent_f);ineq->M(var_index(k,dim,A), c) = lc * (t[5] - h/(g+z_acc) * 20.0 * t[3]);ineq->M(var_index(k,dim,B), c) = lc * (t[4] - h/(g+z_acc) * 12.0 * t[2]);ineq->M(var_index(k,dim,C), c) = lc * (t[3] - h/(g+z_acc) *  6.0 * t[1]);ineq->M(var_index(k,dim,D), c) = lc * (t[2] - h/(g+z_acc) *  2.0);ineq->M.col(c) += lc * Ek*t[1];ineq->M.col(c) += lc * Fk;ineq->v[c]     += lc *(non_dependent_e*t[0] + non_dependent_f);}ineq->v[c] += l.coeff.r - l.s_margin;++c;}
}
}

3.2 轨迹优化的流程

ConstructSplineSequence->optCeff->generateTraj
逻辑和之前一样，但是处理更加漂亮

double discretization_time = 0.1; 
double swing_time = 0.6;         
double stance_time = 0.2;         
double t_stance_initial = 1.0; //sint step = 0;
unsigned int id = 0; // unique identifiers for each splineconst int kSplinesPer4ls = 1;
const int kSplinesPerStep = 1;// 人为的建立抬脚的step sequences:延长了初始和结束的时间1s
for (size_t i = 0; i < step_sequence.size(); ++i) {// 1. insert 4ls-phase when switching between disjoint support triangles// Attention: these 4ls-phases much coincide with the ones in the zmp optimizer        if (i == 0) { // never insert 4ls-phase before first stepspline_infos_.push_back(SplineInfo(id++, t_stance_initial, true, step));} else {LegID swing_leg = step_sequence[i]; // 按照顺序进行抬腿LegID swing_leg_prev = step_sequence[i-1];if (SuppTriangle::Insert4LSPhase(swing_leg_prev, swing_leg)) {for (int s = 0; s < kSplinesPer4ls; s++) spline_infos_.push_back(SplineInfo(id++, t_stance/kSplinesPer4ls, true, step));}}// insert swing phase splinesfor (int s = 0; s < kSplinesPerStep; s++) spline_infos_.push_back(SplineInfo(id++, t_swing/kSplinesPerStep, false, step));// always have last 4ls spline for robot to move into center of feetif (i==step_sequence.size()-1) spline_infos_.push_back(SplineInfo(id++, t_stance_final, true, step));step++;
}

v0.3

v0.3 中添加了nlp形式，同时对qp形式的优化问题，进行了解耦。