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);
}
public static System.Numerics.Vector3[] SpherePointCloud(float radius, System.Numerics.Vector3 center, int pointsNum)
{
var data = new System.Numerics.Vector3[pointsNum];
double x, yPos, y, zPos, z;
for (int i = 0; i < pointsNum; i++)
{
x = radius * (2 * RandomThreadStatic.NextDouble() - 1);
yPos = Math.Sqrt(radius * radius - x * x);
y = yPos * (2 * RandomThreadStatic.NextDouble() - 1);
zPos = Math.Sqrt(yPos * yPos - y * y);
z = RandomThreadStatic.Next(0, 2) == 0 ? -zPos : zPos;
data[i] = new System.Numerics.Vector3((float)x, (float)y, (float)z) + center;
}
return(data);
}
private BezierCurve Repro(BezierCurve bezierCurve)
{
int pointIndex = RandomThreadStatic.Next(1, _bezierControlPointsCount + 1);
Vector3 direction = RandomThreadStatic.NextPointCube(1);
var bezierCurveRepro = new BezierCurve(bezierCurve.Points);
var bezierDirectionRepro = Vector3.Normalize(direction) * 1f * (float)RandomThreadStatic.NextDouble();
bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro;
if (_manipulator.ApproxWithinReach(bezierCurveRepro.Points[pointIndex] + bezierDirectionRepro))
{
bezierCurveRepro.Points[pointIndex] += bezierDirectionRepro;
}
else
{
bezierCurveRepro.Points[pointIndex] -= bezierDirectionRepro;
}
return(bezierCurveRepro);
}
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
});
}
}
public override InverseKinematicsResult Execute(Manipulator manipulator, Vector3 goal, int joint = -1) // TODO: refactor
{
// use gripper if default joint
if (joint == -1)
{
joint = manipulator.Joints.Length - 1;
}
VectorFloat configuration = manipulator.q, dq = VectorFloat.Build.Dense(manipulator.Joints.Length);
var errorPos = goal - manipulator.Joints[joint].Position;
var error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 });
float distance = errorPos.Length();
float maxStepSize = _maxStepSize, scale = 1;
int iterations = 0;
while (distance > _threshold && iterations < _maxIterations)
{
errorMod.Add(error.L2Norm());
configs.Add(VectorFloat.Build.DenseOfVector(configuration));
float range, stepNeg, stepPos;
for (int i = 0; i <= joint; i++)
{
// checking coordinate constraints
range = manipulator.Joints[i].CoordinateRange.X - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD;
stepNeg = range <= -maxStepSize ? -maxStepSize : range;
range = manipulator.Joints[i].CoordinateRange.Y - configuration[i] * MathUtil.SIMD_DEGS_PER_RAD;
stepPos = range >= maxStepSize ? maxStepSize : range;
// generating random coordinate offset
dq[i] = (stepNeg + (float)RandomThreadStatic.NextDouble() * (stepPos - stepNeg)) * MathUtil.SIMD_RADS_PER_DEG;
}
// get the new configuration
var configurationNew = VectorFloat.Build.DenseOfVector(configuration);
configurationNew.AddSubVector(dq);
var fkRes = manipulator.ForwardKinematics(configurationNew);
float distanceNew = fkRes.JointPositions[joint].DistanceTo(goal);
if (distanceNew < distance)
{
// update configuration, distance and scale
configuration = configurationNew;
errorPos = goal - fkRes.JointPositions[joint];
error = VectorFloat.Build.Dense(new float[] { errorPos.X, errorPos.Y, errorPos.Z, 0, 0, 0 });
scale *= distanceNew / distance;
distance = distanceNew;
maxStepSize = _maxStepSize * scale;
}
iterations++;
}
return(new InverseKinematicsResult
{
Converged = iterations < _maxIterations && !JointLimitsExceeded(manipulator, configuration),
Iterations = iterations,
Configuration = configuration,
Error = error
});
}
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
});
}
