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);
}
private void ManipulatorProperties(Manipulator manipulator)
{
ImGui.Checkbox($"Show collider", ref manipulator.ShowCollider);
ImGui.InputFloat3("Goal", ref manipulator.Goal);
ImGui.Separator();
if (ImGui.BeginTabBar("ManipulatorTabs"))
{
if (ImGui.BeginTabItem("Solver"))
{
int currType = (int)manipulator.Controller.InverseKinematicsSolver.Type; // TODO: refactor?
int newType = currType;
ImGui.Combo("Type", ref newType, InverseKinematicsSolver.Types, InverseKinematicsSolver.Types.Length);
// change inverse kinematics solver if queried
if (newType != currType)
{
switch ((InverseKinematicsSolverType)newType)
{
case InverseKinematicsSolverType.JacobianTranspose:
manipulator.Controller.InverseKinematicsSolver = JacobianTranspose.Default();
break;
case InverseKinematicsSolverType.JacobianPseudoinverse:
manipulator.Controller.InverseKinematicsSolver = JacobianPseudoinverse.Default();
break;
case InverseKinematicsSolverType.DampedLeastSquares:
manipulator.Controller.InverseKinematicsSolver = DampedLeastSquares.Default();
break;
case InverseKinematicsSolverType.HillClimbing:
manipulator.Controller.InverseKinematicsSolver = HillClimbing.Default();
break;
}
}
ImGui.Separator();
// inverse kinematics solver properties
ImGui.InputFloat("Threshold", ref manipulator.Controller.InverseKinematicsSolver.Threshold);
ImGui.InputInt("Max iterations", ref manipulator.Controller.InverseKinematicsSolver.MaxIterations);
if (manipulator.Controller.InverseKinematicsSolver is JacobianTranspose jacobianTranspose)
{
ImGui.InputFloat("Base damping coefficient", ref jacobianTranspose.Damping);
}
else if (manipulator.Controller.InverseKinematicsSolver is JacobianPseudoinverse jacobianInverse)
{
// TODO: input something here?
}
else if (manipulator.Controller.InverseKinematicsSolver is DampedLeastSquares dampedLeastSquares)
{
ImGui.InputFloat("Damping coefficient", ref dampedLeastSquares.Damping);
}
else if (manipulator.Controller.InverseKinematicsSolver is HillClimbing hillClimbing)
{
ImGui.InputFloat("Max step size", ref hillClimbing.MaxStepSize);
}
ImGui.EndTabItem();
}
if (ImGui.BeginTabItem("Planner"))
{
int currType = (int)manipulator.Controller.PathPlanner.Type;
int newType = currType;
ImGui.Combo("Type", ref newType, PathPlanner.Types, PathPlanner.Types.Length);
// change path planner if queried
if (newType != currType)
{
switch ((PathPlannerType)newType)
{
case PathPlannerType.RRT:
manipulator.Controller.PathPlanner = RRT.Default();
break;
case PathPlannerType.ARRT:
manipulator.Controller.PathPlanner = ARRT.Default(manipulator);
break;
case PathPlannerType.GeneticAlgorithm:
manipulator.Controller.PathPlanner = GeneticAlgorithm.Default();
break;
}
}
ImGui.Separator();
// path planner properties
ImGui.Checkbox("Collision check", ref manipulator.Controller.PathPlanner.CollisionCheck);
ImGui.InputInt("Max iterations", ref manipulator.Controller.PathPlanner.MaxIterations);
ImGui.InputFloat("Threshold", ref manipulator.Controller.PathPlanner.Threshold);
if (manipulator.Controller.PathPlanner is RRT rrt)
{
ImGui.Checkbox($"Show tree", ref rrt.ShowTree);
ImGui.InputFloat("Step", ref rrt.Step);
ImGui.Checkbox("Enable trimming", ref rrt.EnableTrimming);
ImGui.InputInt("Trim period", ref rrt.TrimPeriod);
if (rrt is ARRT arrt)
{
ImGui.InputInt("Attractors count", ref arrt.AttractorsCount);
}
else
{
ImGui.InputInt("Goal bias period", ref rrt.GoalBiasPeriod);
}
}
else if (manipulator.Controller.PathPlanner is GeneticAlgorithm geneticAlgorithm)
{
// TODO: add genetic algorithm related properties
ImGui.InputInt("Offspring size", ref geneticAlgorithm.OffspringSize);
ImGui.InputInt("Survival size", ref geneticAlgorithm.SurvivalSize);
ImGui.InputInt("Bezier control points", ref geneticAlgorithm.BezierControlPointsCount);
ImGui.InputFloat("Bezier step", ref geneticAlgorithm.BezierStep);
}
ImGui.EndTabItem();
}
if (ImGui.BeginTabItem("Controller"))
{
// TODO: add motion control related properties
ImGui.EndTabItem();
}
ImGui.EndTabBar();
}
}