Beispiel #1
0
        public async static Task MoveJWaypointsSupervised(
            [InputPin(PropertyMode = PropertyMode.Never)] IEnumerable <JointValues> waypoints = null,
            [InputPin(PropertyMode = PropertyMode.Default)] bool collisionChecking            = true,
            [InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling            = 1,
            [InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution           = 0.05,
            [InputPin(PropertyMode = PropertyMode.Default)] double maxDeviation = 0.0,
            [InputPin(PropertyMode = PropertyMode.Default)] bool cacheResult    = false,
            CancellationToken cancel = default(CancellationToken)
            )
        {
            if (waypoints == null)
            {
                throw new ArgumentNullException("Required property 'waypoints' for MoveJWaypoints module was not specified.", nameof(waypoints));
            }
            if (waypoints.ToList().Count == 0)
            {
                throw new ArgumentException("Required property 'waypoints' is empty.", nameof(waypoints));
            }
            JointPath path = new JointPath(waypoints.First().JointSet, waypoints);

            using (var group = MotionService.CreateMoveGroupForJointSet(path.JointSet))
            {
                group.SampleResolution = sampleResolution;
                using (var client = group.MoveJointPathSupervisedAsync(path, velocityScaling, collisionChecking, maxDeviation, null, cancel))
                {
                    await HandleStepwiseMotions(client, group);
                }
            }
        }
Beispiel #2
0
        public void TestInit()
        {
            var joints  = new JointSet("a", "b", "c");
            var start   = new JointValues(joints, new double[] { 1, 2, 3 });
            var goal    = new JointValues(joints, new double[] { 0, 0, 0 });
            var badGoal = new JointValues(new JointSet("d", "b", "c"), new double[] { 0, 0, 0 });

            var p1 = new JointPath(start);

            Assert.Equal(start, p1[0]);

            var p2 = new JointPath(start, goal);

            Assert.Equal(start, p2[0]);
            Assert.Equal(goal, p2[1]);

            Assert.Throws <ArgumentException>(() => new JointPath(start, badGoal));

            var p3 = new JointPath(joints, start, goal);

            Assert.Equal(start, p3[0]);
            Assert.Equal(goal, p3[1]);

            var p4 = new JointPath(joints, new JointValues[] { start, goal });

            Assert.Equal(start, p4[0]);
            Assert.Equal(goal, p4[1]);
        }
Beispiel #3
0
        public override IPlan Plan()
        {
            JointValues      start      = this.Start ?? this.MoveGroup.CurrentJointPositions;                      // get start joint values
            JointPath        jointPath  = new JointPath(this.MoveGroup.JointSet, start, this.Target);              // generate joint path
            IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveJoints(jointPath, this.Parameters); // plan trajectory

            return(new Plan(this.MoveGroup, trajectory, this.Parameters));
        }
Beispiel #4
0
        public void TestPrepend()
        {
            var joints = new JointSet("a", "b", "c");
            var val1   = new JointValues(joints, new double[] { 1, 2, 3 });
            var val2   = new JointValues(joints, new double[] { 5, 5, 5 });
            var val3   = new JointValues(joints, new double[] { 0, 0, 0 });
            var p      = new JointPath(val1).Prepend(new JointValues[] { val2, val3 });

            Assert.Equal(val2, p[0]);
            Assert.Equal(val3, p[1]);
            Assert.Equal(val1, p[2]);
        }
Beispiel #5
0
        public void TestConcat()
        {
            var joints = new JointSet("a", "b", "c");
            var val1   = new JointValues(joints, new double[] { 1, 2, 3 });
            var val2   = new JointValues(joints, new double[] { 5, 5, 5 });
            var val3   = new JointValues(joints, new double[] { 0, 0, 0 });
            var p1     = new JointPath(val1);
            var p2     = new JointPath(val2, val3);
            var p      = p1.Concat(p2);

            Assert.Equal(val1, p[0]);
            Assert.Equal(val2, p[1]);
            Assert.Equal(val3, p[2]);
        }
        public static PyObject ToPython(this JointPath obj)
        {
            var jointSet = PyConvert.ToPyObject(obj.JointSet);

            using (Py.GIL())
            {
                var jointValues = new PyList();
                foreach (var v in obj)
                {
                    jointValues.Append(PyConvert.ToPyObject(v));
                }
                return(pyXamlaMotionTypes.JointPath(jointSet, jointValues));
            }
        }
Beispiel #7
0
        public void TestTransform()
        {
            var joints = new JointSet("a", "b", "c");
            var val1   = new JointValues(joints, new double[] { 1, 2, 3 });
            var val2   = new JointValues(joints, new double[] { 5, 5, 5 });
            var val3   = new JointValues(joints, new double[] { 0, 0, 0 });
            var p1     = new JointPath(joints, val1, val2, val3);

            Func <JointValues, int, JointValues> func = (jVals, i) => jVals.Add(i);
            var p = p1.Transform(func);

            Assert.Equal(val1, p[0]);
            Assert.Equal(new JointValues(joints, new double[] { 6, 6, 6 }), p[1]);
            Assert.Equal(new JointValues(joints, new double[] { 2, 2, 2 }), p[2]);
        }
Beispiel #8
0
        public void TestSub()
        {
            var joints = new JointSet("a", "b", "c");
            var val1   = new JointValues(joints, new double[] { 1, 2, 3 });
            var val2   = new JointValues(joints, new double[] { 5, 5, 5 });
            var val3   = new JointValues(joints, new double[] { 0, 0, 0 });
            var val4   = new JointValues(joints, new double[] { 2, 2, 2 });
            var pBig   = new JointPath(joints, val1, val2, val3, val4);

            var p = pBig.Sub(1, 3);

            Assert.Equal(val2, p[0]);
            Assert.Equal(val3, p[1]);
            Assert.Throws <ArgumentOutOfRangeException>(() => p[2]);
            Assert.Throws <ArgumentOutOfRangeException>(() => pBig.Sub(1, 5));
        }