public void Trim(Manipulator manipulator, InverseKinematicsSolver solver) { switch (Mode) { case TreeBehaviour.Cyclic: var source = new Queue <Node>(); source.Enqueue(Root); while (source.Count != 0) { var childs = new List <Node>(source.Dequeue().Childs); foreach (var child in childs) { manipulator.q = child.q; if (manipulator.CollisionTest().Contains(true)) { RemoveNode(child); } else { source.Enqueue(child); } } } break; case TreeBehaviour.Recursive: foreach (var child in Root.Childs) { TrimRecursive(manipulator, solver, child); } break; } }
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 TrimRecursive(Manipulator manipulator, InverseKinematicsSolver solver, Node node) { manipulator.q = node.q; if (manipulator.CollisionTest().Contains(true)) { RemoveNode(node); return; } var childs = new List <Node>(node.Childs); foreach (var child in childs) { TrimRecursive(manipulator, solver, child); } }
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 }); }