/*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);
}
