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