private float Rate(Path sample)
{
float score = 0;
// apply goal convergence criterion
_manipulator.q = sample.Last.q;
var distance = _manipulator.DistanceTo(_goal);
if (distance > _threshold)
{
score += distance - _threshold;
}
// extract parameters' values from chromosome
foreach (var node in sample.Nodes)
{
_manipulator.q = node.q;
Vector3 currPos = node.Points[node.Points.Length - 1];
// apply fitness functions to the given chromosome's point
int criteriaCount = 0;
float pointWeight = 0;
if (_optimizationCriteria.HasFlag(OptimizationCriteria.CollisionFree))
{
if (_collisionCheck)
{
if (ObstacleHandler.ContainmentTest(currPos, out _))
{
pointWeight += 1;
}
if (_manipulator.CollisionTest().Contains(true))
{
pointWeight += 1;
}
}
criteriaCount++;
}
if (_optimizationCriteria.HasFlag(OptimizationCriteria.PathLength))
{
}
// take median of all criteria weights
score += pointWeight / criteriaCount;
}
return(score);
}
public void InitHandlers()
{
CoOrdinate currentRoom = new CoOrdinate(3, 3);
RobotHardware.IHardwareRobot cleaningRobot = new RobotHardware.Hardware(currentRoom.X, currentRoom.Y);
IRobotVisitMonitor robotVisitMonitor = RobotVisitMonitorFactory.CreateRobotVisitMonitor(
RobotVisitMonitorType.RobotVisitMonitorWithConsoleOutput, cleaningRobot);
Room room = new SimpleRoom();
_algorithmEssentials = new AlgorithmEssentials(room, cleaningRobot, robotVisitMonitor);
_firstObstacleHandler = new ObstacleHandler(_algorithmEssentials);
_obstacleHandlerWithCellRevisit = new ObstacleHandler(_algorithmEssentials, true);
}
public void ObstacleHandlerWithoutCellRevisit()
{
//Move 3 cells forward to reach the corner
RobotUtility.MoveForward(_algorithmEssentials);
RobotUtility.MoveForward(_algorithmEssentials);
RobotUtility.MoveForward(_algorithmEssentials);
// now ask the obstacle handler to handle it
Assert.True(_firstObstacleHandler.HandleMovement());
//Move 2 cells forward to reach the corner
RobotUtility.MoveForward(_algorithmEssentials);
RobotUtility.MoveForward(_algorithmEssentials);
// now ask the obstacle handler to handle it
Assert.True(_firstObstacleHandler.HandleMovement());
//Move 2 cells forward to reach the corner
RobotUtility.MoveForward(_algorithmEssentials);
RobotUtility.MoveForward(_algorithmEssentials);
// now ask the obstacle handler to handle it
Assert.True(_firstObstacleHandler.HandleMovement());
//Move 1 cell forward to reach the visited cell
RobotUtility.MoveForward(_algorithmEssentials);
// now ask the obstacle handler to handle it
Assert.True(_firstObstacleHandler.HandleMovement());
//Move 1 cell forward to reach the center cell
RobotUtility.MoveForward(_algorithmEssentials);
_secondObstacleHandler = new ObstacleHandler(_algorithmEssentials);
_thirdObstacleHandler = new ObstacleHandler(_algorithmEssentials);
_firstObstacleHandler.SetNextMovementHandler(_secondObstacleHandler);
_secondObstacleHandler.SetNextMovementHandler(_thirdObstacleHandler);
//all the three obstacle handlers will fail to handle the case
Assert.False(_firstObstacleHandler.HandleMovement());
}
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
});
}
}
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
});
}
public void Init(ObstacleDefiner _obstacleDefiner)
{
ObstacleHandler.Instance = this;
obstacleDefiner = _obstacleDefiner;
}
private void RenderObstaclesWindow()
{
// obstacles window
if (ImGui.Begin("Obstacles",
ImGuiWindowFlags.NoCollapse |
ImGuiWindowFlags.NoMove |
ImGuiWindowFlags.NoResize |
ImGuiWindowFlags.HorizontalScrollbar))
{
ImGui.SetWindowPos(new System.Numerics.Vector2(0, (int)(0.5 * (Window.Size.Y - 19) + 19)));
ImGui.SetWindowSize(new System.Numerics.Vector2((int)(0.25 * Window.Size.X - 2), (int)(0.5 * (Window.Size.Y - 19))));
if (ImGui.Button("Create"))
{
ImGui.OpenPopup("ObstacleCreate");
}
ImGui.SameLine();
if (ImGui.Button("Remove"))
{
if (InputHandler.CurrentSelectedObject is Obstacle obstacle)
{
ObstacleHandler.Remove(obstacle);
InputHandler.ClearSelection();
}
}
if (ImGui.BeginPopup("ObstacleCreate"))
{
foreach (var shape in ObstacleHandler.Shapes)
{
if (ImGui.Selectable(shape))
{
var shapeValue = (ObstacleShape)Enum.Parse(typeof(ObstacleShape), shape);
ObstacleHandler.AddDefault(shapeValue);
ImGui.CloseCurrentPopup();
}
}
ImGui.EndPopup();
}
ImGui.Separator();
ImGui.PushStyleVar(ImGuiStyleVar.ChildRounding, 5);
if (ImGui.BeginChild("ObstacleList", ImGui.GetContentRegionAvail(), true))
{
if (ObstacleHandler.Count != 0)
{
for (int i = 0; i < ObstacleHandler.Count; i++)
{
var obstacle = ObstacleHandler.Obstacles[i];
ImGui.TreeNodeEx($"Obstacle {i}", _baseTreeLeafFlags | GetTreeNodeSelectionFlag(obstacle));
if (ImGui.IsItemDeactivated())
{
UpdateSelection(obstacle);
}
}
}
else
{
ImGui.Text("Empty.");
}
ImGui.EndChild();
}
ImGui.End();
}
}
private void InitiateHandler()
{
scoreManager = new ScoreManager();
obstacleHandler = new ObstacleHandler();
birdHandler = new BirdHandler();
}
