public void SetupEffectorPosRot(Position position, Rotation rotation)
{
joints[5].Frame.P = position;
var q = new Quaternion();
converter.Convert(rotation.R, rotation.P, rotation.Y, ref q);
var m = converter.BuildMatrix3DFromQuaternion(q);
joints[5].Frame.X = new Vector3D(m.M11, m.M12, m.M13);
joints[5].Frame.X.Normalize();
joints[5].Frame.Y = new Vector3D(m.M21, m.M22, m.M23);
joints[5].Frame.Y.Normalize();
joints[5].Frame.Z = new Vector3D(m.M31, m.M32, m.M33);
joints[5].Frame.Z.Normalize();
}
private void SetupStartConfiguration()
{
var eulerStartTransformGroup = new Transform3DGroup();
var quaternionStartTransformGroup = new Transform3DGroup();
eulerStartTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(xDirection, startAngleR)));
eulerStartTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(yDirection, startAngleP)));
eulerStartTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(zDirection, startAngleY)));
eulerStartTransformGroup.Children.Add(new TranslateTransform3D(StartPositionX, StartPositionY, StartPositionZ));
FrameStartEuler.Transform = eulerStartTransformGroup;
startQuaternion = new Quaternion(StartQuaternionX, StartQuaternionY, StartQuaternionZ, StartQuaternionW);
quaternionStartTransformGroup.Children.Add(new RotateTransform3D(new QuaternionRotation3D(startQuaternion)));
quaternionStartTransformGroup.Children.Add(new TranslateTransform3D(StartPositionX, StartPositionY, StartPositionZ));
FrameStartQuaternion.Transform = quaternionStartTransformGroup;
var startPosition = new Position(StartPositionX, StartPositionY, StartPositionZ);
var startRotation = new Rotation(startAngleR, startAngleP, startAngleY);
//SetupConfiguration(FrameStartEuler, FrameStartQuaternion, startPosition, startRotation, ref startQuaternion);
robotForInternalCoordinates.SetupEffectorPosRot(startPosition, startRotation);
robotForInternalCoordinates.CalculateConfiguration(true);
robotForEffectoPosition.SetupEffectorPosRot(startPosition, startRotation);
robotForEffectoPosition.CalculateConfiguration(true);
}
private void SetupEndConfiguration()
{
var eulerEndTransformGroup = new Transform3DGroup();
var quaternionEndTransformGroup = new Transform3DGroup();
eulerEndTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(xDirection, EndAngleR)));
eulerEndTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(yDirection, EndAngleP)));
eulerEndTransformGroup.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(zDirection, EndAngleY)));
eulerEndTransformGroup.Children.Add(new TranslateTransform3D(EndPositionX, EndPositionY, EndPositionZ));
FrameEndEuler.Transform = eulerEndTransformGroup;
endQuaternion = new Quaternion(EndQuaternionX, EndQuaternionY, EndQuaternionZ, EndQuaternionW);
quaternionEndTransformGroup.Children.Add(new RotateTransform3D(new QuaternionRotation3D(endQuaternion)));
quaternionEndTransformGroup.Children.Add(new TranslateTransform3D(EndPositionX, EndPositionY, EndPositionZ));
FrameEndQuaternion.Transform = quaternionEndTransformGroup;
var endPosition = new Position(EndPositionX, EndPositionY, EndPositionZ);
var endRotation = new Rotation(endAngleR, endAngleP, endAngleY);
// SetupConfiguration(FrameEndEuler, FrameEndQuaternion, endPosition, endRotation, ref endQuaternion);
robotForInternalCoordinates.SetupEffectorPosRot(endPosition, endRotation);
robotForInternalCoordinates.CalculateConfiguration(false);
robotForEffectoPosition.SetupEffectorPosRot(endPosition, endRotation);
robotForEffectoPosition.CalculateConfiguration(false);
}
private void dispatcherTimer_Tick(object sender, EventArgs e)
{
var currentTime = DateTime.Now;
var timeDifference = currentTime - startTime - timeDelay;
var elapsedMilliseconds = timeDifference.TotalMilliseconds;
var animationTimeInMilliseconds = AnimationTime * 1000;
var normalizedTime = elapsedMilliseconds / animationTimeInMilliseconds;
if (elapsedMilliseconds > animationTimeInMilliseconds)
{
ResetButton_Click(null, null);
return;
}
var currentRotation = new Rotation(currentAngleR, currentAngleP, currentAngleY);
linearInterpolator.CalculateCurrentPosition(ref currentPosition, normalizedTime);
linearInterpolator.CalculateCurrentAngle(ref currentRotation, normalizedTime);
if (lerpActivated)
linearInterpolator.CalculateCurrentQuaternion(ref currentQuaternion, normalizedTime);
else
{
sphericalLinearInterpolator.CalculateCurrentQuaternion(ref currentQuaternion, normalizedTime);
}
SetupCurrentConfiguration();
robotForInternalCoordinates.Interpolate(realTimeInterpolator, normalizedTime);
robotForEffectoPosition.SetupEffectorPosQuat(currentPosition, currentQuaternion);
robotForEffectoPosition.CalculateConfiguration(true);
}
public void CalculateCurrentAngle(ref Rotation currentRotation, double normalizedTime)
{
currentRotation.R = StartAngleR + normalizedTime * (EndAngleR - StartAngleR);
currentRotation.P = StartAngleP + normalizedTime * (EndAngleP - StartAngleP);
currentRotation.Y = StartAngleY + normalizedTime * (EndAngleY - StartAngleY);
}
public void CalculateCurrentAngle(ref Rotation currentRotation, double normalizedTime)
{
currentRotation.R = StartAngleR + normalizedTime * (EndAngleR - StartAngleR);
currentRotation.P = StartAngleP + normalizedTime * (EndAngleP - StartAngleP);
currentRotation.Y = StartAngleY + normalizedTime * (EndAngleY - StartAngleY);
}
