public Controller(Manipulator manipulator, PathPlanner pathPlanner, InverseKinematicsSolver inverseKinematicsSolver, MotionController motionController)
{
Manipulator = manipulator;
InverseKinematicsSolver = inverseKinematicsSolver;
PathPlanner = pathPlanner;
MotionController = motionController;
}
public void Trim(Manipulator manipulator, InverseKinematicsSolver solver)
{
switch (Mode)
{
case TreeBehaviour.Cyclic:
var source = new Queue <Node>();
source.Enqueue(Root);
while (source.Count != 0)
{
var childs = new List <Node>(source.Dequeue().Childs);
foreach (var child in childs)
{
manipulator.q = child.q;
if (manipulator.CollisionTest().Contains(true))
{
RemoveNode(child);
}
else
{
source.Enqueue(child);
}
}
}
break;
case TreeBehaviour.Recursive:
foreach (var child in Root.Childs)
{
TrimRecursive(manipulator, solver, child);
}
break;
}
}
public void TrimRecursive(Manipulator manipulator, InverseKinematicsSolver solver, Node node)
{
manipulator.q = node.q;
if (manipulator.CollisionTest().Contains(true))
{
RemoveNode(node);
return;
}
var childs = new List <Node>(node.Childs);
foreach (var child in childs)
{
TrimRecursive(manipulator, solver, child);
}
}
public PathPlanningResult Run(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken = default)
{
var res = new PathPlanningResult();
try
{
// restart measuring execution time
Timer.Restart();
// turn the controller on
State = ControllerState.Running;
// execute path planning
using (var manipulatorCopy = manipulator.DeepCopy())
{
res = RunAbstract(manipulatorCopy, goal, solver, cancellationToken);
}
// turn the controller off
State = ControllerState.Idle;
}
catch (OperationCanceledException oce)
{
// TODO: provide some log info
Reset();
// indicate that the process has been aborted
State = ControllerState.Aborted;
}
finally
{
// stop measuring execution time
Timer.Stop();
}
return(res);
}
protected abstract PathPlanningResult RunAbstract(Manipulator manipulator, Vector3 goal, InverseKinematicsSolver solver, CancellationToken cancellationToken);
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
});
}
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
});
}
