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