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));
}