电影网站推荐哪个网站好,怎么做卖车网站,长春生活信息网,WordPress打开 速度1. Introduction
towr是非常优美的足式机器人规划代码#xff0c;通过阅读towr重要的几个迭代版本的代码深入了解。
2 v0.1
第一代的版本#xff0c;foot的位置是提前给定的#xff0c;只对COG的trajectory进行优化。
2.1 cost
公式 仅仅只考虑加速度#xff0c; ∫ …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)dtvq^TGqv ∫0Tx¨2(t)dtvqTGqv
其中 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::arraydouble, 10 t_span cache_exponents10(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*coeffv0 M∗coeffv0 位置、速度和加速度的约束 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]Tv0初始和终止状态约束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;连接点约束 初始点t0; t_end t_duration
int curr_spline var_index(s, dim, A);
int next_spline var_index(s1, 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 t0...
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)); //不等式约束 Mqv0// calculate zmp
for (double time(0.0); time s.duration_; time kDt) {std::arraydouble,6 t cache_exponents6(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/(gz_acc) * 20.0 * t[3]);ineq-M(x B, c) l.coeff.p * (t[4] - h/(gz_acc) * 12.0 * t[2]);ineq-M(x C, c) l.coeff.p * (t[3] - h/(gz_acc) * 6.0 * t[1]);ineq-M(x D, c) l.coeff.p * (t[2] - h/(gz_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/(gz_acc) * 20.0 * t[3]);ineq-M(y B, c) l.coeff.q * (t[4] - h/(gz_acc) * 12.0 * t[2]);ineq-M(y C, c) l.coeff.q * (t[3] - h/(gz_acc) * 6.0 * t[1]);ineq-M(y D, c) l.coeff.q * (t[2] - h/(gz_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: duration0.60 four_leg_supp0 step0
Spline: id 1: duration0.60 four_leg_supp0 step1
Spline: id 2: duration0.20 four_leg_supp1 step2
Spline: id 3: duration0.60 four_leg_supp0 step2
Spline: id 4: duration0.60 four_leg_supp0 step3
Spline: id 5: duration0.20 four_leg_supp1 step4
Spline: id 6: duration0.60 four_leg_supp0 step4
Spline: id 7: duration0.60 four_leg_supp0 step5
Spline: id 8: duration0.20 four_leg_supp1 step6
Spline: id 9: duration0.60 four_leg_supp0 step6
Spline: id 10: duration0.60 four_leg_supp0 step7
Spline: id 11: duration0.20 four_leg_supp1 step82.5 数据结构 2.6 注意事项
在LH,LF,RH, RF, (LF,RH)切换的时候机器人是四个腿全放在地面的所以不会形成triangle suppTriangles 的数量和splines的数量并不一致
SuppTriangles SuppTriangle::FromFootholds(LegDataMapFoothold stance,const Footholds steps,const MarginValues margins, LegDataMapFoothold last_stance)
{SuppTriangles tr;ArrayF3 non_swing;for (std::size_t s0; ssteps.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的triangle2.7 代码的技巧 3 v0.2
第二版中最主要的改变是将6阶spline中只优化其中四个参数提高了运算速度。
3.1 优化器设置
所要优化的参数变量
3.1.1 cost
仍然采用二阶积分计算cost. for (const SplineInfo s : spline_infos_) {std::arraydouble, 10 t_span cache_exponents10(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 c1; 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^5Bt^4Ct^3Dt^2EtF f(x)At5Bt4Ct3Dt2EtF 只对加速度和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*coeffdim*coeffcoeff nspline∗kDim2d∗coeffdim∗coeffcoeff 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^44B_it_i^33C_it_i^22D_it_iE_i E_{i1} \\ Coeff * Ek\sum_i^{n-1}E_{i1}-E_{i}E_n-E_0 \\ nondependent E_0 \\ Coeff*Eknondependent E_n \end{aligned} 5Aiti44Biti33Citi22DitiEiCoeff∗EknondependentCoeff∗EknondependentEi1i∑n−1Ei1−EiEn−E0E0En 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_istartcogp \\ \end{aligned} Fknondependent[5t04∑0k5t04ti5t14∑1k5t14ti4t03∑0k4t03ti4t13∑1k4t13ti......3t02∑0k3t02ti3t12∑1k3t12ti2t0∑0k2t0ti2t1∑1k2t1ti]startcogv∗∑tistartcogp 考虑到位置隐含关系 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^5B_it_i^4C_it_i^3D_it_i^2E_it_iF_i F_{i1} \\ \sum_0^{i-1}(5A_jt_j^4t_i4B_jt_j^3t_i3D_jt_j^2t_i2D_jt_jt_i) \sum_0^{i-1}(E_{j1}-E_j)t_i(E_{i}-E_0)t_i \\ Coeff * Fk\sum_i^{n-1}(F_{i1}-F_{i}-E_it_iE_it_i-E_0t_i)F_n-F_0-E_0t_{sum} \\ nondependent E_0t_{sum}F_0 \\ Coeff * Fknondependent F_n \end{aligned} Aiti5Biti4Citi3Diti2EitiFi0∑i−1(5Ajtj4ti4Bjtj3ti3Djtj2ti2Djtjti)Coeff∗FknondependentCoeff∗FknondependentFi10∑i−1(Ej1−Ej)ti(Ei−E0)tii∑n−1(Fi1−Fi−EitiEiti−E0ti)Fn−F0−E0tsumE0tsumF0Fn
借助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*Eknondependente)*t_n \\(Coeff*Fknondependentf) (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∗Eknondependente)∗tn(Coeff∗Fknondependentf)(endP−Entn−Fn)En∗tnFn(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*Eknondependente) \\(end_V-E_n) \\ E_n(end_v-E_n) \\ end_V \end{aligned} (Coeff∗Eknondependente)(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::arraydouble,6 t_duration cache_exponents6(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::arraydouble,6 t_duration cache_exponents6(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到边缘的距离满足要求 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.pZ_y*l_qr-m 0 \\ z pos_t-E_i*t-F_i(Coeff*EKE_0)*t \\ (Coeff*FKnondeF)-\frac{h}{g_{zacc}}*acc_t \end{aligned} Zx∗l.pZy∗lqr−mz0post−Ei∗t−Fi(Coeff∗EKE0)∗t(Coeff∗FKnondeF)−gzacch∗acct
for (double i0; i std::floor(s.duration_/kDt); i) {double time i*kDt;std::arraydouble,6 t cache_exponents6(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/(gz_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 dimX; dimY; dim) {double lc (dimX) ? 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/(gz_acc) * 20.0 * t[3]);ineq-M(var_index(k,dim,B), c) lc * (t[4] - h/(gz_acc) * 12.0 * t[2]);ineq-M(var_index(k,dim,C), c) lc * (t[3] - h/(gz_acc) * 6.0 * t[1]);ineq-M(var_index(k,dim,D), c) lc * (t[2] - h/(gz_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 (istep_sequence.size()-1) spline_infos_.push_back(SplineInfo(id, t_stance_final, true, step));step;
}v0.3
v0.3 中添加了nlp形式同时对qp形式的优化问题进行了解耦。
