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