public static void AddDefault(ObstacleShape shape)
{
switch (shape)
{
case ObstacleShape.Box:
Add(new Obstacle(Primitives.Cube(0.5f, 0.5f, 0.5f, _defaultMaterial),
PhysicsHandler.CreateKinematicCollider(new BoxShape(0.5f, 0.5f, 0.5f))));
break;
case ObstacleShape.Sphere:
Add(new Obstacle(Primitives.Sphere(0.5f, 50, 50, _defaultMaterial),
PhysicsHandler.CreateKinematicCollider(new SphereShape(0.5f))));
break;
case ObstacleShape.Cylinder:
Add(new Obstacle(new Model(new Mesh[]
{
Primitives.Cylinder(0.25f, 1f, 1f, 50, _defaultMaterial)
}), PhysicsHandler.CreateKinematicCollider(new CylinderShape(0.25f, 1f, 0.25f))));
break;
case ObstacleShape.Cone:
Add(new Obstacle(new Model(new Mesh[]
{
Primitives.Cone(0.5f, 2, 50, _defaultMaterial)
}), PhysicsHandler.CreateKinematicCollider(new ConeShape(0.5f, 2)))); // TODO: cone's rigid body center is not the circle center, but the center of mass; fix
break;
default:
throw new ArgumentException("The given obstacle shape is not implemented yet.", "shape");
}
}
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);
}