private Path ConstructPath(BezierCurve bezierCurve, float step) // TODO: refactor! { // reset manipulator _manipulator.q = _initialConfiguration; float counter = 0; var points = new List <Vector3[]> { _manipulator.DKP }; var configs = new List <VectorFloat> { _manipulator.q }; while (counter <= 1) { var ikRes = _solver.Execute(_manipulator, bezierCurve.CalculatePoint(counter)); _manipulator.q = ikRes.Configuration; points.Add(_manipulator.DKP); configs.Add(_manipulator.q); counter += step; } return(new Path(points, configs, false)); }
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 }); }