//Calculates the rotation matrix for the rotation around Z axis
private RotationMatrix GetRotationMatrixZ(double angle)
{
RotationMatrix result = new RotationMatrix();
result.line0 = new Point(Math.Cos(angle), -Math.Sin(angle), 0);
result.line1 = new Point(Math.Sin(angle), Math.Cos(angle), 0);
result.line2 = new Point(0, 0, 1);
return(result);
}
public void setRotation(RotationMatrix rotationMatrix)
{
this.elements[0, 0] = rotationMatrix.line0.GetX();
this.elements[0, 1] = rotationMatrix.line0.GetY();
this.elements[0, 2] = rotationMatrix.line0.GetZ();
this.elements[1, 0] = rotationMatrix.line1.GetX();
this.elements[1, 1] = rotationMatrix.line1.GetY();
this.elements[1, 2] = rotationMatrix.line1.GetZ();
this.elements[2, 0] = rotationMatrix.line2.GetX();
this.elements[2, 1] = rotationMatrix.line2.GetY();
this.elements[2, 2] = rotationMatrix.line2.GetZ();
}
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);
}
/*\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 RotationMatrix getRotation()
{
RotationMatrix result = new RotationMatrix();
result.line0.SetX(this.elements[0, 0]);
result.line0.SetY(this.elements[0, 1]);
result.line0.SetZ(this.elements[0, 2]);
result.line1.SetX(this.elements[1, 0]);
result.line1.SetY(this.elements[1, 1]);
result.line1.SetZ(this.elements[1, 2]);
result.line2.SetX(this.elements[2, 0]);
result.line2.SetY(this.elements[2, 1]);
result.line2.SetZ(this.elements[2, 2]);
return(result);
}
//Overloaded constructor
public HomogeneousMatrix(RotationMatrix rotationMatrix)
{
this.elements[0, 0] = rotationMatrix.line0.GetX();
this.elements[0, 1] = rotationMatrix.line0.GetY();
this.elements[0, 2] = rotationMatrix.line0.GetZ();
this.elements[1, 0] = rotationMatrix.line1.GetX();
this.elements[1, 1] = rotationMatrix.line1.GetY();
this.elements[1, 2] = rotationMatrix.line1.GetZ();
this.elements[2, 0] = rotationMatrix.line2.GetX();
this.elements[2, 1] = rotationMatrix.line2.GetY();
this.elements[2, 2] = rotationMatrix.line2.GetZ();
this.elements[0, 3] = 0;
this.elements[1, 3] = 0;
this.elements[2, 3] = 0;
this.elements[3, 0] = this.elements[3, 1] = this.elements[3, 2] = 0;
this.elements[3, 3] = 1;
}
// -> Order of multiplication of matrices matter
public static RotationMatrix operator *(RotationMatrix thisRotationMatrix, RotationMatrix rotationMatrix)
{
RotationMatrix result = new RotationMatrix();
//Setting result.line0
result.line0.SetX(thisRotationMatrix.line0.GetX() * rotationMatrix.line0.GetX() +
thisRotationMatrix.line0.GetY() * rotationMatrix.line1.GetX() +
thisRotationMatrix.line0.GetZ() * rotationMatrix.line2.GetX());
result.line0.SetY(thisRotationMatrix.line0.GetX() * rotationMatrix.line0.GetY() +
thisRotationMatrix.line0.GetY() * rotationMatrix.line1.GetY() +
thisRotationMatrix.line0.GetZ() * rotationMatrix.line2.GetY());
result.line0.SetZ(thisRotationMatrix.line0.GetX() * rotationMatrix.line0.GetZ() +
thisRotationMatrix.line0.GetY() * rotationMatrix.line1.GetZ() +
thisRotationMatrix.line0.GetZ() * rotationMatrix.line2.GetZ());
//Setting result.line1
result.line1.SetX(thisRotationMatrix.line1.GetX() * rotationMatrix.line0.GetX() +
thisRotationMatrix.line1.GetY() * rotationMatrix.line1.GetX() +
thisRotationMatrix.line1.GetZ() * rotationMatrix.line2.GetX());
result.line1.SetY(thisRotationMatrix.line1.GetX() * rotationMatrix.line0.GetY() +
thisRotationMatrix.line1.GetY() * rotationMatrix.line1.GetY() +
thisRotationMatrix.line1.GetZ() * rotationMatrix.line2.GetY());
result.line1.SetZ(thisRotationMatrix.line1.GetX() * rotationMatrix.line0.GetZ() +
thisRotationMatrix.line1.GetY() * rotationMatrix.line1.GetZ() +
thisRotationMatrix.line1.GetZ() * rotationMatrix.line2.GetZ());
//Setting result.line2
result.line2.SetX(thisRotationMatrix.line2.GetX() * rotationMatrix.line0.GetX() +
thisRotationMatrix.line2.GetY() * rotationMatrix.line1.GetX() +
thisRotationMatrix.line2.GetZ() * rotationMatrix.line2.GetX());
result.line2.SetY(thisRotationMatrix.line2.GetX() * rotationMatrix.line0.GetY() +
thisRotationMatrix.line2.GetY() * rotationMatrix.line1.GetY() +
thisRotationMatrix.line2.GetZ() * rotationMatrix.line2.GetY());
result.line2.SetZ(thisRotationMatrix.line2.GetX() * rotationMatrix.line0.GetZ() +
thisRotationMatrix.line2.GetY() * rotationMatrix.line1.GetZ() +
thisRotationMatrix.line2.GetZ() * rotationMatrix.line2.GetZ());
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);
}
