public static void fkTest(bool b)
        {
            float[] x      = { 0f, 0f, 0f };
            float[] y      = { 0f, 0f, 0f };
            float[] z      = { 0f, 0f, 0f };
            float[] length = { 50, 50, 50 };
            float[] dest   = { 70, 70, 70 };
            int[]   Xmin   = { 0, 0, 0 };
            int[]   Xmax   = { 360, 360, 360 };
            int[]   Ymin   = { 0, 0, 0 };
            int[]   Ymax   = { 360, 360, 360 };
            int[]   Zmin   = { 0, 0, 0 };
            int[]   Zmax   = { 360, 360, 360 };

            ForwardKinematics fk = new ForwardKinematics(x, y, z, length, Xmin, Xmax, Ymin, Ymax, Zmin, Zmax);

            if (b)
            {
                ikTest(fk, dest);
            }

            float[] ep = fk.getEndpoint();

            Console.WriteLine(ep[0] + " " + ep[1] + " " + ep[2]);
        }
Example #2
0
        public IK(ForwardKinematics FK, float[] destination)
        {
            this.FK          = FK;
            this.destination = destination;
            currentPos       = FK.getEndpoint();

            cost = calculateCost();
        }
        public static void ikTest(ForwardKinematics fk, float[] dest)
        {
            IK inv = new IK(fk, dest);

            float[][] finalAngles = inv.getAngles();
        }