public static List <Attractor> Create(Manipulator manipulator, int count, float goalThreshold, bool discardOutliers = true)
{
var attractors = new List <Attractor>();
//// adding goal attractor
//Vector3 attrPoint = manipulator.Goal;
//attractors.Add(new Attractor(attrPoint));
// adding ancillary attractors
while (attractors.Count < count)
{
// generating attractor point
Vector3 point = RandomThreadStatic.NextPointSphere(manipulator.WorkspaceRadius) + manipulator.Base;
// checking whether the attractor is inside any obstacle or not if requested
if (!(discardOutliers && ObstacleHandler.ContainmentTest(point, out _))) // TODO: consider creating a list of bad attractors; they may serve as repulsion points
{
// adding attractor to the list
Vector3 attrPoint = point;
attractors.Add(new Attractor(attrPoint));
}
}
return(attractors);
}
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
});
}