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); } } }
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]); }
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)); }
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]); }
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)); } }
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]); }
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)); }