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; }
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg, const vec3f& pos, vec3f* q_bent_forward, vec3f* q_bent_rearward ) const { if ((int)leg < 0 || (int)leg >= NUM_LEGS) { return CMUK_BAD_LEG_INDEX; } else if (!q_bent_forward || !q_bent_rearward) { return CMUK_INSUFFICIENT_ARGUMENTS; } debug << "*** computing IK...\n"; int hipflags = 0; // subtract off hip position vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK); vec3f orig = pos; // get dist from hip rx joint to y rotation plane const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1]; // get the squared length of the distance on the plane float yz = p[1]*p[1] + p[2]*p[2]; // alpha is the angle of the foot in the YZ plane with respect to the Y axis float alpha = atan2(p[2], p[1]); // h is the distance of foot from hip in YZ plane float h = sqrt(yz); // beta is the angle between the foot-hip vector (projected in YZ // plane) and the top hip link. float cosbeta = d / h; debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n"; if (fabs(cosbeta) > 1) { debug << "violated triangle inequality when calculating hip_rx_angle!\n" ; if (fabs(cosbeta) - 1 > 1e-4) { hipflags = hipflags | IK_UPPER_DISTANCE; } cosbeta = (cosbeta < 0) ? -1 : 1; if (yz < 1e-4) { p[1] = d; p[2] = 0; } else { float scl = fabs(d) / h; p[1] *= scl; p[2] *= scl; orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK); } } float beta = acos(cosbeta); // Now compute the two possible hip angles float hip_rx_angles[2], badness[2]; int flags[2]; flags[0] = hipflags; flags[1] = hipflags; hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI); hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI); const float& min = jl(_kc, leg, HIP_RX, 0); const float& max = jl(_kc, leg, HIP_RX, 1); // See how badly we violate the joint limits for this hip angles for (int i=0; i<2; ++i) { float& angle = hip_rx_angles[i]; badness[i] = fabs(compute_badness(angle, min, max)); if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; } } // Put the least bad (and smallest) hip angle first bool swap = false; if ( badness[1] <= badness[0] ) { // We want the less bad solution for hip angle swap = true; } else if (badness[0] == 0 && badness[1] == 0) { // We want the solution for hip angle that leaves the hip up. if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) { swap = true; } else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) { swap = true; } } if (swap) { std::swap(hip_rx_angles[0], hip_rx_angles[1]); std::swap(badness[0], badness[1]); std::swap(flags[0], flags[1]); } int hip_solution_cnt = 2; if (badness[0] == 0 && badness[1] != 0) { hip_solution_cnt = 1; } debug << "hip_rx_angles[0]=" << hip_rx_angles[0] << ", badness=" << badness[0] << ", flags=" << flags[0] << "\n"; debug << "hip_rx_angles[1]=" << hip_rx_angles[1] << ", badness=" << badness[1] << ", flags=" << flags[1] << "\n"; debug << "hip_solution_cnt = " << hip_solution_cnt << "\n"; vec3f qfwd[2], qrear[2]; for (int i=0; i<hip_solution_cnt; ++i) { debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n"; float hip_rx = hip_rx_angles[i]; // now make inv. transform to get rid of hip rotation Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK)); vec3f ptx = tx.transformInv(orig); debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n"; // calculate lengths for cosine law float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK); float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK); float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK); float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK); float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2]; float k = sqrt(ksqr); debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n"; // check triangle inequality if (k > l1 + l2) { debug << "oops, violated the triangle inequality for lower segments: " << "k = " << k << ", " << "l1 + l2 = " << l1 + l2 << "\n"; if (k - (l1 + l2) > 1e-4) { flags[i] = flags[i] | IK_LOWER_DISTANCE; } k = l1 + l2; ksqr = k * k; } // 2*theta is the acute angle formed by the spread // of the two hip rotations... float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k); if (fabs(costheta) > 1) { debug << "costheta = " << costheta << " > 1\n"; if (fabs(costheta) - 1 > 1e-4) { flags[i] = flags[i] | IK_LOWER_DISTANCE; } costheta = (costheta < 0) ? -1 : 1; } float theta = acos(costheta); // gamma is the angle of the foot with respect to the z axis float gamma = atan2(-ptx[0], -ptx[2]); // hip angles are just offsets off of gamma now float hip_ry_1 = gamma - theta; float hip_ry_2 = gamma + theta; // phi is the obtuse angle of the parallelogram float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2); if (fabs(cosphi) > 1) { debug << "cosphi = " << cosphi << " > 1\n"; if (fabs(cosphi) - 1 > 1e-4) { flags[i] = flags[i] | IK_LOWER_DISTANCE; } cosphi = (cosphi < 0) ? -1 : 1; } float phi = acos(cosphi); // epsilon is the "error" caused by not having feet offset directly // along the z-axis (if they were, epsilon would equal zero) float epsilon = le(_kc, leg, _centeredFootIK); // now we can directly solve for knee angles float knee_ry_1 = M_PI - phi - epsilon; float knee_ry_2 = -M_PI + phi - epsilon; // now fill out angle structs and check limits qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1); qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2); debug << "before wrap, qfwd = " << qfwd[i] << "\n"; debug << "before wrap, qrear = " << qrear[i] << "\n"; check_wrap(_kc, qfwd[i], leg); check_wrap(_kc, qrear[i], leg); debug << "after wrap, qfwd = " << qfwd[i] << "\n"; debug << "after wrap, qrear = " << qrear[i] << "\n"; if (!check_limits(_kc, qfwd[i], leg)) { debug << "violated limits forward!\n"; flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD; } if (!check_limits(_kc, qrear[i], leg)) { debug << "violated limits rearward!\n"; flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR; } } // for each viable hip solution int best = 0; if (hip_solution_cnt == 2) { if (howbad(flags[0]) > howbad(flags[1])) { best = 1; } debug << "best overall solution is " << (best+1) << "\n"; } *q_bent_forward = qfwd[best]; *q_bent_rearward = qrear[best]; return flags_to_errcode(flags[best]); }