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; }