public async static Task MovePoseLinearSupervised(
[InputPin(PropertyMode = PropertyMode.Default, Editor = "Pose")] PoseProperty target,
[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)] bool cacheResult = true,
CancellationToken cancel = default(CancellationToken)
)
{
if (target == null)
{
throw new ArgumentNullException(nameof(target), "Required property 'target' of MovePoseLinear module was not specified.");
}
var targetPose = await ResolveProperty(target);
var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName);
if (endEffector == null)
{
throw new Exception($"EndEffector '{endEffectorName}' not available.");
}
using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name))
{
group.SampleResolution = sampleResolution;
group.IkJumpThreshold = ikJumpThreshold;
using (var client = group.GetEndEffector(endEffector.Name).MovePoseLinearSupervisedAsync(targetPose, velocityScaling, collisionChecking, null, cancel))
{
await HandleStepwiseMotions(client, group);
}
}
}
public async static Task <bool> WithinCuboid(
[InputPin(PropertyMode = PropertyMode.Never)] PoseModel source,
[InputPin(PropertyMode = PropertyMode.Default, Editor = "Pose")] PoseProperty center,
[InputPin(PropertyMode = PropertyMode.Default)] double width = 0.1,
[InputPin(PropertyMode = PropertyMode.Default)] double height = 0.1,
[InputPin(PropertyMode = PropertyMode.Default)] double depth = 0.1,
CancellationToken cancel = default(CancellationToken))
{
if (center == null)
{
throw new ArgumentNullException(nameof(center), "Required property 'center' of CheckCurrentPose module was not specified.");
}
if (source == null)
{
throw new ArgumentNullException(nameof(source), "Required property 'source' of CheckCurrentPose module was not specified.");
}
var centerPose = await ResolveProperty(center);
var currentPose = source.ToPose();
var dist = Vector3.Abs(Vector3.Subtract(centerPose.Translation, currentPose.Translation));
return(dist.X < width && dist.Y < height && dist.Z < depth);
}
private static async Task <Pose> ResolveProperty(PoseProperty p)
{
if (p == null)
{
return(null);
}
if (p.Source == PropertySource.Constant)
{
return(p.Value);
}
return((await worldViewService.GetPoseByPath(p.Path)).Value.ToPose());
}
public async static Task MovePoseCollisionFreeSupervised(
[InputPin(PropertyMode = PropertyMode.Default, Editor = "Pose")] PoseProperty target,
[InputPin(PropertyMode = PropertyMode.Default)] string endEffectorName = null,
[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 (target == null)
{
throw new ArgumentNullException(nameof(target), "Required property 'target' for MovePoseCollisionFreeSupervised module was not specified.");
}
var endEffector = MotionService.QueryAvailableEndEffectors().FirstOrDefault(x => x.Name == endEffectorName);
if (endEffector == null)
{
throw new Exception($"EndEffector '{endEffectorName}' not available.");
}
var targetPose = await ResolveProperty(target);
var seedValues = await ResolveProperty(seed);
using (var group = MotionService.CreateMoveGroup(endEffector.MoveGroupName, endEffector.Name))
{
if (seed == null)
{
seedValues = group.CurrentJointPositions;
}
group.SampleResolution = sampleResolution;
using (var client = group.GetEndEffector(endEffector.Name).MovePoseCollisionFreeSupervisedAsync(targetPose, seedValues, cancel))
{
await HandleStepwiseMotions(client, group);
}
}
}
public async static Task <Tuple <TwistModel, float, float> > CalculateTwist(
[InputPin(PropertyMode = PropertyMode.Default, Editor = "Pose")] PoseProperty source,
[InputPin(PropertyMode = PropertyMode.Default, Editor = "Pose")] PoseProperty destination,
CancellationToken cancel = default(CancellationToken))
{
if (source == null)
{
throw new ArgumentNullException(nameof(source), "Required property 'source' of CalculateTwist module was not specified.");
}
if (destination == null)
{
throw new ArgumentNullException(nameof(destination), "Required property 'destination' of CalculateTwist module was not specified.");
}
var sourcePose = await ResolveProperty(source);
var destinationPose = await ResolveProperty(destination);
if (sourcePose.Frame != destinationPose.Frame)
{
// ## AKo: If both poses have a different base frame we need to resolve absolute poses.
// e.g. it would be possible to use ROS tf to get the actual poses or calculate the difference.
throw new NotSupportedException("Currently twist can only calculated if both poses share the same reference frame.");
}
var transformationA = sourcePose.TransformMatrix;
var transformationInverted = transformationA;
var transformationB = destinationPose.TransformMatrix;
Matrix4x4.Invert(transformationA, out transformationInverted);
var deltaPose = new Pose(Matrix4x4.Multiply(transformationB, transformationInverted), sourcePose.Frame);
Twist resultTwist = deltaPose.ToTwist();
return(Tuple.Create(resultTwist.ToModel(), resultTwist.Linear.Length(), resultTwist.Angular.Length()));
}
