Esempio n. 1
0
        public static List <Attractor> Create(Manipulator manipulator, int count, float goalThreshold, bool discardOutliers = true)
        {
            var attractors = new List <Attractor>();

            //// adding goal attractor
            //Vector3 attrPoint = manipulator.Goal;

            //attractors.Add(new Attractor(attrPoint));

            // adding ancillary attractors
            while (attractors.Count < count)
            {
                // generating attractor point
                Vector3 point = RandomThreadStatic.NextPointSphere(manipulator.WorkspaceRadius) + manipulator.Base;

                // checking whether the attractor is inside any obstacle or not if requested
                if (!(discardOutliers && ObstacleHandler.ContainmentTest(point, out _)))  // TODO: consider creating a list of bad attractors; they may serve as repulsion points
                {
                    // adding attractor to the list
                    Vector3 attrPoint = point;
                    attractors.Add(new Attractor(attrPoint));
                }
            }

            return(attractors);
        }
Esempio n. 2
0
        public void Run(Manipulator manipulator, CancellationToken cancellationToken = default)
        {
            try
            {
                // start measuring execution time
                Timer.Restart();

                // turn the controller on
                State = ControllerState.Running;

                // start motion control if a path has been found
                if (manipulator.Path != null)
                {
                    using (var manipulatorCopy = manipulator.DeepCopy())
                    {
                        // execute motion control
                        Path.Node gripperPos = manipulator.Path.Current;
                        while (gripperPos.Child != null)
                        {
                            cancellationToken.ThrowIfCancellationRequested();

                            var current = gripperPos.Child;
                            while (current.Child != null)  // last point may not be deformed, since it is a goal point
                            {
                                for (int j = current.Points.Length - 1; j > 0; j--)
                                {
                                    if (ObstacleHandler.ContainmentTest(current.Points[j], out Obstacle obstacle))
                                    {
                                        //Deform(manipulatorCopy, obstacle, current, j);
                                    }
                                }

                                current = current.Child;
                            }

                            //Discretize(manipulatorCopy, gripperPos);

                            gripperPos = manipulator.Path.Current;
                        }
                    }
                }

                // turn the controller off
                State = ControllerState.Idle;
            }
            catch (OperationCanceledException oce)
            {
                // indicate that the process has been aborted
                State = ControllerState.Aborted;
            }
            finally
            {
                // stop measuring execution time
                Timer.Stop();
            }
        }