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