Beispiel #1
0
 public Controller(Manipulator manipulator, PathPlanner pathPlanner, InverseKinematicsSolver inverseKinematicsSolver, MotionController motionController)
 {
     Manipulator             = manipulator;
     InverseKinematicsSolver = inverseKinematicsSolver;
     PathPlanner             = pathPlanner;
     MotionController        = motionController;
 }
Beispiel #2
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);
        }
        public static void Remove(Manipulator manipulator)
        {
            int index = Manipulators.IndexOf(manipulator);

            if (Manipulators.Remove(manipulator))
            {
                manipulator.Dispose();

                // remove goal model
                MainWindow._goalModels[index].Dispose();
                MainWindow._goalModels.RemoveAt(index);

                // remove goal model
                //MainWindow._treeModels[index].Dispose();
                //MainWindow._treeModels.RemoveAt(index);

                // remove goal model
                //MainWindow._pathModels[index].Dispose();
                //MainWindow._pathModels.RemoveAt(index);

                // remove goal model
                //MainWindow._gaModels[index].Dispose();
                //MainWindow._gaModels.RemoveAt(index);
            }
        }
Beispiel #4
0
 public static void RecalculateWeights(List <Attractor> attractors, Manipulator manipulator, float goalThreshold)
 {
     foreach (var attractor in attractors)
     {
         attractor.Weight = CalculateAttractorWeight(manipulator, attractor.Center);
         attractor.Radius = CalculateAttractorRadius(manipulator, attractor.Weight, goalThreshold);
     }
 }
Beispiel #5
0
        public static void Execute(Manipulator manip)
        {
            var AD = Dispatcher.WorkspaceBuffer.AlgBuffer;

            /*// generating random tree
             * var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime);  // TODO: solvers should be declared inside planners!
             * var planner = new RRT(Obstacles, solver, AD.k, true, AD.d);
             * PathPlanner pp = planner;
             * var resRRT = pp.Execute(manip, null);
             *
             * // acquiring all the Vector3s and configurations along the path
             * manip.Path = resRRT.Item1;
             * manip.States["Path"] = true;
             * manip.Configs = resRRT.Item2;*/

            /*// generating random tree
             * var solver = new HillClimbing(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime);  // TODO: solvers should be declared inside planners!
             * var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);
             * PathPlanner pp = planner;
             * var resRRT = pp.Execute(manip, null);
             *
             * // acquiring all the Vector3s and configurations along the path
             * manip.Path = resRRT.Item1;
             * manip.States["Path"] = true;
             * manip.Configs = resRRT.Item2;*/

            var solver  = new Jacobian(Obstacles, AD.Precision, AD.StepSize, AD.MaxTime); // TODO: solvers should be declared inside planners!
            var planner = new DynamicRRT(Obstacles, solver, AD.k, true, AD.d, AD.k / 100);

            planner.Start(manip, Vector3.Zero);

            /*var resGA = PathPlanner.GeneticAlgorithm(manip, Obstacles, manip.Goal, resRRT.Item2.ToArray(),
             *  0.99, manip.Joints.Length, 20, 0.95, 0.1, 10000,
             *  PathPlanner.OptimizationCriterion.CollisionFree,
             *  PathPlanner.SelectionMode.NormalDistribution,
             *  PathPlanner.CrossoverMode.WeightedMean,
             *  t => t * Math.PI / 180);*/

            /*var input = new (Vector3, float[])[resRRT.Item1.Count];
             * for (int i = 0; i < input.Length; i++)
             * {
             *  input[i].Item1 = resRRT.Item1[i];
             *  input[i].Item2 = resRRT.Item2[i];
             * }
             *
             * var jac = new Jacobian(Obstacles, manip.q.Length, AD.Precision, AD.StepSize, AD.MaxTime);
             * var resGA = PathPlanner.GeneticAlgorithmD(manip, Obstacles, manip.Goal, jac,
             *  input, 0.99, manip.Joints.Length, 4, 0.95, 0.1, 10000,
             *  PathPlanner.OptimizationCriterion.CollisionFree,
             *  PathPlanner.SelectionMode.NormalDistribution,
             *  PathPlanner.CrossoverMode.WeightedMean,
             *  t => t * Math.PI / 180);
             *
             * // acquiring all the Vector3s and configurations along the path
             * manip.Path = resGA.Item1;
             * manip.States["Path"] = true;
             * manip.Configs = resGA.Item2;*/
        }
Beispiel #6
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();
            }
        }
        public static void Plan(Manipulator manip)
        {
            /*var resGA = PathPlanner.GeneticAlgorithm(manip, Obstacles, manip.Goal, resRRT.Item2.ToArray(),
             *  0.99, manip.Joints.Length, 20, 0.95, 0.1, 10000,
             *  PathPlanner.OptimizationCriterion.CollisionFree,
             *  PathPlanner.SelectionMode.NormalDistribution,
             *  PathPlanner.CrossoverMode.WeightedMean,
             *  t => t * Math.PI / 180);*/

            /*var jac = new Jacobian(Obstacles, manip.q.Length, AD.Precision, AD.StepSize, AD.MaxTime);
             * var resGA = PathPlanner.GeneticAlgorithmD(manip, Obstacles, manip.Goal, jac,
             *  input, 0.99, manip.Joints.Length, 4, 0.95, 0.1, 10000,
             *  PathPlanner.OptimizationCriterion.CollisionFree,
             *  PathPlanner.SelectionMode.NormalDistribution,
             *  PathPlanner.CrossoverMode.WeightedMean,
             *  t => t * Math.PI / 180);*/
        }
