public MoveCartesianPathOperation(MoveCartesianPathArgs args)
{
if (args == null)
{
throw new ArgumentNullException(nameof(args));
}
if (args.EndEffector == null)
{
throw new ArgumentException("EndEffector member of arguments must not be null.");
}
this.EndEffector = args.EndEffector;
this.Seed = args.Seed;
this.Waypoints = args.Waypoints;
this.StartPose = args.StartPose;
this.Start = args.Start;
this.MoveGroup = this.EndEffector.MoveGroup;
this.VelocityScaling = args.VelocityScaling;
this.AccelerationScaling = args.AccelerationScaling;
this.Parameters = this.MoveGroup.BuildPlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution);
this.TaskSpaceParameters = this.EndEffector.BuildTaskSpacePlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.IkJumpThreshold);
if (args.SampleResolution.HasValue)
{
this.TaskSpaceParameters = this.TaskSpaceParameters.WithSampleResolution(args.SampleResolution.Value);
}
}
private void SetDefaultEndEffector(string endEffectorName = null, IList <MoveGroupDescription> groupDescription = null)
{
var groups = groupDescription ?? this.motionService.QueryAvailableMoveGroups();
var groupDetails = groups.ToDictionary(x => x.Name, x => x);
if (!groupDetails.TryGetValue(this.name, out this.details))
{
throw new Exception($"Move group with name '{this.name}' not found.");
}
Dictionary <string, IEndEffector> tmp = new Dictionary <string, IEndEffector>();
for (int i = 0; i < this.details.EndEffectorNames.Length; i++)
{
tmp.Add(this.details.EndEffectorNames[i], (IEndEffector) new EndEffector(this, this.details.EndEffectorNames[i], this.details.EndEffectorLinkNames[i]));
}
this.endEffectors = tmp;
if (!this.endEffectors.ContainsKey(endEffectorName ?? ""))
{
endEffectorName = this.details.EndEffectorNames.FirstOrDefault(); // use first end effector as default
}
this.defaultEndEffectorName = endEffectorName;
var limits = this.motionService.QueryEndEffectorLimits(endEffectorName);
this.defaultTaskSpacePlanParameters = this.motionService.CreateTaskSpacePlanParameters(
this.defaultEndEffectorName,
limits.MaxXYZVelocity,
limits.MaxXYZAcceleration,
limits.MaxAngularVelocity,
limits.MaxAngularAcceleration
);
}
public void TestWithIkJumpThreshold()
{
var builder = new TaskSpacePlanParameters.Builder();
var parameters = new TaskSpacePlanParameters(builder);
Assert.Equal(5, parameters.WithIkJumpThreshold(5).IkJumpThreshold);
Assert.Equal(-3, parameters.WithIkJumpThreshold(-3).IkJumpThreshold);
}
public void TestWithMaxDeviation()
{
var builder = new TaskSpacePlanParameters.Builder();
var parameters = new TaskSpacePlanParameters(builder);
Assert.Equal(5, parameters.WithMaxDeviation(5).MaxDeviation);
Assert.Equal(-3, parameters.WithMaxDeviation(-3).MaxDeviation);
}
public void TestWithSampleResolution()
{
var builder = new TaskSpacePlanParameters.Builder();
var parameters = new TaskSpacePlanParameters(builder);
Assert.Equal(5, parameters.WithSampleResolution(5).SampleResolution);
Assert.Equal(-3, parameters.WithSampleResolution(-3).SampleResolution);
}
public void TestWithCollision()
{
var builder = new TaskSpacePlanParameters.Builder();
var parameters = new TaskSpacePlanParameters(builder);
Assert.True(parameters.WithCollisionCheck(true).CollisionCheck);
Assert.False(parameters.WithCollisionCheck(false).CollisionCheck);
}
public MovePoseOperationBase(MovePoseArgs args)
{
this.EndEffector = args.EndEffector;
this.Seed = args.Seed;
this.TargetPose = args.TargetPose;
this.StartPose = args.StartPose;
this.Start = args.Start;
this.MoveGroup = this.EndEffector.MoveGroup;
this.VelocityScaling = args.VelocityScaling;
this.AccelerationScaling = args.AccelerationScaling;
this.Parameters = this.MoveGroup.BuildPlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.SampleResolution);
this.TaskSpaceParameters = this.EndEffector.BuildTaskSpacePlanParameters(args.VelocityScaling, args.CollisionCheck, args.MaxDeviation, args.AccelerationScaling, args.IkJumpThreshold);
if (args.SampleResolution.HasValue)
{
this.TaskSpaceParameters = this.TaskSpaceParameters.WithSampleResolution(args.SampleResolution.Value);
}
}
