public Controller(Manipulator manipulator, PathPlanner pathPlanner, InverseKinematicsSolver inverseKinematicsSolver, MotionController motionController)
{
Manipulator = manipulator;
InverseKinematicsSolver = inverseKinematicsSolver;
PathPlanner = pathPlanner;
MotionController = motionController;
}
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 void Remove(Manipulator manipulator)
{
int index = Manipulators.IndexOf(manipulator);
if (Manipulators.Remove(manipulator))
{
manipulator.Dispose();
// remove goal model
MainWindow._goalModels[index].Dispose();
MainWindow._goalModels.RemoveAt(index);
// remove goal model
//MainWindow._treeModels[index].Dispose();
//MainWindow._treeModels.RemoveAt(index);
// remove goal model
//MainWindow._pathModels[index].Dispose();
//MainWindow._pathModels.RemoveAt(index);
// remove goal model
//MainWindow._gaModels[index].Dispose();
//MainWindow._gaModels.RemoveAt(index);
}
}
public static void RecalculateWeights(List <Attractor> attractors, Manipulator manipulator, float goalThreshold)
{
foreach (var attractor in attractors)
{
attractor.Weight = CalculateAttractorWeight(manipulator, attractor.Center);
attractor.Radius = CalculateAttractorRadius(manipulator, attractor.Weight, goalThreshold);
}
}
public static void Execute(Manipulator manip)
{
var AD = Dispatcher.WorkspaceBuffer.AlgBuffer;
/*// generating random tree
* var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
* var planner = new RRT(Obstacles, solver, AD.k, true, AD.d);
* PathPlanner pp = planner;
* var resRRT = pp.Execute(manip, null);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resRRT.Item1;
* manip.States["Path"] = true;
* manip.Configs = resRRT.Item2;*/
/*// generating random tree
* var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
* var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);
* PathPlanner pp = planner;
* var resRRT = pp.Execute(manip, null);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resRRT.Item1;
* manip.States["Path"] = true;
* manip.Configs = resRRT.Item2;*/
var solver = new Jacobian(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);
planner.Start(manip, Vector3.Zero);
/*var resGA = PathPlanner.GeneticAlgorithm(manip, Obstacles, manip.Goal, resRRT.Item2.ToArray(),
* 0.99, manip.Joints.Length, 20, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);*/
/*var input = new (Vector3, float[])[resRRT.Item1.Count];
* for (int i = 0; i < input.Length; i++)
* {
* input[i].Item1 = resRRT.Item1[i];
* input[i].Item2 = resRRT.Item2[i];
* }
*
* var jac = new Jacobian(Obstacles, manip.q.Length, AD.Precision, AD.StepSize, AD.MaxTime);
* var resGA = PathPlanner.GeneticAlgorithmD(manip, Obstacles, manip.Goal, jac,
* input, 0.99, manip.Joints.Length, 4, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);
*
* // acquiring all the Vector3s and configurations along the path
* manip.Path = resGA.Item1;
* manip.States["Path"] = true;
* manip.Configs = resGA.Item2;*/
}
public void Run(Manipulator manipulator, CancellationToken cancellationToken = default)
{
try
{
// start measuring execution time
Timer.Restart();
// turn the controller on
State = ControllerState.Running;
// start motion control if a path has been found
if (manipulator.Path != null)
{
using (var manipulatorCopy = manipulator.DeepCopy())
{
// execute motion control
Path.Node gripperPos = manipulator.Path.Current;
while (gripperPos.Child != null)
{
cancellationToken.ThrowIfCancellationRequested();
var current = gripperPos.Child;
while (current.Child != null) // last point may not be deformed, since it is a goal point
{
for (int j = current.Points.Length - 1; j > 0; j--)
{
if (ObstacleHandler.ContainmentTest(current.Points[j], out Obstacle obstacle))
{
//Deform(manipulatorCopy, obstacle, current, j);
}
}
current = current.Child;
}
//Discretize(manipulatorCopy, gripperPos);
gripperPos = manipulator.Path.Current;
}
}
}
// turn the controller off
State = ControllerState.Idle;
}
catch (OperationCanceledException oce)
{
// indicate that the process has been aborted
State = ControllerState.Aborted;
}
finally
{
// stop measuring execution time
Timer.Stop();
}
}
public static void Plan(Manipulator manip)
{
/*var resGA = PathPlanner.GeneticAlgorithm(manip, Obstacles, manip.Goal, resRRT.Item2.ToArray(),
* 0.99, manip.Joints.Length, 20, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);*/
/*var jac = new Jacobian(Obstacles, manip.q.Length, AD.Precision, AD.StepSize, AD.MaxTime);
* var resGA = PathPlanner.GeneticAlgorithmD(manip, Obstacles, manip.Goal, jac,
* input, 0.99, manip.Joints.Length, 4, 0.95, 0.1, 10000,
* PathPlanner.OptimizationCriterion.CollisionFree,
* PathPlanner.SelectionMode.NormalDistribution,
* PathPlanner.CrossoverMode.WeightedMean,
* t => t * Math.PI / 180);*/
}
public Manipulator(Manipulator source) // TODO: make a m.CreateCopy() method, not a constructor! that's much better
{
Links = Misc.CopyArray(source.Links);
Joints = Array.ConvertAll(source.Joints, x => x.Copy()); //Misc.CopyArray(source.Joints);
Base = source.Base; // TODO: review referencing
DH = Array.ConvertAll(source.DH, x => x.Copy()); //Misc.CopyArray(source.DH);
for (int i = 0; i < DH.Length; i++)
{
DH[i].joint = Joints[i];
}
UpdateTransMatrices();
WorkspaceRadius = source.WorkspaceRadius;
Goal = source.Goal; // TODO: review referencing
States = new Dictionary <string, bool>(source.States);
}
private static float CalculateAttractorRadius(Manipulator manipulator, float weight, float goalThreshold)
{
return(goalThreshold * weight / manipulator.DistanceTo(manipulator.Goal)); /*(float)Math.Pow(weight / manipulator.DistanceTo(manipulator.Goal), 2);*/
}
private static float CalculateAttractorWeight(Manipulator manipulator, Vector3 point)
{
return(manipulator.DistanceTo(point) + manipulator.Goal.DistanceTo(point));
}
public static void Initialize()
{
var LD = Dispatcher.WorkspaceBuffer.LinkBuffer;
var JD = Dispatcher.WorkspaceBuffer.JointBuffer;
var OD = Dispatcher.WorkspaceBuffer.ObstBuffer;
// manipulators
Manipulators = new Manipulator[1];
Manipulators[0] = new Manipulator(LD, JD, new TupleDH[]
{
new TupleDH(0, JD[0].Length + LD[0].Length, 90 * (float)Math.PI / 180, 0),
new TupleDH(-90 * (float)Math.PI / 180, 0, 0, JD[1].Length + LD[1].Length),
new TupleDH(0, 0, 0, JD[2].Length + LD[2].Length)
});
Manipulators[0].Goal = new Vector3(0, 0.5f, 2.5f);
// obstacles
Obstacles = new Obstacle[OD.Length];
for (int i = 0; i < OD.Length; i++)
{
Obstacles[i] = new Obstacle(Primitives.Sphere(OD[i].r, new Vector3(OD[i].c.X, OD[i].c.Y, OD[i].c.Z), OD[i].Vector3s_num, new Random()), ColliderShape.Sphere);
}
var manip = Manipulators[0];
manip.GoodAttractors = new List <Attractor>();
manip.BadAttractors = new List <Attractor>();
Random rng = new Random();
float work_radius = manip.WorkspaceRadius, x, y_pos, y, z_pos, z;
// adding main attractor
Vector3 AttrVector3 = manip.Goal;
float AttrWeight = manip.DistanceTo(manip.Goal);
float r = Dispatcher.WorkspaceBuffer.AlgBuffer.d * (float)Math.Pow(AttrWeight / manip.DistanceTo(manip.Goal), 4);
Vector3[] AttrArea = Primitives.Sphere(r, AttrVector3, 64, new Random());
manip.GoodAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
manip.States["Goal"] = true;
var AD = Dispatcher.WorkspaceBuffer.AlgBuffer;
// adding ancillary attractors
while (manip.GoodAttractors.Count < AD.AttrNum)
{
// generating attractor Vector3
x = -work_radius + (float)rng.NextDouble() * 2 * work_radius;
y_pos = (float)Math.Sqrt(work_radius * work_radius - x * x);
y = -y_pos + (float)rng.NextDouble() * 2 * y_pos;
z_pos = (float)Math.Sqrt(work_radius * work_radius - x * x - y * y);
z = -z_pos + (float)rng.NextDouble() * 2 * z_pos;
Vector3 p = new Vector3(x, y, z) + manip.Base;
// checking whether the attractor is inside any obstacle or not
bool collision = false;
foreach (var obst in Obstacles)
{
if (obst.Contains(p))
{
collision = true;
break;
}
}
// adding attractor to the list
AttrVector3 = p;
AttrWeight = manip.DistanceTo(p) + manip.Goal.DistanceTo(p);
r = AD.d * (float)Math.Pow(AttrWeight / manip.DistanceTo(manip.Goal), 4);
AttrArea = Primitives.Sphere(r, AttrVector3, 64, new Random());
if (!collision)
{
manip.GoodAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
}
else
{
manip.BadAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
}
}
manip.States["Attractors"] = true;
}
public static Manipulator CreateDefaultManipulator()
{
// set links' parameters
var links = new LinkData[_defaultLinksNumber];
links.Fill(new LinkData
{
Length = _defaultLinksLength
});
// define model and collider for each link
for (int i = 0; i < links.Length; i++)
{
links[i].Model = _defaultLinkModel.DeepCopy();
links[i].Collider = PhysicsHandler.CreateKinematicCollider(new CylinderShape(0.15f, 0.5f, 0.15f));
}
// set joints' parameters
var joints = new JointData[_defaultLinksNumber + 1];
joints.Fill(_defaultJoint);
joints[joints.Length - 1] = _defaultGripper;
// define model and collider for each joint
for (int i = 0; i < joints.Length - 1; i++)
{
joints[i].Model = _defaultJointModel.DeepCopy();
joints[i].Collider = PhysicsHandler.CreateKinematicCollider(new SphereShape(0.2f));
}
// TODO: gripper collider is not affected by the initial transform; fix!
joints[_defaultLinksNumber].Model = _defaultGripperModel.DeepCopy();
joints[_defaultLinksNumber].Collider = PhysicsHandler.CreateKinematicCollider(new SphereShape(0.1f));
// set joints' axes
var jointAxes = new Vector3[_defaultLinksNumber + 1];
jointAxes[0] = jointAxes[jointAxes.Length - 1] = Vector3.UnitY;
for (int i = 1; i < _defaultLinksNumber; i++)
{
jointAxes[i] = /*Vector3.UnitX;*/ i % 2 == 0 ? Vector3.UnitZ : Vector3.UnitX;
}
// set joints' positions
var jointPositions = new Vector3[_defaultLinksNumber + 1];
jointPositions[0] = Vector3.Zero;
for (int i = 1; i < _defaultLinksNumber + 1; i++)
{
jointPositions[i] = jointPositions[i - 1] + ((joints[i - 1].Length + joints[i].Length) / 2 + links[i - 1].Length) * Vector3.UnitY;
}
// create a default manipulator
var manipulator = new Manipulator(new ManipData
{
N = _defaultLinksNumber,
Links = links,
Joints = joints,
JointAxes = jointAxes,
JointPositions = jointPositions,
Goal = _defaultGoal,
ShowTree = true
});
var solver = DampedLeastSquares.Default();
var planner = GeneticAlgorithm.Default(); /*ARRT.Default(manipulator);*/
var controller = MotionController.Default();
manipulator.Controller = new Controller(manipulator, planner, solver, controller);
// add manipulator to the list
Add(manipulator);
return(manipulator);
}
public static void Add(Manipulator manipulator)
{
Manipulators.Add(manipulator);
}
