vec3f cmuk::computeWorldPointRel( FrameIndex frame,
const KState& s0,
const vec3f& p0,
const KState& s1 ) const {
if (frame < FRAME_TRUNK || frame > FRAME_HR_FOOT) {
return p0;
}
vec3f b = s0.xform().transformInv(p0);
if (frame == FRAME_TRUNK) {
return s1.xform().transformFwd(b);
} else {
// get the leg offset and deal
LegIndex leg = LegIndex((frame-1)/4);
if (s1.leg_rot[leg] == s0.leg_rot[leg]) {
return s1.xform().transformFwd(b);
}
JointOffset joint = JointOffset((frame-1)%4);
Transform3f x0[4];
Transform3f x1[4];
computeLegTransforms(leg, s0.leg_rot[leg], x0);
computeLegTransforms(leg, s1.leg_rot[leg], x1);
vec3f l = x0[joint].transformInv(b);
b = x1[joint].transformFwd(l);
return s1.xform().transformFwd(b);
}
}
bool HuboPlus::stanceIK( KState& state,
const Transform3 manipXforms[NUM_MANIPULATORS],
const IKMode mode[NUM_MANIPULATORS],
const bool shouldInitIK[NUM_MANIPULATORS],
Transform3Array& work,
bool* ikvalid ) const {
bool allOK = true;
kbody.transforms(state.jvalues, work);
for (int i=0; i<NUM_MANIPULATORS; ++i) {
Transform3 desired = manipXforms[i];
bool doIK = false;
switch (mode[i]) {
case IK_MODE_BODY:
doIK = true;
break;
case IK_MODE_WORLD:
case IK_MODE_SUPPORT:
doIK = true;
desired = state.xform().inverse() * desired;
break;
default:
doIK = false;
break;
}
bool valid = true;
if (doIK) {
Transform3 cur = kbody.manipulatorFK(work,i);
if ( (cur.translation() - desired.translation()).norm() > kbody.DEFAULT_PTOL ||
quat::dist(cur.rotation(), desired.rotation()) > kbody.DEFAULT_QTOL) {
if (shouldInitIK[i]) { initIK(i, desired, state.jvalues); }
valid = manipIK(i, desired, state.jvalues, work);
}
}
if (!valid) { allOK = false; }
if (ikvalid) { ikvalid[i] = valid; }
}
return allOK;
}
CMUK_ERROR_CODE cmuk::computeWorldPointFootDK( LegIndex leg,
JointOffset link,
const KState& state,
const vec3f& pworld,
mat3f* J_trans,
mat3f* J_rot,
float* det,
float det_tol,
float lambda ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!J_trans || !J_rot) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
Transform3f xform = state.xform();
vec3f pbody = xform.transformInv(pworld);
vec3f fbody;
mat3f J;
CMUK_ERROR_CODE err =
computePointFootDK( leg, link, state.leg_rot[leg], pbody, &J,
&fbody, det, det_tol, lambda );
if (err != CMUK_OKAY && err != CMUK_SINGULAR_MATRIX) {
return err;
}
const mat3f& R = xform.rotFwd();
const mat3f& Rinv = xform.rotInv();
*J_trans = mat3f::identity() - R * J * Rinv;
//*J_rot = R * (-mat3f::cross(pbody) + J * mat3f::cross(fbody));
*J_rot = R * ( J * mat3f::cross(fbody) - mat3f::cross(pbody) ) * Rinv;
return err;
}
bool HuboPlus::comIK( KState& state,
const vec3& dcom,
const Transform3 manipXforms[NUM_MANIPULATORS],
const IKMode mode[NUM_MANIPULATORS],
const bool globalIK[NUM_MANIPULATORS],
Transform3Array& work,
real ascl,
real fscl,
bool* ikvalid ) const {
bool ok = false;
MatX gT, gpT, gxT, fxT, fpT, lambda, gxxpT(6, 3), deltap;
MatX gfT, deltaf;
const real alpha = 0.5;
IndexArray pdofs;
for (size_t i=DOF_POS_X; i<=DOF_ROT_Z; ++i) {
pdofs.push_back(i);
}
IndexArray fdofs;
for (int i=0; i<4; ++i) {
if (fscl && mode[i] == IK_MODE_FREE) {
const IndexArray& jidx = kbody.manipulators[i].jointIndices;
for (size_t j=0; j<jidx.size(); ++j) {
fdofs.push_back(jidx[j]);
}
}
}
for (size_t iter=0; iter<DEFAULT_COM_ITER; ++iter) {
// try doing IK
ok = stanceIK( state, manipXforms, mode, globalIK, work, ikvalid );
// compute the COM pos
kbody.transforms(state.jvalues, work);
vec3 com = state.xform() * kbody.com(work);
// get the error
vec3 comerr = dcom - com;
if (comerr.norm() < DEFAULT_COM_PTOL) {
debug << "breaking after " << iter << " iterations\n";
break;
} else {
ok = false;
}
if (ascl == 0) {
state.body_pos += alpha * comerr;
} else {
// get jacobians ftw
kbody.comJacobian(work, pdofs, gpT);
gpT.block(3, 0, 3, 3) *= ascl;
debug << "gpT=" << gpT << "\n\n";
if (!fdofs.empty()) {
kbody.comJacobian(work, fdofs, gfT);
debug << "gfT=" << gfT << "\n\n";
}
gxxpT.setZero();
for (int i=0; i<4; ++i) {
if (mode[i] == IK_MODE_WORLD || mode[i] == IK_MODE_SUPPORT) {
const std::string& name = kbody.manipulators[i].name;
kbody.comJacobian(work, kbody.manipulators[i].jointIndices, gxT);
kbody.manipulatorJacobian(work, i, pdofs, fpT);
kbody.manipulatorJacobian(work, i, fxT);
lambda = fxT.colPivHouseholderQr().solve(gxT);
fpT.block(3, 0, 3, 6) *= ascl;
debug << "gxT[" << name << "]=\n" << gxT << "\n\n";
debug << "fpT[" << name << "]=\n" << fpT << "\n\n";
debug << "fxT[" << name << "]=\n" << fxT << "\n\n";
debug << "lambda[" << name << "]=\n" << lambda << "\n\n";
gxxpT += fpT * lambda;
}
}
gT = gpT - gxxpT;
Eigen::Vector3d cerr(comerr[0], comerr[1], comerr[2]);
deltap = alpha * gT * cerr;
debug << "gxxpT = \n" << gxxpT << "\n\n";
debug << "gT = \n" << gT << "\n\n";
debug << "deltap = \n" << deltap.transpose() << "\n\n";
vec3 dp(deltap(0), deltap(1), deltap(2));
vec3 dq(deltap(3), deltap(4), deltap(5));
state.body_pos += dp;
state.body_rot = quat::fromOmega(-dq) * state.body_rot;
}
if (!fdofs.empty()) {
Eigen::Vector3d cerr(comerr[0], comerr[1], comerr[2]);
deltaf = fscl * gfT * cerr;
debug << "deltaf = \n" << deltaf.transpose() << "\n\n";
for (size_t i=0; i<fdofs.size(); ++i) {
state.jvalues[fdofs[i]] += deltaf(i);
}
}
}
return ok;
}
