private float Rate(Path sample) { float score = 0; // apply goal convergence criterion _manipulator.q = sample.Last.q; var distance = _manipulator.DistanceTo(_goal); if (distance > _threshold) { score += distance - _threshold; } // extract parameters' values from chromosome foreach (var node in sample.Nodes) { _manipulator.q = node.q; Vector3 currPos = node.Points[node.Points.Length - 1]; // apply fitness functions to the given chromosome's point int criteriaCount = 0; float pointWeight = 0; if (_optimizationCriteria.HasFlag(OptimizationCriteria.CollisionFree)) { if (_collisionCheck) { if (ObstacleHandler.ContainmentTest(currPos, out _)) { pointWeight += 1; } if (_manipulator.CollisionTest().Contains(true)) { pointWeight += 1; } } criteriaCount++; } if (_optimizationCriteria.HasFlag(OptimizationCriteria.PathLength)) { } // take median of all criteria weights score += pointWeight / criteriaCount; } return(score); }
public static void FitnessFunction() { Manipulator Contestant = new Manipulator(Agent); for (int i = 0; i < Chs.Length; i++) { //extracting parameters' values from chromosome double[] dq = Chs[i].GetParamAll(Decode); Contestant.q = Agent.q.Zip(dq, (t, s) => { return(t + s); }).ToArray(); //applying fitness functions to the given chromosome Fit[i] = Contestant.DistanceTo(Goal); } }
protected override PathPlanningResult RunAbstract(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken) { if (manipulator.DistanceTo(goal) < _threshold) { // the goal is already reached return new PathPlanningResult { Iterations = 0, Path = null } } ; // create new tree Tree = new Tree(new Tree.Node(null, manipulator.GripperPos, manipulator.q), _maxIterations + 1); // define local attractors as the goal attractor and copies of static attractors var attractors = new List <Attractor>() { new Attractor(goal) }; attractors.AddRange(_attractors); // recalculate attractors' weights Attractor.RecalculateWeights(attractors, manipulator, _threshold); // sort attractors attractors = attractors.OrderBy(a => a.Weight).ToList(); // create necessary locals Attractor attractorFirst = attractors.First(); Attractor attractorLast = attractors.Last(); // TODO: last attractor has too big radius (~5 units for 0.04 step); fix! float mu = attractorFirst.Weight; float sigma = (attractorLast.Weight - attractorFirst.Weight) / 2.0f; List <float> weights = new List <float>(); Iterations = 0; while (Iterations < _maxIterations) { cancellationToken.ThrowIfCancellationRequested(); Iterations++; // trim tree if (_enableTrimming && Iterations % _trimPeriod == 0) { Tree.Trim(manipulator, solver); } // generate normally distributed weight float num = RandomThreadStatic.NextGaussian(mu, sigma); weights.Add(num); // get the index of the most relevant attractor int index = attractors.IndexOfNearest(num, a => a.Weight); // get point of the obtained attractor Vector3 sample = attractors[index].Center; // find the closest node to that attractor Tree.Node nodeClosest = Tree.Closest(sample); // get new tree node point Vector3 point = nodeClosest.Point + Vector3.Normalize(sample - nodeClosest.Point) * _step; if (!(_collisionCheck && ObstacleHandler.ContainmentTest(point, out _))) { // solve inverse kinematics for the new node manipulator.q = nodeClosest.q; var ikRes = solver.Execute(manipulator, point); if (ikRes.Converged && !(_collisionCheck && manipulator.CollisionTest().Contains(true))) // TODO: is convergence check really needed? { manipulator.q = ikRes.Configuration; // add node to the tree Tree.Node node = new Tree.Node(nodeClosest, manipulator.GripperPos, manipulator.q); Tree.AddNode(node); // check exit condition if (point.DistanceTo(attractors[index].Center) < attractors[index].Radius) { if (index == 0) { // stop in case the main attractor has been hit continue;//break; } else { // remove attractor if it has been hit attractors.RemoveAt(index); } } } } } // retrieve resultant path along with respective configurations return(new PathPlanningResult { Iterations = Iterations, Path = Tree.GetPath(manipulator, Tree.Closest(goal)) // TODO: refactor! tree should be written to temp variable in path planner, not permanent in manipulator }); } }
protected override PathPlanningResult RunAbstract(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken) { if (manipulator.DistanceTo(goal) < _threshold) { // the goal is already reached return new PathPlanningResult { Iterations = 0, Path = null } } ; // create new tree Tree = new Tree(new Tree.Node(null, manipulator.GripperPos, manipulator.q), _maxIterations + 1); Iterations = 0; while (Iterations < _maxIterations) { cancellationToken.ThrowIfCancellationRequested(); Iterations++; // trim tree if (_enableTrimming && Iterations % _trimPeriod == 0) { Tree.Trim(manipulator, solver); } // get sample point Vector3 sample; if (Iterations % _goalBiasPeriod == 0) { // use goal bias sample = goal; } else { // generate sample sample = RandomThreadStatic.NextPointSphere(manipulator.WorkspaceRadius) + manipulator.Base; } // find the closest node to the generated sample point Tree.Node nodeClosest = Tree.Closest(sample); // get new tree node point Vector3 point = nodeClosest.Point + Vector3.Normalize(sample - nodeClosest.Point) * _step; if (!(_collisionCheck && ObstacleHandler.ContainmentTest(point, out _))) { // solve inverse kinematics for the new node to obtain the agent configuration manipulator.q = nodeClosest.q; var ikRes = solver.Execute(manipulator, point); if (ikRes.Converged && !(_collisionCheck && manipulator.CollisionTest().Contains(true))) { manipulator.q = ikRes.Configuration; // add new node to the tree Tree.Node node = new Tree.Node(nodeClosest, manipulator.GripperPos, manipulator.q); Tree.AddNode(node); // check exit condition if (manipulator.DistanceTo(goal) < _threshold) { break; } } } } // retrieve resultant path along with respective configurations return(new PathPlanningResult { Iterations = Iterations, Path = Tree.GetPath(manipulator, Tree.Closest(goal)) // TODO: refactor! tree should be written to temp variable in path planner, not permanent in manipulator }); }
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; }
protected override PathPlanningResult RunAbstract(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken) { if (manipulator.DistanceTo(goal) < _threshold) { // the goal is already reached return new PathPlanningResult { Iterations = 0, Path = null } } ; _manipulator = manipulator; _initialConfiguration = manipulator.q; _solver = solver; _goal = goal; var generation = new List <Chromosome>(); // get initial solution var bezierInitial = ConstructBezier(_manipulator.GripperPos, goal, _bezierControlPointsCount); var pathInitial = ConstructPath(bezierInitial, _bezierStep); var chromosomeInitial = new Chromosome(bezierInitial, pathInitial, Rate(pathInitial)); generation.Add(chromosomeInitial); Dominant = chromosomeInitial; Changed = true; // evolve generations Console.SetCursorPosition(0, 10); Console.WriteLine("Starting genetic algorithm..."); Iterations = 0; while (generation[0].Weight > 0 && Iterations++ < _maxIterations) { cancellationToken.ThrowIfCancellationRequested(); // get new generation generation = Evolve(generation, _offspringSize, _survivalSize); Console.SetCursorPosition(0, 11); Console.WriteLine($"Generations passed: {Iterations}"); Console.WriteLine($"Best fit: {generation[0].Weight}"); if (!Locked) { Dominant = generation[0]; // TODO: it seems like previous dominant is not disposed properly; fix!! Changed = true; } } Console.SetCursorPosition(0, 15); Console.WriteLine("Found path's Bezier curve:"); foreach (var point in generation[0].BezierCurve.Points) { Console.WriteLine(point); } var path = generation[0].Path; path.SetModel(); return(new PathPlanningResult { Iterations = Iterations - 1, Path = path }); }
public Tuple <bool, double, double[], bool[]> Execute(Point goal) { Goal = goal; /*Chs = new Chromosome<double>[GenSize]; * Fit = new double[GenSize]; * * for (int i = 0; i < GenSize; i++) * { * Chs[i] = new Chromosome<double>(ParamNum); * for (int j = 0; j < ParamNum; j++) * Chs[i].Genes[j] = Agent.StepRanges[j, 0] + (Agent.StepRanges[j, 1] - Agent.StepRanges[j, 0]) * Rng.NextDouble(); * } * * Chromosome<double> dominant = null; * double Min = double.PositiveInfinity; * bool Converged = false; * Stopwatch sw = new Stopwatch(); * sw.Start(); * while (Time++ < MaxTime) * { * //fitting * FitnessFunction(); * Array.ConvertAll(Fit, (t) => { return Math.Abs(t); }); * * //qualification * Min = Fit.Min(); * if (Min < Precision) * { * dominant = Chs[Array.IndexOf(Fit, Min)]; * Converged = true; * break; * } * * //selection * RouletteWheelSelection(Chs, Fit); * * //mutation * Mutation(Chs, Pm, t => t + -0.1 + 0.2 * Rng.NextDouble()); * * //crossover * Crossover(Chs); * } * sw.Stop(); * TotalRealTime += (int)sw.ElapsedTicks / 10; * Console.WriteLine("IKP Time: {0}; Real time: {1}", Time, sw.ElapsedTicks / 10); * * //if dominant chromosome wasn't found, consider last result the best result * if (dominant == null) * { * dominant = Chs[Array.IndexOf(Fit, Min)]; * } * * //decoding chromosome * double[] dq = dominant.GetParamAll(Decode); * * //detect all collisions * Manipulator AgentNext = new Manipulator(Agent) * { * q = Agent.q.Zip(dq, (t, s) => { return t + s; }).ToArray() * }; * bool[] Collisions = DetectCollisions(AgentNext); * * Time = 0; * return new Tuple<bool, double, double[], bool[]>(Converged, Min, dq, Collisions);*/ double range = 0; for (int i = 0; i < ParamNum; i++) { range = -120 * Math.PI / 180 - Agent.q[i]; Agent.StepRanges[i, 0] = range / 5; // <= -1 ? -1 : range; range = 120 * Math.PI / 180 - Agent.q[i]; Agent.StepRanges[i, 1] = range / 5; // >= 1 ? 1 : range; } Manipulator Contestant = new Manipulator(Agent); double[] q_init = new double[ParamNum]; Array.Copy(Agent.q, q_init, ParamNum); double dist = Agent.DistanceTo(Goal), init_dist = dist, coeff = 1; double Min = double.PositiveInfinity; bool Converged = false; Stopwatch sw = new Stopwatch(); sw.Start(); while (Time++ < 1000) { Chromosome <double> ch = new Chromosome <double>(ParamNum); for (int i = 0; i < ParamNum; i++) { ch.Genes[i] = Agent.StepRanges[i, 0] + (Agent.StepRanges[i, 1] - Agent.StepRanges[i, 0]) * Rng.NextDouble(); ch.Genes[i] *= coeff; } double[] dq = ch.GetParamAll(Decode); Contestant.q = Agent.q.Zip(dq, (t, s) => { return(t + s); }).ToArray(); double dist_new = Contestant.DistanceTo(Goal); if (dist_new < dist) { Array.Copy(Contestant.q, Agent.q, ParamNum); Min = dist = dist_new; coeff = dist / init_dist; } if (dist < 0.002) { Converged = true; break; } } sw.Stop(); Console.WriteLine("IKP Time: {0}; Real time: {1}", Time, sw.ElapsedTicks / 10); bool[] Collisions = DetectCollisions(Agent); Time = 0; return(new Tuple <bool, double, double[], bool[]>(Converged, Min, Agent.q.Zip(q_init, (t, s) => { return t - s; }).ToArray(), Collisions)); }