Beispiel #8
0
        public Manipulator(Manipulator source)  // TODO: make a m.CreateCopy() method, not a constructor! that's much better
        {
            Links  = Misc.CopyArray(source.Links);
            Joints = Array.ConvertAll(source.Joints, x => x.Copy()); //Misc.CopyArray(source.Joints);

            Base = source.Base;                                      // TODO: review referencing

            DH = Array.ConvertAll(source.DH, x => x.Copy());         //Misc.CopyArray(source.DH);
            for (int i = 0; i < DH.Length; i++)
            {
                DH[i].joint = Joints[i];
            }
            UpdateTransMatrices();

            WorkspaceRadius = source.WorkspaceRadius;

            Goal = source.Goal;  // TODO: review referencing

            States = new Dictionary <string, bool>(source.States);
        }
Beispiel #9
0
 private static float CalculateAttractorRadius(Manipulator manipulator, float weight, float goalThreshold)
 {
     return(goalThreshold * weight / manipulator.DistanceTo(manipulator.Goal));  /*(float)Math.Pow(weight / manipulator.DistanceTo(manipulator.Goal), 2);*/
 }
Beispiel #10
0
 private static float CalculateAttractorWeight(Manipulator manipulator, Vector3 point)
 {
     return(manipulator.DistanceTo(point) + manipulator.Goal.DistanceTo(point));
 }
Beispiel #11
0
        public static void Initialize()
        {
            var LD = Dispatcher.WorkspaceBuffer.LinkBuffer;
            var JD = Dispatcher.WorkspaceBuffer.JointBuffer;
            var OD = Dispatcher.WorkspaceBuffer.ObstBuffer;

            // manipulators
            Manipulators    = new Manipulator[1];
            Manipulators[0] = new Manipulator(LD, JD, new TupleDH[]
            {
                new TupleDH(0, JD[0].Length + LD[0].Length, 90 * (float)Math.PI / 180, 0),
                new TupleDH(-90 * (float)Math.PI / 180, 0, 0, JD[1].Length + LD[1].Length),
                new TupleDH(0, 0, 0, JD[2].Length + LD[2].Length)
            });
            Manipulators[0].Goal = new Vector3(0, 0.5f, 2.5f);

            // obstacles
            Obstacles = new Obstacle[OD.Length];
            for (int i = 0; i < OD.Length; i++)
            {
                Obstacles[i] = new Obstacle(Primitives.Sphere(OD[i].r, new Vector3(OD[i].c.X, OD[i].c.Y, OD[i].c.Z), OD[i].Vector3s_num, new Random()), ColliderShape.Sphere);
            }

            var manip = Manipulators[0];

            manip.GoodAttractors = new List <Attractor>();
            manip.BadAttractors  = new List <Attractor>();

            Random rng = new Random();
            float  work_radius = manip.WorkspaceRadius, x, y_pos, y, z_pos, z;

            // adding main attractor
            Vector3 AttrVector3 = manip.Goal;

            float AttrWeight = manip.DistanceTo(manip.Goal);

            float r = Dispatcher.WorkspaceBuffer.AlgBuffer.d * (float)Math.Pow(AttrWeight / manip.DistanceTo(manip.Goal), 4);

            Vector3[] AttrArea = Primitives.Sphere(r, AttrVector3, 64, new Random());

            manip.GoodAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
            manip.States["Goal"] = true;

            var AD = Dispatcher.WorkspaceBuffer.AlgBuffer;

            // adding ancillary attractors
            while (manip.GoodAttractors.Count < AD.AttrNum)
            {
                // generating attractor Vector3
                x     = -work_radius + (float)rng.NextDouble() * 2 * work_radius;
                y_pos = (float)Math.Sqrt(work_radius * work_radius - x * x);
                y     = -y_pos + (float)rng.NextDouble() * 2 * y_pos;
                z_pos = (float)Math.Sqrt(work_radius * work_radius - x * x - y * y);
                z     = -z_pos + (float)rng.NextDouble() * 2 * z_pos;

                Vector3 p = new Vector3(x, y, z) + manip.Base;

                // checking whether the attractor is inside any obstacle or not
                bool collision = false;
                foreach (var obst in Obstacles)
                {
                    if (obst.Contains(p))
                    {
                        collision = true;
                        break;
                    }
                }

                // adding attractor to the list
                AttrVector3 = p;

                AttrWeight = manip.DistanceTo(p) + manip.Goal.DistanceTo(p);

                r        = AD.d * (float)Math.Pow(AttrWeight / manip.DistanceTo(manip.Goal), 4);
                AttrArea = Primitives.Sphere(r, AttrVector3, 64, new Random());

                if (!collision)
                {
                    manip.GoodAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
                }
                else
                {
                    manip.BadAttractors.Add(new Attractor(AttrVector3, AttrWeight, AttrArea, r));
                }
            }
            manip.States["Attractors"] = true;
        }
        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);
        }
 public static void Add(Manipulator manipulator)
 {
     Manipulators.Add(manipulator);
 }