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); } }
public static void RRT(Random rng, ref Tree holder, Point goal, Manipulator manip, Obstacle[] obstacles, int k, double d) { Agent = new Manipulator(manip); Obstacles = obstacles; holder = new Tree(new Tree.Node(null, Agent.GripperPos, Agent.q)); Attractor[] arr = new Attractor[Manager.Attractors.Count]; Manager.Attractors.CopyTo(arr); var AttractorsLoc = arr.ToList(); AttractorsLoc.Sort((t, s) => { return(t.Weight <= s.Weight ? (t.Weight < s.Weight ? -1 : 0) : 1); }); for (int i = 0; i < k; i++) { /*if ((i + 1) % 5000 == 0) * { * tree.RectifyWhole(); * }*/ double radius; double x; double y_plus, y_minus; double y; double num = Misc.BoxMullerTransform(rng, AttractorsLoc[0].Weight, AttractorsLoc[AttractorsLoc.Count / 2 - 1].Weight / 3); Attractor attr = AttractorsLoc.Find((t) => { return(t.Weight > num); }); int index = 0; if (attr == null) { index = rng.Next(AttractorsLoc.Count / 2 - 1, AttractorsLoc.Count); } else { index = AttractorsLoc.IndexOf(attr); } //int index = rng.Next(0, Manager.Attractors.Count); radius = AttractorsLoc[index].Radius; x = -radius + rng.NextDouble() * 2 * radius; y_plus = Math.Sqrt(radius * radius - x * x); y_minus = -y_plus; y = AttractorsLoc[index].Center.y + (rng.NextDouble() * 2 * (y_plus - y_minus) - (y_plus - y_minus)) / 2; Point p = new Point(x + AttractorsLoc[index].Center.x, y); Tree.Node min_node = holder.Min(p); Vector v = new Vector(min_node.p, p); Point p_n = min_node.p + v.Normalized * d; bool collision = false; foreach (var obst in Obstacles) { if (obst.Contains(p_n)) { collision = true; break; } } if (!collision) { Array.Copy(min_node.q, Algorithm.Agent.q, min_node.q.Length); var res = Solver.Execute(p_n); if (res.Item1 && !res.Item4.Contains(true)) { if (AttractorsLoc[index].InliersCount < 5) { Tree.Node node = new Tree.Node(min_node, p_n, Algorithm.Agent.q); holder.AddNode(node); if (p_n.DistanceTo(AttractorsLoc[index].Center) < AttractorsLoc[index].Radius) { AttractorsLoc[index].InliersCount++; } Manager.Buffer.Add(node); } else { AttractorsLoc.RemoveAt(index); } } } if (AttractorsLoc[0].InliersCount != 0) { break; } } }