Exemplo n.º 1
0
 public Link(LinkData data)
 {
     Model  = data.Model;
     Length = data.Length;
 }
Exemplo n.º 2
0
        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);
        }