/*FGROUP SSM Schedule SSM recording whenmode: <10 -reserved for after/before mode starts from CTP emulator window 2 -start After when 1st SW START or STARTSIGNAL selected 3 -start Before --- */ void SSMschedule(w32 whenmode) { if(whenmode<10) { /* see SLMswstart, SLMsetstart */ printf("SSM recording scheduled:%s\n",getAB(whenmode)); SSMSCHEDULER=whenmode; }; }
// traj z part should be offset to ground, not abs z coord!! void LipmVarHeightPlanner::setZMPTraj(const Traj<3,3> &traj, const std::vector <double> &z0) { assert(traj.size() > 0 && traj.size() == z0.size()); _zmp_d = traj; _z0 = z0; // get last step Vxx and initial Ks const TrajPoint<3,3> &end = traj[traj.size()-1]; double x[6] = {0}; double u[3] = {0}; dvec_copy(x, end.x, 6); dvec_copy(u, end.u, 3); Eigen::Matrix<double,6,6> A; Eigen::Matrix<double,6,3> B; getAB(x, u, _z0[_z0.size()-1], A, B); _lqr.setQ(_Q); _lqr.setR(_R); _lqr.infTimeLQR(A, B); _Vxx = _lqr.getV(); if (traj.size() == _du.size()) { for (size_t i = 0; i < traj.size(); i++) { _du[i].setZero(); _K[i] = _lqr.getK(); } } else { _du.resize(traj.size(), Eigen::Matrix<double,3,1>::Zero()); _K.resize(traj.size(), _lqr.getK()); } // initial trajectory _traj0 = Traj<3,3>(); for (size_t i = 0; i < _zmp_d.size(); i++) { const TrajPoint<3,3> &end = traj[i]; dvec_copy(x, end.x, 6); dvec_copy(u, end.u, 3); _traj0.append(end.time, end.type, x, x+3, NULL, u); } Traj<3,3> tmpTraj; forwardPass(traj[0].x, tmpTraj); _traj0 = tmpTraj; assert(_zmp_d.size() == _du.size() && _zmp_d.size() == _K.size() && _zmp_d.size() == _traj0.size()); }
bool SR865::getRTheta(double &R, double &Theta) const { return getAB(2, R, 3, Theta); }
void LipmVarHeightPlanner::backwardPass(const Traj<3,3> &com) { Eigen::Matrix<double,6,6> Qxx; Eigen::Matrix<double,3,3> Quu; Eigen::Matrix<double,3,3> invQuu; Eigen::Matrix<double,3,6> Qux; Eigen::Matrix<double,6,3> Qxu; Eigen::Matrix<double,6,1> Qx; Eigen::Matrix<double,3,1> Qu; Eigen::Matrix<double,6,6> A; Eigen::Matrix<double,6,3> B; Eigen::Matrix<double,6,6> Vxx = _Vxx; Eigen::Matrix<double,6,1> Vx = Eigen::Matrix<double,6,1>::Zero(); Eigen::Matrix<double,6,1> cur_x; Eigen::Matrix<double,3,1> cur_u; Eigen::Matrix<double,6,1> x_h; Eigen::Matrix<double,3,1> u_h; //NEW_GSL_MATRIX(tmpXX0, tmpXX0_d, tmpXX0_v, 6, 6); //NEW_GSL_MATRIX(tmpUX0, tmpUX0_d, tmpUX0_v, 3, 6); const double *x0, *u0; for (int t = (int)_K.size()-1; t >= 0; t--) { //printf("======================================\n"); // get x u x0 = _traj0[t].x; u0 = _traj0[t].u; //printf("%g %g %g %g %g %g\n", x0[0], x0[1], x0[2], x0[3], x0[4], x0[5]); //printf("%g %g %g\n", u0[0], u0[1], u0[2]); for (int i = 0; i < 6; i++) { cur_x(i) = x0[i]; x_h(i) = x0[i] - _zmp_d[t].x[i]; } for (int i = 0; i < 3; i++) { cur_u(i) = u0[i]; u_h(i) = u0[i] - _zmp_d[t].u[i]; } // get A B getAB(x0, u0, _z0[t], A, B); //printGSLMatrix("A", A); //printGSLMatrix("B", B); // Qx = Q*x_hat + A'*Vx Qx = _Q*x_h + A.transpose()*Vx; // Qu = R*u_hat + B'*Vx Qu = _R*u_h + B.transpose()*Vx; // Qxx = Q + A'*Vxx*A Qxx = _Q + A.transpose()*Vxx*A; // Qxu = A'*Vxx*B Qxu = A.transpose()*Vxx*B; // Quu = R + B'*Vxx*B Quu = _R + B.transpose()*Vxx*B; // Qux = Qxu' Qux = B.transpose()*Vxx*A; // K = -inv(Quu)*Qux _K[t] = -(Quu.llt().solve(Qux)); // du = -inv(Quu)*Qu _du[t] = -(Quu.llt().solve(Qu)); // Vx = Qx + K'*Qu Vx = Qx + _K[t].transpose()*Qu; // Vxx = Qxx + Qxu*K Vxx = Qxx + Qxu*_K[t]; //getchar(); //assert(!hasInfNan(_K[t])); //assert(!hasInfNan(_du[t])); } }
bool SR865::getXY(double &X, double &Y) const { return getAB(0, X, 1, Y); }