Пример #1
0
        public AssociativeTable(Manipulator Agent, int entries)
        {
            Table = new Dictionary <double, double[]>();
            double[] q_init = new double[IKP.ParamNum];
            Array.Copy(Agent.q, q_init, IKP.ParamNum);

            Random rng    = new Random();
            IKP    Solver = new IKP(0.02, Agent.Links.Length, 10, 0.2, 50);

            for (int i = 0; i < 3600; i++)
            {
                double d = 0.04, a = i;  //a = Math.Round(-Math.PI + 2 * Math.PI * rng.NextDouble(), 2);
                Vector v = new Vector(d * Math.Cos(a / 10), d * Math.Sin(a / 10));

                var res = Solver.Execute(Agent.GripperPos + v);
                Array.Copy(q_init, Agent.q, IKP.ParamNum);
                while (!res.Item1)
                {
                    res = Solver.Execute(Agent.GripperPos + v);
                    Array.Copy(q_init, Agent.q, IKP.ParamNum);
                }

                Table.Add(a, res.Item3);
            }
        }
Пример #2
0
        public static void Execute()
        {
            States = new Dictionary <string, bool>();
            States.Add("Obstacles", false);
            States.Add("Goal", false);
            States.Add("Path", false);
            States.Add("Joints", false);
            States.Add("Tree", false);
            States.Add("Attractors", false);

            //manipulator
            Manip = new Manipulator
            {
                Links = new double[] { 2, 2, 2, 2 },
                q     = Misc.ToRad(new double[] { 0, 0, 0, 0 }),
                Base  = Point.Zero
            };

            double MaxStep = 1;

            Manip.StepRanges = new double[Manip.Links.Length, 2];
            for (int i = 0; i < Manip.Links.Length; i++)
            {
                Manip.StepRanges[i, 0] = -MaxStep;
                Manip.StepRanges[i, 1] = MaxStep;
            }

            //obstacles
            Obstacles = new Obstacle[2];
            Point[] obst_data = new Point[360];
            for (int i = 0; i < obst_data.Length; i++)
            {
                obst_data[i] = new Point
                               (
                    Math.Cos(i * Math.PI / 180) + 0,
                    Math.Sin(i * Math.PI / 180) + 1.5
                               );
            }
            Obstacles[0] = new Obstacle(obst_data);
            for (int i = 0; i < obst_data.Length; i++)
            {
                obst_data[i] = new Point
                               (
                    Math.Cos(i * Math.PI / 180) - 2.2,
                    Math.Sin(i * Math.PI / 180) + 0
                               );
            }
            Obstacles[1]        = new Obstacle(obst_data);
            States["Obstacles"] = true;

            //goal
            Goal           = new Point(-2 * Math.Sqrt(2) - 2, 0);
            States["Goal"] = true;

            //initialize genetic algorithm parameters
            Algorithm.Initialize(Manip, Obstacles);

            IKP Solver = new IKP(0.02, Manip.Links.Length, 10, 0.2, 50);

            Attractors = new List <Attractor>();

            Random rng = new Random();
            double work_radius = Manip.Links.Sum(), x, y_plus, y_minus, y;

            //adding main attractor
            Point AttrPoint = Goal;

            double AttrWeight = Manip.DistanceTo(Goal);

            Point[] AttrArea = new Point[180];
            double  r        = 0.05 * Math.Pow(AttrWeight / Manip.DistanceTo(Goal), 4);

            for (int i = 0; i < AttrArea.Length; i++)
            {
                AttrArea[i] = new Point
                              (
                    r * Math.Cos(i * Math.PI / (AttrArea.Length / 2)) + AttrPoint.x,
                    r * Math.Sin(i * Math.PI / (AttrArea.Length / 2)) + AttrPoint.y
                              );
            }

            Attractors.Add(new Attractor(AttrPoint, AttrWeight, AttrArea, r));

            //adding ancillary attractors
            while (Attractors.Count < 1000)
            {
                //work_radius = Manip.Links.Sum();
                x            = Manip.Base.x + rng.NextDouble() * 2 * work_radius - work_radius;
                y_plus       = Math.Sqrt(work_radius * work_radius - x * x);
                y_minus      = -y_plus;
                y            = Manip.Base.y + (rng.NextDouble() * 2 * (y_plus - y_minus) - (y_plus - y_minus)) / 2;
                work_radius -= Manip.Links.Sum() / 2000;

                Point p         = new Point(x, y);
                bool  collision = false;
                foreach (var obst in Obstacles)
                {
                    if (obst.Contains(p))
                    {
                        collision = true;
                        break;
                    }
                }

                if (!collision)
                {
                    AttrPoint = p;

                    AttrWeight = Manip.DistanceTo(p) + Goal.DistanceTo(p);

                    AttrArea = new Point[180];
                    r        = 0.05 * Math.Pow(AttrWeight / Manip.DistanceTo(Goal), 4);
                    for (int i = 0; i < AttrArea.Length; i++)
                    {
                        AttrArea[i] = new Point
                                      (
                            r * Math.Cos(i * Math.PI / (AttrArea.Length / 2)) + AttrPoint.x,
                            r * Math.Sin(i * Math.PI / (AttrArea.Length / 2)) + AttrPoint.y
                                      );
                    }

                    Attractors.Add(new Attractor(AttrPoint, AttrWeight, AttrArea, r));
                }
            }
            States["Attractors"] = true;

            Generator.Solver = Solver;
            Generator.RRT(new Random(), ref Tree, Goal, Manip, Obstacles, 15000, 0.04);
            //Tree.RectifyWhole();

            Tree.Node       start = Tree.Min(Goal), node_curr = start;
            List <Point>    path    = new List <Point>();
            List <double[]> configs = new List <double[]>();

            for (int i = start.Layer; i >= 0; i--)
            {
                if (i == 0)
                {
                    int a;
                    a = 2;
                }

                if (node_curr.Layer == i)
                {
                    path.Add(node_curr.p);

                    configs.Add(node_curr.q);
                    if (node_curr.Parent != null)
                    {
                        int pointsNum = node_curr.Layer - node_curr.Parent.Layer - 1;
                        if (pointsNum > 0)
                        {
                            Tree.Node[] nodes = Tree.Discretize(node_curr, node_curr.Parent, pointsNum);
                            foreach (var node in nodes)
                            {
                                configs.Add(node.q);
                            }
                        }
                    }

                    node_curr = node_curr.Parent;
                }
            }
            path.Reverse();
            configs.Reverse();
            States["Tree"] = true;

            Path           = path;
            States["Path"] = true;

            Joints = new List <Point[]>();
            for (int i = 0; i < configs.Count; i++)
            {
                if (configs[i] == null)
                {
                    break;
                }

                if (i == 0)
                {
                    int a;
                    a = 0;
                }
                Manip.q = configs[i];
                Joints.Add(Manip.Joints);
            }
            States["Joints"] = true;
        }