Ejemplo n.º 1
0
        //TODO: computation of angle 2 + 3 is dependent ont the wrist point relative to axis 2
        // when lloking backward wristpoint is completly different. need to fix that up.
        public void GetAngles1To3(XPoint wristPoint, List <DHParameter> dhParam)
        {
            var wristToAxis1 = new XPoint(dhParam.First().JointStandardTransform.DenseMatrix *wristPoint.DensePoint3D);

            double angleDeg1Plus  = Math.Atan2(wristToAxis1.Y, wristToAxis1.X) * 180 / Math.PI;
            double angleDeg1Minus = Math.Atan2(-wristToAxis1.Y, -wristToAxis1.X) * 180 / Math.PI;

            ResultAxisSolutions.Add(new double[] { angleDeg1Plus, angleDeg1Minus });

            // TODO: needs more commenting and finalization.
            var rootToAxis2Trans = new TransformationMatrix(dhParam.First().JointTransform.DenseMatrix *dhParam[1].JointStandardTransform.DenseMatrix);

            // this is the transformation from joint 2 to the wrist center point
            XPoint wristToAxis2 = new XPoint(
                (DenseMatrix)rootToAxis2Trans.DenseMatrix.Inverse() *
                wristPoint.DensePoint3D);

            double distanceXYPlane    = Math.Sqrt(Math.Pow(wristToAxis2.Y, 2) + Math.Pow(wristToAxis2.Z, 2));
            double distanceZDirection = wristToAxis2.X;

            double a = Math.Sqrt(Math.Pow(dhParam[3].A, 2) + Math.Pow(dhParam[3].D, 2));
            double b = dhParam[2].A;
            double c = Math.Sqrt(Math.Pow(distanceXYPlane, 2) + Math.Pow(distanceZDirection, 2));

            double angle1 = Math.Acos((Math.Pow(a, 2) - Math.Pow(b, 2) - Math.Pow(c, 2)) / (-2 * c * b));
            double angle2 = Math.Acos((Math.Pow(c, 2) - Math.Pow(b, 2) - Math.Pow(a, 2)) / (-2 * b * a));

            double angle2Deg = 90 - angle2.RadToDeg();

            double flipFlapLookForward  = Math.Atan2(distanceZDirection, 1 * distanceXYPlane);
            double flipFlapLookBackward = Math.Atan2(distanceZDirection, -1 * distanceXYPlane);

            double lookForwardSolution1 = Math.PI / 2 - (flipFlapLookForward + angle1);
            double lookForwardSolution2 = Math.PI / 2 - angle2;
            double lookForwardSolution3 = Math.PI / 2 - (flipFlapLookForward - angle1);
            double lookForwardSolution4 = angle2 - Math.PI * 1.5;

            double lookBackwardSolution1 = Math.PI / 2 - (flipFlapLookBackward + angle1);
            double lookBackwardSolution2 = Math.PI / 2 - angle2;
            double lookBackwardSolution3 = Math.PI / 2 - (flipFlapLookBackward - angle1);
            double lookBackwardSolution4 = angle2 - Math.PI * 1.5;

            ResultAxisSolutions.Add(new double[] {
                lookForwardSolution1.RadToDeg() - dhParam[1].AngleOffset,                  //look forward elbow up axis 2
                lookForwardSolution3.RadToDeg() - dhParam[1].AngleOffset,                  //look forward elbow down axis 2
                lookBackwardSolution1.RadToDeg() - dhParam[1].AngleOffset,                 //look backward elbow up axis 2
                lookBackwardSolution3.RadToDeg() - dhParam[1].AngleOffset,                 //look backward elbow down axis 2
            });

            ResultAxisSolutions.Add(new double[] {
                lookForwardSolution2.RadToDeg() - dhParam[2].AngleOffset,                  //look forward elbow up axis 3
                lookForwardSolution4.RadToDeg() - dhParam[2].AngleOffset,                  //look forward elbow down axis 3
                lookBackwardSolution2.RadToDeg() - dhParam[2].AngleOffset,                 //look backward elbow up axis 3
                lookBackwardSolution4.RadToDeg() - dhParam[2].AngleOffset,                 //look backward elbow down axis 3
            });
        }
Ejemplo n.º 2
0
        public void GetAngles4To6(TransformationMatrix targetMatrix, List <DHParameter> dhParam)
        {
            //TODO: this is only temporary to check the correctness of the algorithm
            var target = targetMatrix.DenseMatrix;

            var tempRes = dhParam[0].JointTransform.DenseMatrix *
                          dhParam[1].JointTransform.DenseMatrix *
                          dhParam[2].JointTransform.DenseMatrix;

            var rotMat4To6 = tempRes.Inverse() * target;

            double R23 = rotMat4To6[1, 2];
            double R22 = rotMat4To6[1, 1];
            double R21 = rotMat4To6[1, 0];
            double R33 = rotMat4To6[2, 2];
            double R13 = rotMat4To6[0, 2];

            double[] theta4 = new double[2];
            double[] theta5 = new double[2];
            double[] theta6 = new double[2];

            if (!R33.Equals(1.0))
            {
                theta5[0] = Math.Acos(R23);
                theta5[1] = -Math.Acos(R23);
            }

            if (Math.Sin(theta5[0]).DoubleEquals(0.0))
            {
                theta4[0] = -Math.Atan2(R33, -R13);
            }
            else
            {
                theta4[0] = -Math.Atan2(R33 / Math.Sin(theta5[0]), -R13 / Math.Sin(theta5[0]));
            }

            if (Math.Sin(theta5[1]).DoubleEquals(0.0))
            {
                theta4[1] = -Math.Atan2(R33, -R13);
            }
            else
            {
                theta4[1] = -Math.Atan2(R33 / Math.Sin(theta5[1]), -R13 / Math.Sin(theta5[1]));
            }

            if (Math.Sin(theta5[0]).DoubleEquals(0.0))
            {
                theta6[1] = Math.Atan2(R22, R21);
            }
            else
            {
                theta6[1] = Math.Atan2(R22 / Math.Sin(theta5[0]), R21 / Math.Sin(theta5[0]));
            }

            if (Math.Sin(theta5[1]).DoubleEquals(0.0))
            {
                theta6[0] = Math.Atan2(R22, R21);
            }
            else
            {
                theta6[0] = Math.Atan2(R22 / Math.Sin(theta5[1]), R21 / Math.Sin(theta5[1]));
            }

            ResultAxisSolutions.Add(new double[] { theta4[0].RadToDeg(), theta4[1].RadToDeg() });
            ResultAxisSolutions.Add(new double[] { theta5[0].RadToDeg(), theta5[1].RadToDeg() });
            ResultAxisSolutions.Add(new double[] { theta6[0].RadToDeg(), theta6[1].RadToDeg() });
        }