public static IJointPath PlanCartesianPath(
[InputPin(PropertyMode = PropertyMode.Allow)] ICartesianPath waypoints,
[InputPin(PropertyMode = PropertyMode.Default)] int numSteps,
[InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters
)
{
return(MotionService.PlanCartesianPath(waypoints, numSteps, parameters));
}
public static ICartesianPath PlanMoveCartesian(
[InputPin(PropertyMode = PropertyMode.Allow)] ICartesianPath path,
[InputPin(PropertyMode = PropertyMode.Default)] int numSteps,
[InputPin(PropertyMode = PropertyMode.Allow)] PlanParameters parameters
)
{
return(MotionService.PlanMoveCartesian(path, numSteps, parameters));
}
private MoveCartesianPathArgs CreateMoveCartesianPathArgs(ICartesianPath waypoints) =>
new MoveCartesianPathArgs
{
Waypoints = waypoints,
EndEffector = this,
MoveGroup = this.MoveGroup,
IkJumpThreshold = this.MoveGroup.IkJumpThreshold,
VelocityScaling = this.MoveGroup.VelocityScaling,
AccelerationScaling = this.MoveGroup.VelocityScaling,
CollisionCheck = this.MoveGroup.CollisionCheck,
SampleResolution = this.MoveGroup.SampleResolution,
MaxDeviation = this.MoveGroup.MaxDeviation
};
public override IPlan Plan()
{
if (this.Waypoints.Count == 0)
{
return(new Plan(this.MoveGroup, JointTrajectory.Empty, this.Parameters));
}
JointValues seed = this.Seed ?? this.MoveGroup.CurrentJointPositions; // get current posture for seed jointvalues
Pose startPose = this.StartPose ?? this.EndEffector.CurrentPose; // get current pose
ICartesianPath path = this.Waypoints.Prepend(startPose); // generate joint path
IJointTrajectory trajectory = this.MoveGroup.MotionService.PlanMoveCartesianPathLinear(path, seed, this.TaskSpaceParameters); // plan trajectory
return(new Plan(this.MoveGroup, trajectory, this.Parameters));
}
public void TestConcat()
{
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 path1 = new CartesianPath(poses1);
ICartesianPath path2 = new CartesianPath(poses2);
ICartesianPath path = path1.Concat(path2);
for (int i = 0; i < 2 * count; ++i)
{
Assert.Equal(poses[i], path[i]);
}
Assert.Equal(2 * count, path.Count());
}
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 static CartesianPathModel ToModel(this ICartesianPath path) =>
new CartesianPathModel
{
Points = path.Select(x => x.ToModel()).ToList()
};
/// <summary> /// Appends the given <c>Pose</c> values to the current path. /// </summary> public static ICartesianPath Append(this ICartesianPath path, params Pose[] values) => path.Append((IEnumerable <Pose>)values);
/// <summary> /// Creates a new <c>CartesianPath</c> by appending the given Cartesian path, to the current one. /// </summary> /// <param name="other">A Cartesian path that should be appended to the current one.</param> /// <returns>A new <c>CartesianPath</c> containing the current Cartesian path followed by the given one.</returns> public ICartesianPath Concat(ICartesianPath other) => Append(other);
public IMoveCartesianPathOperation MoveCartesianPath(ICartesianPath waypoints) => new MoveCartesianPathOperation(CreateMoveCartesianPathArgs(waypoints));
