/*\define The interface to call the Inverse Kinematics.
*/
public List <float> legInvKin(int foot, Point targetPos, Point targetOri)
{
List <float> jLegDeg = new List <float>();
jLegDeg.Clear();
RotationMatrix targetRot, targetRotX, targetRotZ, targetRotY;
Point targetTrans = targetPos;
targetRotZ = new RotationMatrix(
new Point(Geometry.Cos(targetOri.GetZ()), -Geometry.Sin(targetOri.GetZ()), 0),
new Point(Geometry.Sin(targetOri.GetZ()), Geometry.Cos(targetOri.GetZ()), 0),
new Point(0, 0, 1));
targetRotY = new RotationMatrix(
new Point(1, 0, 0),
new Point(0, Geometry.Cos(targetOri.GetY()), -Geometry.Sin(targetOri.GetY())),
new Point(0, Geometry.Sin(targetOri.GetY()), Geometry.Cos(targetOri.GetY())));
targetRotX = new RotationMatrix(
new Point(Geometry.Cos(targetOri.GetX()), 0, Geometry.Sin(targetOri.GetX())),
new Point(0, 1, 0),
new Point(-Geometry.Sin(targetOri.GetX()), 0, Geometry.Cos(targetOri.GetX())));
targetRot = targetRotY * targetRotX * targetRotZ;
HomogeneousMatrix target = new HomogeneousMatrix(targetRot, targetTrans);
bool isLeft = false;
if (foot == 0)
{
isLeft = true;
}
this.inverseKinematics.calculateLegJoints(target, jLegDeg, isLeft);
if (foot == LEFTFOOT)
{
jLegDeg[1] *= -1;
jLegDeg[5] *= -1;
}
jLegDeg[2] *= -1;
jLegDeg[3] *= -1;
jLegDeg[4] *= -1;
return(jLegDeg);
}
public HomogeneousMatrix getInverse()
{
HomogeneousMatrix result = new HomogeneousMatrix();
RotationMatrix rotationMatrix = new RotationMatrix(this.getRotation().line0, this.getRotation().line1, this.getRotation().line2);
Point translationVector = new Point(this.getTranslation().GetX(), this.getTranslation().GetY(), this.getTranslation().GetZ());
result.setRotation(rotationMatrix.getInverse());
result.setTranslation(
result.getRotation() * new Point(-translationVector.GetX(), -translationVector.GetY(), -translationVector.GetZ()));
return(result);
}
public static HomogeneousMatrix operator *(HomogeneousMatrix thisHomogeneousMatrix, HomogeneousMatrix homogeneousMatrix)
{
HomogeneousMatrix result = new HomogeneousMatrix();
result.elements[0, 0] = thisHomogeneousMatrix.elements[0, 0] * homogeneousMatrix.elements[0, 0] +
thisHomogeneousMatrix.elements[0, 1] * homogeneousMatrix.elements[1, 0] +
thisHomogeneousMatrix.elements[0, 2] * homogeneousMatrix.elements[2, 0] +
thisHomogeneousMatrix.elements[0, 3] * homogeneousMatrix.elements[3, 0];
result.elements[0, 1] = thisHomogeneousMatrix.elements[0, 0] * homogeneousMatrix.elements[0, 1] +
thisHomogeneousMatrix.elements[0, 1] * homogeneousMatrix.elements[1, 1] +
thisHomogeneousMatrix.elements[0, 2] * homogeneousMatrix.elements[2, 1] +
thisHomogeneousMatrix.elements[0, 3] * homogeneousMatrix.elements[3, 1];
result.elements[0, 2] = thisHomogeneousMatrix.elements[0, 0] * homogeneousMatrix.elements[0, 2] +
thisHomogeneousMatrix.elements[0, 1] * homogeneousMatrix.elements[1, 2] +
thisHomogeneousMatrix.elements[0, 2] * homogeneousMatrix.elements[2, 2] +
thisHomogeneousMatrix.elements[0, 3] * homogeneousMatrix.elements[3, 2];
result.elements[0, 3] = thisHomogeneousMatrix.elements[0, 0] * homogeneousMatrix.elements[0, 3] +
thisHomogeneousMatrix.elements[0, 1] * homogeneousMatrix.elements[1, 3] +
thisHomogeneousMatrix.elements[0, 2] * homogeneousMatrix.elements[2, 3] +
thisHomogeneousMatrix.elements[0, 3] * homogeneousMatrix.elements[3, 3];
result.elements[1, 0] = thisHomogeneousMatrix.elements[1, 0] * homogeneousMatrix.elements[0, 0] +
thisHomogeneousMatrix.elements[1, 1] * homogeneousMatrix.elements[1, 0] +
thisHomogeneousMatrix.elements[1, 2] * homogeneousMatrix.elements[2, 0] +
thisHomogeneousMatrix.elements[1, 3] * homogeneousMatrix.elements[3, 0];
result.elements[1, 1] = thisHomogeneousMatrix.elements[1, 0] * homogeneousMatrix.elements[0, 1] +
thisHomogeneousMatrix.elements[1, 1] * homogeneousMatrix.elements[1, 1] +
thisHomogeneousMatrix.elements[1, 2] * homogeneousMatrix.elements[2, 1] +
thisHomogeneousMatrix.elements[1, 3] * homogeneousMatrix.elements[3, 1];
result.elements[1, 2] = thisHomogeneousMatrix.elements[1, 0] * homogeneousMatrix.elements[0, 2] +
thisHomogeneousMatrix.elements[1, 1] * homogeneousMatrix.elements[1, 2] +
thisHomogeneousMatrix.elements[1, 2] * homogeneousMatrix.elements[2, 2] +
thisHomogeneousMatrix.elements[1, 3] * homogeneousMatrix.elements[3, 2];
result.elements[1, 3] = thisHomogeneousMatrix.elements[1, 0] * homogeneousMatrix.elements[0, 3] +
thisHomogeneousMatrix.elements[1, 1] * homogeneousMatrix.elements[1, 3] +
thisHomogeneousMatrix.elements[1, 2] * homogeneousMatrix.elements[2, 3] +
thisHomogeneousMatrix.elements[1, 3] * homogeneousMatrix.elements[3, 3];
result.elements[2, 0] = thisHomogeneousMatrix.elements[2, 0] * homogeneousMatrix.elements[0, 0] +
thisHomogeneousMatrix.elements[2, 1] * homogeneousMatrix.elements[1, 0] +
thisHomogeneousMatrix.elements[2, 2] * homogeneousMatrix.elements[2, 0] +
thisHomogeneousMatrix.elements[2, 3] * homogeneousMatrix.elements[3, 0];
result.elements[2, 1] = thisHomogeneousMatrix.elements[2, 0] * homogeneousMatrix.elements[0, 1] +
thisHomogeneousMatrix.elements[2, 1] * homogeneousMatrix.elements[1, 1] +
thisHomogeneousMatrix.elements[2, 2] * homogeneousMatrix.elements[2, 1] +
thisHomogeneousMatrix.elements[2, 3] * homogeneousMatrix.elements[3, 1];
result.elements[2, 2] = thisHomogeneousMatrix.elements[2, 0] * homogeneousMatrix.elements[0, 2] +
thisHomogeneousMatrix.elements[2, 1] * homogeneousMatrix.elements[1, 2] +
thisHomogeneousMatrix.elements[2, 2] * homogeneousMatrix.elements[2, 2] +
thisHomogeneousMatrix.elements[2, 3] * homogeneousMatrix.elements[3, 2];
result.elements[2, 3] = thisHomogeneousMatrix.elements[2, 0] * homogeneousMatrix.elements[0, 3] +
thisHomogeneousMatrix.elements[2, 1] * homogeneousMatrix.elements[1, 3] +
thisHomogeneousMatrix.elements[2, 2] * homogeneousMatrix.elements[2, 3] +
thisHomogeneousMatrix.elements[2, 3] * homogeneousMatrix.elements[3, 3];
result.elements[3, 0] = thisHomogeneousMatrix.elements[3, 0] * homogeneousMatrix.elements[0, 0] +
thisHomogeneousMatrix.elements[3, 1] * homogeneousMatrix.elements[1, 0] +
thisHomogeneousMatrix.elements[3, 2] * homogeneousMatrix.elements[2, 0] +
thisHomogeneousMatrix.elements[3, 3] * homogeneousMatrix.elements[3, 0];
result.elements[3, 1] = thisHomogeneousMatrix.elements[3, 0] * homogeneousMatrix.elements[0, 1] +
thisHomogeneousMatrix.elements[3, 1] * homogeneousMatrix.elements[1, 1] +
thisHomogeneousMatrix.elements[3, 2] * homogeneousMatrix.elements[2, 1] +
thisHomogeneousMatrix.elements[3, 3] * homogeneousMatrix.elements[3, 1];
result.elements[3, 2] = thisHomogeneousMatrix.elements[3, 0] * homogeneousMatrix.elements[0, 2] +
thisHomogeneousMatrix.elements[3, 1] * homogeneousMatrix.elements[1, 2] +
thisHomogeneousMatrix.elements[3, 2] * homogeneousMatrix.elements[2, 2] +
thisHomogeneousMatrix.elements[3, 3] * homogeneousMatrix.elements[3, 2];
result.elements[3, 3] = thisHomogeneousMatrix.elements[3, 0] * homogeneousMatrix.elements[0, 3] +
thisHomogeneousMatrix.elements[3, 1] * homogeneousMatrix.elements[1, 3] +
thisHomogeneousMatrix.elements[3, 2] * homogeneousMatrix.elements[2, 3] +
thisHomogeneousMatrix.elements[3, 3] * homogeneousMatrix.elements[3, 3];
return(result);
}
public bool calculateLegJoints(HomogeneousMatrix position, List <float> joints, bool isLeft)
{
HomogeneousMatrix target = position;
int sign = 1;
if (isLeft)
{
sign = -1;
}
//Translate to the origin of Leg
target.translate(0, (double)(this.LENGTH_BETWEEN_LEGS / 2) * sign, 0);
//Rotate by 45° around origin for the SimSpark NAO
double angOrt = Math.Sqrt(2.0) * 0.5;
RotationMatrix rotationX_pi_4 = new RotationMatrix(new Point(1, 0, 0), new Point(0, angOrt, angOrt * (-sign)),
new Point(0, angOrt * sign, angOrt));
target.setTranslation(rotationX_pi_4 * target.getTranslation());
target.setRotation(rotationX_pi_4 * target.getRotation());
//Solve the inverse kinematics problem based on Cosine Law
target = target.getInverse();
double length = (target.getTranslation()).getMagnitude();
double sqrLength = Math.Pow(length, 2);
float upperLegLength = this.UPPER_LEG_LENGTH;
double sqrUpperLegLength = Math.Pow(upperLegLength, 2);
float lowerLegLength = this.LOWER_LEG_LENGTH;
double sqrLowerLegLength = Math.Pow(lowerLegLength, 2);
double cosLowerLeg = (sqrLowerLegLength + sqrLength - sqrUpperLegLength) / (2 * lowerLegLength * length);
double cosKnee = (sqrUpperLegLength + sqrLowerLegLength - sqrLength) / (2 * upperLegLength * lowerLegLength);
bool isReachable = true;
if (!(cosKnee >= -1 && cosKnee <= 1))
{
if (cosKnee < -1)
{
cosKnee = -1;
}
if (cosKnee > 1)
{
cosKnee = 1;
}
if (cosLowerLeg < -1)
{
cosLowerLeg = -1;
}
if (cosLowerLeg > 1)
{
cosLowerLeg = 1;
}
isReachable = false;
}
double angKnee = Geometry.PI - Math.Acos(cosKnee);
double angFootPitch = -Math.Acos(cosLowerLeg);
angFootPitch -= Math.Atan2(target.getTranslation().GetX(), new Point(0, target.getTranslation().GetY(),
target.getTranslation().GetZ()).getMagnitude());
double angFootRoll = Math.Atan2(target.getTranslation().GetY(), target.getTranslation().GetZ()) * sign;
RotationMatrix hipFromFoot = new RotationMatrix();
hipFromFoot.rotateX(angFootRoll * -sign);
hipFromFoot.rotateY(-angFootPitch - angKnee);
RotationMatrix hip = hipFromFoot.getInverse() * target.getRotation();
double angHipRoll = Math.Asin(-hip.line1.GetZ()) * -sign;
angHipRoll -= Geometry.PI / 4;
double angHipPitch = -Math.Atan2(hip.line0.GetZ(), hip.line2.GetZ());
double angHipYaw = Math.Atan2(hip.line1.GetX(), hip.line1.GetY()) * -sign;
//Set computed joints in joints list
joints.Clear();
joints.Add((float)Geometry.convertRadianToDegree(angHipYaw));
joints.Add((float)Geometry.convertRadianToDegree(angHipRoll));
joints.Add((float)Geometry.convertRadianToDegree(angHipPitch));
joints.Add((float)Geometry.convertRadianToDegree(angKnee));
joints.Add((float)Geometry.convertRadianToDegree(angFootPitch));
joints.Add((float)Geometry.convertRadianToDegree(angFootRoll));
return(true);
}