public Controller(Manipulator manipulator, PathPlanner pathPlanner, InverseKinematicsSolver inverseKinematicsSolver, MotionController motionController)
{
Manipulator = manipulator;
InverseKinematicsSolver = inverseKinematicsSolver;
PathPlanner = pathPlanner;
MotionController = motionController;
}
public void Run()
{
_cancellationTokenSource = new CancellationTokenSource();
var cancellationToken = _cancellationTokenSource.Token;
PlanningTask = Task.Run(() =>
{
var res = PathPlanner.Run(Manipulator, Manipulator.Goal, InverseKinematicsSolver, cancellationToken);
Manipulator.Path = res.Path;
}, cancellationToken);
ControlTask = PlanningTask.ContinueWith(task =>
{
MotionController.Run(Manipulator, cancellationToken);
}, cancellationToken);
}
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);
}
