std::vector<double> darwinop_kinematics_inverse_leg( Transform trLeg, int leg, double) { std::vector<double> qLeg(6); bool left = (leg == LEG_LEFT); // Left leg Transform trInvLeg = inv(trLeg); // printTransform(trInvLeg); // printTransform(trLeg); // Hip Offset vector in Torso frame double xHipOffset[3]; if (left) { xHipOffset[0] = 0; xHipOffset[1] = hipOffsetY; xHipOffset[2] = -hipOffsetZ; } else { xHipOffset[0] = 0; xHipOffset[1] = -hipOffsetY; xHipOffset[2] = -hipOffsetZ; } // Hip Offset in Leg frame double xLeg[3]; for (int i = 0; i < 3; i++) xLeg[i] = xHipOffset[i]; trInvLeg.apply(xLeg); xLeg[2] -= footHeight; // Knee pitch double dLeg = xLeg[0]*xLeg[0] + xLeg[1]*xLeg[1] + xLeg[2]*xLeg[2]; double cKnee = .5*(dLeg-dTibia*dTibia-dThigh*dThigh)/(dTibia*dThigh); if (cKnee > 1) cKnee = 1; if (cKnee < -1) cKnee = -1; double kneePitch = acos(cKnee); // Angle pitch and roll double ankleRoll = atan2(xLeg[1], xLeg[2]); double lLeg = sqrt(dLeg); if (lLeg < 1e-16) lLeg = 1e-16; double pitch0 = asin(dThigh*sin(kneePitch)/lLeg); double anklePitch = asin(-xLeg[0]/lLeg) - pitch0; Transform rHipT = trLeg; rHipT = rHipT.rotateX(-ankleRoll).rotateY(-anklePitch-kneePitch); double hipYaw = atan2(-rHipT(0,1), rHipT(1,1)); double hipRoll = asin(rHipT(2,1)); double hipPitch = atan2(-rHipT(2,0), rHipT(2,2)); // Need to compensate for KneeOffsetX: qLeg[0] = hipYaw; qLeg[1] = hipRoll; qLeg[2] = hipPitch-aThigh; qLeg[3] = kneePitch+aThigh+aTibia; qLeg[4] = anklePitch-aTibia; qLeg[5] = ankleRoll; return qLeg; }
std::vector<double> nao_kinematics_inverse_leg( Transform trLeg, int leg, double hipYawPitch) { std::vector<double> qLeg(6); bool left = (leg == LEG_LEFT); // Left leg bool calcHipYawPitch = (hipYawPitch > 0); Transform trInvLeg = inv(trLeg); // printTransform(trInvLeg); // printTransform(trLeg); // Hip Offset vector in Torso frame double xHipOffset[3]; if (left) { xHipOffset[0] = 0; xHipOffset[1] = hipOffsetY; xHipOffset[2] = -hipOffsetZ; } else { xHipOffset[0] = 0; xHipOffset[1] = -hipOffsetY; xHipOffset[2] = -hipOffsetZ; } // Hip Offset in Leg frame double xLeg[3]; for (int i = 0; i < 3; i++) xLeg[i] = xHipOffset[i]; trInvLeg.apply(xLeg); xLeg[2] -= footHeight; // Knee pitch double dLeg = xLeg[0]*xLeg[0] + xLeg[1]*xLeg[1] + xLeg[2]*xLeg[2]; double cKnee = .5*(dLeg -tibiaLength*tibiaLength -thighLength*thighLength)/ (tibiaLength*thighLength); if (cKnee > 1) cKnee = 1; if (cKnee < -1) cKnee = -1; double kneePitch = acos(cKnee); // Angle pitch and roll double ankleRoll = atan2(xLeg[1], xLeg[2]); double lLeg = sqrt(dLeg); if (lLeg < 1e-16) lLeg = 1e-16; double pitch0 = asin(thighLength*sin(kneePitch)/lLeg); double anklePitch = asin(-xLeg[0]/lLeg) - pitch0; if (calcHipYawPitch) { // Use hip rotation to calculate HipYawPitch Transform rHipT = trLeg; rHipT = rHipT.rotateX(-ankleRoll).rotateY(-anklePitch-kneePitch); if (left) hipYawPitch = atan2(SQRT2*rHipT(0,1), rHipT(1,1)+rHipT(2,1)); else hipYawPitch = atan2(-SQRT2*rHipT(0,1), rHipT(1,1)-rHipT(2,1)); // printf("hipYawPitch = %g\n", hipYawPitch); } double xAnkle[3]; xAnkle[0] = 0; xAnkle[1] = 0; xAnkle[2] = footHeight; trLeg.apply(xAnkle); for (int i = 0; i < 3; i++) { xAnkle[i] -= xHipOffset[i]; } if (left) { inv(rotYawPitchLeft(hipYawPitch)).apply(xAnkle); } else { inv(rotYawPitchRight(hipYawPitch)).apply(xAnkle); } // for (int i = 0; i < 3; i++) printf("xAnkle[%d] = %g\n", i, xAnkle[i]); double hipRoll = atan2(xAnkle[1], -xAnkle[2]); double pitch1 = asin(tibiaLength*sin(kneePitch)/lLeg); double hipPitch = asin(-xAnkle[0]/lLeg) - pitch1; qLeg[0] = hipYawPitch; qLeg[1] = hipRoll; qLeg[2] = hipPitch; qLeg[3] = kneePitch; qLeg[4] = anklePitch; qLeg[5] = ankleRoll; return qLeg; }