예제 #1
0
파일: Puma.cs 프로젝트: awitwicka/PUMA
        private Matrix[] CalculateTransformation()
        {
            var    F0  = new ArmOrientation(Vector3.Right, Vector3.Up, Vector3.Backward, Vector3.Zero);
            var    F01 = Matrix.CreateFromAxisAngle(F0.AlphaY, Angle1);
            Matrix F1  = F01 * F0.ToMatrix();

            //var F12 = Matrix.CreateTranslation(Vector3.Up * Length1) * Matrix.CreateFromAxisAngle(F1.Forward, Angle2); //here
            //Matrix F2 = F1 * F12;
            Matrix F2 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) *
                        F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1);

            //var F23 = Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3);
            //Matrix F3 = F2 * F23;
            Matrix F3 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
                        F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2); //ok

            //var F34 = Matrix.CreateTranslation(Vector3.Up * -Length3) * Matrix.CreateFromAxisAngle(F3.Up, Angle4);
            //Matrix F4 = F3 * F34;
            Matrix F4 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
                        Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) *
                        F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3);

            //var F45 = Matrix.CreateTranslation(Vector3.Right * Length4) * Matrix.CreateFromAxisAngle(F4.Right, Angle5);
            //Matrix F5 = F4 * F45;
            Matrix F5 = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
                        Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) * Matrix.CreateFromAxisAngle(F4.Up, Angle5) *
                        F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3) * Matrix.CreateTranslation(F4.Up * Length4);

            Matrix EndPoint = F01 * Matrix.CreateFromAxisAngle(F1.Forward, Angle2) * Matrix.CreateFromAxisAngle(F2.Forward, Angle3) *
                              Matrix.CreateFromAxisAngle(F3.Forward, (float)(2 * Math.PI - Math.PI / 2)) * Matrix.CreateFromAxisAngle(F3.Up, -Angle4) * Matrix.CreateFromAxisAngle(F4.Up, Angle5) *
                              F0.ToMatrix() * Matrix.CreateTranslation(F1.Up * Length1) * Matrix.CreateTranslation(F2.Up * Length2) * Matrix.CreateTranslation(F3.Up * Length3) * Matrix.CreateTranslation(F4.Up * Length4) * Matrix.CreateTranslation(F5.Up * Length5);

            Matrix[] result = { F1, F2, F3, F4, F5, EndPoint };

            return(result);
        }
