public override IPlan Plan()
{
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // start pose
CartesianPath posePath = new CartesianPath(startPose, this.TargetPose); // generate taskspace path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(posePath, seed, this.TaskSpaceParameters);
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public static PyObject ToPython(this CartesianPath obj)
{
using (Py.GIL())
{
var poses = new PyList();
foreach (var p in obj)
{
poses.Append(PyConvert.ToPyObject(p));
}
return(pyXamlaMotionTypes.CartesianPath(poses));
}
}
public static async Task UpdateCartesianPath(
[InputPin(PropertyMode = PropertyMode.Default)] string elementPath,
[InputPin(PropertyMode = PropertyMode.Allow)] CartesianPath cartesianPath
)
{
var trace = await worldViewService.ResolvePath(elementPath, WorldViewElementType.JointValues);
var element = await worldViewService.GetCartesianPathByTrace(trace);
element.Value = cartesianPath.ToModel();
await worldViewService.UpdateCartesianPath(element);
}
public void TestInit()
{
int count = 50;
List <Pose> poses = poseHelper.RandomPoses(count).ToList();
var path = new CartesianPath(poses);
int i = 0;
foreach (var pose in poses)
{
Assert.Equal(pose, path[i]);
++i;
}
Assert.Equal(count, path.Count());
}
public static async Task AddCartesianPath(
[InputPin(PropertyMode = PropertyMode.Default)] string elementPath,
[InputPin(PropertyMode = PropertyMode.Allow)] CartesianPath cartesianPath,
[InputPin(PropertyMode = PropertyMode.Default)] bool transient = false
)
{
var path = new PosixPath(elementPath);
string containerPath = path.RemoveLastElement().ToPosixPath();
string displayName = path.LastElement;
var containerTrace = await worldViewService.ResolvePath(containerPath);
await worldViewService.AddCartesianPath(containerTrace, cartesianPath.ToModel(), displayName, transient);
}
public void TestAppend()
{
int count = 50;
List <Pose> poses1 = poseHelper.RandomPoses(count).ToList();
List <Pose> poses2 = poseHelper.RandomPoses(count).ToList();
List <Pose> poses = poses1.Concat(poses2).ToList();
ICartesianPath path = new CartesianPath(poses1);
path = path.Append(poses2);
for (int i = 0; i < 2 * count; ++i)
{
Assert.Equal(poses[i], path[i]);
}
Assert.Equal(2 * count, path.Count());
}
public async static Task MovePoseWaypointsSupervised(
[InputPin(PropertyMode = PropertyMode.Never)] IEnumerable <Pose> waypoints = null,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorName = null,
[InputPin(PropertyMode = PropertyMode.Default)] bool collisionChecking = false,
[InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling = 1,
[InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution = 0.05,
[InputPin(PropertyMode = PropertyMode.Default, Editor = "JointValues")] JointValuesProperty seed = null,
[InputPin(PropertyMode = PropertyMode.Default)] bool cacheResult = true,
CancellationToken cancel = default(CancellationToken)
)
{
if (waypoints == null)
{
throw new ArgumentNullException("Required property 'waypoints' for MovePoseLinearWaypointsSupervised module was not specified.", nameof(waypoints));
}
if (waypoints.ToList().Count == 0)
{
throw new ArgumentException("Required property 'waypoints' is empty.", nameof(waypoints));
}
var seedValues = await ResolveProperty(seed);
var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName);
if (endEffector == null)
{
throw new Exception($"EndEffector '{endEffectorName}' not available.");
}
CartesianPath path = new CartesianPath(waypoints);
using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name))
{
if (seed == null)
{
seedValues = group.CurrentJointPositions;
}
group.SampleResolution = sampleResolution;
group.CollisionCheck = collisionChecking;
group.VelocityScaling = velocityScaling;
using (var client = group.GetEndEffector(endEffectorName).MoveCartesianPathSupervisedAsync(path, seedValues, cancel))
{
await HandleStepwiseMotions(client, group);
}
}
}
public void TestSub()
{
int count = 10;
List <Pose> poses = poseHelper.RandomPoses(count).ToList();
ICartesianPath pathBig = new CartesianPath(poses);
int beg = 2;
int end = 5;
ICartesianPath path = pathBig.Sub(beg, end);
Assert.Equal(end - beg, path.Count());
for (int i = 0; i < end - beg; ++i)
{
Assert.Equal(pathBig[i + beg], path[i]);
}
Assert.Throws <ArgumentOutOfRangeException>(() => path.Sub(0, count + 1));
Assert.Throws <ArgumentOutOfRangeException>(() => path.Sub(-1, count - 1));
}
public async static Task MoveLWaypoints(
[InputPin(PropertyMode = PropertyMode.Never)] IEnumerable <Pose> waypoints = null,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorName = null,
[InputPin(PropertyMode = PropertyMode.Default)] bool collisionChecking = false,
[InputPin(PropertyMode = PropertyMode.Default)] double velocityScaling = 1,
[InputPin(PropertyMode = PropertyMode.Default)] double sampleResolution = 0.05,
[InputPin(PropertyMode = PropertyMode.Default)] double ikJumpThreshold = 1.6,
[InputPin(PropertyMode = PropertyMode.Default)] double maxDeviation = 0.0,
[InputPin(PropertyMode = PropertyMode.Default)] bool cacheResult = true,
CancellationToken cancel = default(CancellationToken)
)
{
if (waypoints == null)
{
throw new ArgumentNullException("Required property 'waypoints' for MoveLWayPoints module was not specified.", nameof(waypoints));
}
if (waypoints.ToList().Count == 0)
{
throw new ArgumentException("Required property 'waypoints' is empty.", nameof(waypoints));
}
var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName);
if (endEffector == null)
{
throw new Exception($"EndEffector '{endEffectorName}' not available.");
}
CartesianPath path = new CartesianPath(waypoints);
using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name))
{
group.SampleResolution = sampleResolution;
group.IkJumpThreshold = ikJumpThreshold;
var planParameters = group.BuildTaskSpacePlanParameters(velocityScaling, collisionChecking, maxDeviation, null, endEffector.Name);
var(trajectory, parameters) = group.GetEndEffector(endEffector.Name).PlanMovePoseLinearWaypoints(path, planParameters);
await MotionService.ExecuteJointTrajectory(trajectory, parameters.CollisionCheck, cancel);
}
}
