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);
        }
Example #2
0
        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);
        }