예제 #2
0
파일: Puma.cs 프로젝트: awitwicka/PUMA
        private Configuration CalculateAnglesFromEndPosition(ArmOrientation endOrientation)
        {
            //check for Asin = NaN (3 places)
            //take best option from angle4
            //chaneck ranges


            //flipping z axis to match puma cooedinate system -> TODO: move it to QuaternionRotation in Cylinder
            endOrientation.Position.Z *= -1;
            Configuration angles = new Configuration();


            /******* angle 1 & 4 **********/

            angles.Angle1 = (float)Math.Atan2(endOrientation.Position.Z - Length4 * endOrientation.AlphaX.Z, endOrientation.Position.X - Length4 * endOrientation.AlphaX.X);

            angles.Angle4 = (float)Math.Asin(Math.Cos(angles.Angle1) * endOrientation.AlphaX.Z - Math.Sin(angles.Angle1) * endOrientation.AlphaX.X); //1 or 2 sol for given a1
            //sec sol
            float angle4a = ((Angle4 > 0) ? (float)Math.PI - Angle4 : -(float)Math.PI + Angle4);


            /********* angle 5 ***********/

            var aCos = (Math.Cos(angles.Angle1) * endOrientation.AlphaZ.Z - Math.Sin(angles.Angle1) * endOrientation.AlphaZ.X) / (Math.Cos(angles.Angle4));

            if (aCos > 1)
            {
                aCos = 1;
            }
            if (aCos < -1)
            {
                aCos = -1;
            }
            var   Angle5a  = (float)Math.Acos(aCos); //uniq
            float angle5a2 = -Angle5a;
            var   Angle5b  = (float)Math.Asin((Math.Sin(angles.Angle1) * endOrientation.AlphaY.X - Math.Cos(angles.Angle1) * endOrientation.AlphaY.Z) / (Math.Cos(angles.Angle4)));
            float angle5b2 = ((Angle5b > 0) ? (float)Math.PI - Angle5b : -(float)Math.PI - Angle5b);

            if (Angle5b > 0)
            {
                if (Angle5a > 0)
                {
                    angles.Angle5 = Angle5a;
                }
                else
                {
                    angles.Angle5 = angle5a2;
                }
            }
            else
            {
                if (Angle5a < 0)
                {
                    angles.Angle5 = Angle5a;
                }
                else
                {
                    angles.Angle5 = angle5a2;
                }
            }


            /********* angle 2 and length ***********/

            angles.Angle2 = (float)Math.Atan2(-(Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4) * (endOrientation.Position.Y - Length4 * endOrientation.AlphaX.Y - Length1) + Length3 * (endOrientation.AlphaX.X + Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4))),
                                              Math.Cos(angles.Angle4) * (endOrientation.Position.X - Length4 * endOrientation.AlphaX.X) - Math.Cos(angles.Angle1) * Length3 * endOrientation.AlphaX.Y);

            angles.Length2 = (float)((Math.Cos(angles.Angle4) * (endOrientation.Position.X - Length4 * endOrientation.AlphaX.X) - Math.Cos(angles.Angle1) * Length3 * endOrientation.AlphaX.Y)
                                     / (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle2) * Math.Cos(angles.Angle4)));


            /********* anglee 3 *******************/

            float angle23;

            aCos = (endOrientation.AlphaX.X + (Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4))) / (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4));
            if (aCos > 1)
            {
                aCos = 1;
            }
            if (aCos < -1)
            {
                aCos = -1;
            }
            var angle23Cos  = (float)Math.Acos(aCos);
            var angle23Cos2 = -angle23Cos;
            var angle23Sin  = (float)Math.Asin(-(endOrientation.AlphaX.Y) / Math.Cos(angles.Angle4));
            var angle23Sin2 = (angle23Sin > 0) ? (float)Math.PI - angle23Sin : -(float)Math.PI - angle23Sin;

            if (angle23Sin > 0)
            {
                if (angle23Cos > 0)
                {
                    angle23 = angle23Cos;
                }
                else
                {
                    angle23 = angle23Cos2;
                }
            }
            else
            {
                if (angle23Cos < 0)
                {
                    angle23 = angle23Cos;
                }
                else
                {
                    angle23 = angle23Cos2;
                }
            }

            angles.Angle3 = angle23 - angles.Angle2;

            /*var Angle3a = (float)Math.Acos((endOrientation.AlphaX.X + Math.Sin(angles.Angle1) * Math.Sin(angles.Angle4)) / (Math.Cos(angles.Angle1) * Math.Cos(angles.Angle4)))
             *  - angles.Angle2; //uniq
             * //float angle3a2 = ((Angle3a < 0) ? Angle3a + (float)Math.PI : Angle3a - (float)Math.PI);
             * angles.Angle3 = (float)Math.Asin(-endOrientation.AlphaX.Y / Math.Cos(angles.Angle4)) - angles.Angle2;
             *
             */
            //+ start position
            angles.Angle2 += (float)Math.PI / 2;
            angles.Angle3 += (float)Math.PI / 2;

            //range check
            if (angles.Angle1 <= -(float)Math.PI)
            {
                angles.Angle2 += 2 * (float)Math.PI;
            }
            if (angles.Angle2 <= -(float)Math.PI)
            {
                angles.Angle2 += 2 * (float)Math.PI;
            }
            if (angles.Angle3 <= -(float)Math.PI)
            {
                angles.Angle2 += 2 * (float)Math.PI;
            }
            if (angles.Angle4 <= -(float)Math.PI)
            {
                angles.Angle2 += 2 * (float)Math.PI;
            }
            if (angles.Angle5 <= -(float)Math.PI)
            {
                angles.Angle2 += 2 * (float)Math.PI;
            }

            if (angles.Angle1 > (float)Math.PI)
            {
                angles.Angle2 -= 2 * (float)Math.PI;
            }
            if (angles.Angle2 > (float)Math.PI)
            {
                angles.Angle2 -= 2 * (float)Math.PI;
            }
            if (angles.Angle3 > (float)Math.PI)
            {
                angles.Angle2 -= 2 * (float)Math.PI;
            }
            if (angles.Angle4 > (float)Math.PI)
            {
                angles.Angle2 -= 2 * (float)Math.PI;
            }
            if (angles.Angle5 > (float)Math.PI)
            {
                angles.Angle2 -= 2 * (float)Math.PI;
            }

            return(angles);
        }