towr code阅读
1. Introduction
towr是非常优美的足式机器人规划代码，通过阅读towr重要的几个迭代版本的代码深入了解。
2 v0.1
第一代的版本，foot的位置是提前给定的，只对COG的trajectory进行优化。
2.1 cost
- 公式
仅仅只考虑加速度，
∫ 0 T x ¨ 2 ( t ) d t + v = q T G q + v \int_0^T \ddot{x}^2(t)dt+v=q^TGq+v ∫0Tx¨2(t)dt+v=qTGq+v
其中，
q = [ a b c d ] T q = \begin{bmatrix} a & b &c &d \end{bmatrix}^T q=[abcd]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 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 ∗ c o e f f + v = 0 M*coeff+v=0 M∗coeff+v=0
- 位置、速度和加速度的约束
M [ a b c d e f ] T + v = 0 M \begin{bmatrix} a & b & c & d & e & f \end{bmatrix}^T + v = 0 M[abcdef]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);
// 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(last_spline + E, i) = 1;
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;
- 连接点约束
初始点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 triangle
for (SuppTriangle::TrLine l: lines) {
double z_acc = 0.0; // TODO: calculate z_acc based on foothold height
const 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 stance
int 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 phase
if (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 efficiency
SuppTriangle::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 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);
}
}
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 + E t + F f(x)=At^5+Bt^4+Ct^3+Dt^2+Et+F f(x)=At5+Bt4+Ct3+Dt2+Et+F
只对加速度和jerk进行限制
[ 0 0 0 2.0 0 0 6 0 ] [ A B C D ] = [ 0 0 ] \begin{bmatrix} 0 & 0 & 0 & 2.0 \\ 0 & 0 & 6 & 0 \end{bmatrix} \begin{bmatrix} A \\ B \\ C \\D \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix} [0000062.00] ABCD =[00]
代码：
// 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 = s p l i n e ∗ k D i m 2 d ∗ c o e f f + d i m ∗ c o e f f + c o e f f n = spline*kDim2d*coeff+dim*coeff+coeff n=spline∗kDim2d∗coeff+dim∗coeff+coeff
DescribeEByPrev的计算效果
E k = [ 5 t 0 4 4 t 0 3 3 t 0 2 2 t 0 . . . 5 t i 4 4 t i 3 3 t i 2 2 t i . . . 5 t n − 1 4 4 t n − 1 3 3 t n − 1 2 2 t n − 1 ] n o n d e p e n d e n t = s t a r t c o g v \begin{aligned} Ek & = \begin{bmatrix} 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{bmatrix} \\ nondependent & = startcogv \end{aligned} Eknondependent=[5t044t033t022t0...5ti44ti33ti22ti...5tn−144tn−133tn−122tn−1]=startcogv
考虑到隐含关系
5 A i t i 4 + 4 B i t i 3 + 3 C i t i 2 + 2 D i t i + E i = E i + 1 C o e f f ∗ E k = ∑ i n − 1 E i + 1 − E i = E n − E 0 n o n d e p e n d e n t = E 0 C o e f f ∗ E k + n o n d e p e n d e n t = E n \begin{aligned} 5A_it_i^4+4B_it_i^3+3C_it_i^2+2D_it_i+E_i &=E_{i+1} \\ Coeff * Ek&=\sum_i^{n-1}E_{i+1}-E_{i}=E_n-E_0 \\ nondependent & = E_0 \\ Coeff*Ek+nondependent &=E_n \end{aligned} 5Aiti4+4Biti3+3Citi2+2Diti+EiCoeff∗EknondependentCoeff∗Ek+nondependent=Ei+1=i∑n−1Ei+1−Ei=En−E0=E0=En
DescribeFByPrev的计算效果
F k = [ 5 t 0 4 + ∑ 0 k 5 t 0 4 t i 4 t 0 3 + ∑ 0 k 4 t 0 3 t i . . . 3 t 0 2 + ∑ 0 k 3 t 0 2 t i 2 t 0 + ∑ 0 k 2 t 0 t i 5 t 1 4 + ∑ 1 k 5 t 1 4 t i 4 t 1 3 + ∑ 1 k 4 t 1 3 t i . . . 3 t 1 2 + ∑ 1 k 3 t 1 2 t i 2 t 1 + ∑ 1 k 2 t 1 t i ] n o n d e p e n d e n t = s t a r t c o g v ∗ ∑ t i + s t a r t c o g p \begin{aligned} Fk & = \begin{bmatrix} 5t_0^4+\sum_0^k5t_0^4t_i & 4t_0^3+ \sum_0^k4t_0^3t_i & ... & 3t_0^2+\sum_0^k3t_0^2t_i & 2t_0+ \sum_0^k2t_0t_i \\ 5t_1^4+\sum_1^k5t_1^4t_i & 4t_1^3+ \sum_1^k4t_1^3t_i & ... & 3t_1^2+\sum_1^k3t_1^2t_i & 2t_1+ \sum_1^k2t_1t_i \end{bmatrix} \\ nondependent & = startcogv * \sum t_i+startcogp \\ \end{aligned} Fknondependent=[5t04+∑0k5t04ti5t14+∑1k5t14ti4t03+∑0k4t03ti4t13+∑1k4t13ti......3t02+∑0k3t02ti3t12+∑1k3t12ti2t0+∑0k2t0ti2t1+∑1k2t1ti]=startcogv∗∑ti+startcogp
考虑到位置隐含关系
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 ∑ 0 i − 1 ( 5 A j t j 4 t i + 4 B j t j 3 t i + 3 D j t j 2 t i + 2 D j t j t i ) = ∑ 0 i − 1 ( E j + 1 − E j ) t i = ( E i − E 0 ) t i C o e f f ∗ F k = ∑ 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 s u m n o n d e p e n d e n t = E 0 t s u m + F 0 C o e f f ∗ F k + n o n d e p e n d e n t = F n \begin{aligned} A_it_i^5+B_it_i^4+C_it_i^3+D_it_i^2+E_it_i+F_i &=F_{i+1} \\ \sum_0^{i-1}(5A_jt_j^4t_i+4B_jt_j^3t_i+3D_jt_j^2t_i+2D_jt_jt_i) & = \sum_0^{i-1}(E_{j+1}-E_j)t_i=(E_{i}-E_0)t_i \\ Coeff * Fk&=\sum_i^{n-1}(F_{i+1}-F_{i}-E_it_i+E_it_i-E_0t_i)=F_n-F_0-E_0t_{sum} \\ nondependent & = E_0t_{sum}+F_0 \\ Coeff * Fk+nondependent &=F_n \end{aligned} Aiti5+Biti4+Citi3+Diti2+Eiti+Fi0∑i−1(5Ajtj4ti+4Bjtj3ti+3Djtj2ti+2Djtjti)Coeff∗FknondependentCoeff∗Fk+nondependent=Fi+1=0∑i−1(Ej+1−Ej)ti=(Ei−E0)ti=i∑n−1(Fi+1−Fi−Eiti+Eiti−E0ti)=Fn−F0−E0tsum=E0tsum+F0=Fn
借助DescribeEByPrev和DescribeFByPrev，同样可以计算pos和velocity对应的数值
- 对于终止位置
( C o e f f ∗ E k + n o n d e p e n d e n t e ) ∗ t n + ( C o e f f ∗ F k + n o n d e p e n d e n t f ) + ( e n d P − E n t n − F n ) = = E n ∗ t n + F n + ( e n d P − E n t n − F n ) = e n d P \begin{aligned} (Coeff*Ek+nondependente)*t_n+ & \\(Coeff*Fk+nondependentf) +(end_P-E_nt_n-F_n)& = \\ & = E_n*t_n+ F_n+(end_P-E_nt_n-F_n) \\ & = end_P \end{aligned} (Coeff∗Ek+nondependente)∗tn+(Coeff∗Fk+nondependentf)+(endP−Entn−Fn)==En∗tn+Fn+(endP−Entn−Fn)=endP - 对于终止速度
( C o e f f ∗ E k + n o n d e p e n d e n t e ) + ( e n d V − E n ) = = E n + ( e n d v − E n ) = e n d V \begin{aligned} (Coeff*Ek+nondependente)+ & \\(end_V-E_n)& = \\ & = E_n+(end_v-E_n) \\ & = end_V \end{aligned} (Coeff∗Ek+nondependente)+(endV−En)==En+(endv−En)=endV - 对于加速度不需要借助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);
// 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;
}
}
3.1.3 inequality constraints
ZMP的作用位置
zmp到边缘的距离满足要求
Z x ∗ l . p + Z y ∗ l q + r − m > 0 z = p o s t − E i ∗ t − F i + ( C o e f f ∗ E K + E 0 ) ∗ t + ( C o e f f ∗ F K + n o n d e F ) − h g z a c c ∗ a c c t \begin{aligned} Z_x*l.p+Z_y*l_q+r-m &>0 \\ z &= pos_t-E_i*t-F_i+(Coeff*EK+E_0)*t+ \\ &(Coeff*FK+nondeF)-\frac{h}{g_{zacc}}*acc_t \end{aligned} Zx∗l.p+Zy∗lq+r−mz>0=post−Ei∗t−Fi+(Coeff∗EK+E0)∗t+(Coeff∗FK+nondeF)−gzacch∗acct
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 triangle
for (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 + 2d
double z_acc = 0.0; // TODO: calculate z_acc based on foothold height
for (int dim=X; dim<=Y; dim++) {
double lc = (dim==X) ? l.coeff.p : l.coeff.q;
// 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);
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; //s
int step = 0;
unsigned int id = 0; // unique identifiers for each spline
const 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 step
spline_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 splines
for (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 feet
if (i==step_sequence.size()-1)
spline_infos_.push_back(SplineInfo(id++, t_stance_final, true, step));
step++;
}
v0.3
v0.3 中添加了nlp形式，同时对qp形式的优化问题，进行了解耦。