private void PlayButton_Click(object sender, RoutedEventArgs e) { if (buttonsFlags[0]) return; for (int i = 0; i < buttonsFlags.Length; i++) buttonsFlags[i] = false; buttonsFlags[0] = true; AllFramesModeButton.IsEnabled = false; AllFramesModeClearButton.IsEnabled = false; linearInterpolator.SetupInterpolator( startAngleR, startAngleP, startAngleY, endAngleR, endAngleP, endAngleY, startPositionX, startPositionY, startPositionZ, endPositionX, endPositionY, endPositionZ, startQuaternion, endQuaternion); sphericalLinearInterpolator.SetupInterpolator( startQuaternion, endQuaternion ); if (!animationStarted) { animationStarted = true; CalibrateEulerAngles(); HelixViewportLeft.Children.Remove(FrameStartEulerManipulator); HelixViewportLeft.Children.Remove(FrameEndEulerManipulator); HelixViewportRight.Children.Remove(FrameStartQuaternionManipulator); HelixViewportRight.Children.Remove(FrameEndQuaternionManipulator); currentPosition = new Position(StartPositionX, StartPositionY, StartPositionZ); currentAngleR = startAngleR; currentAngleP = startAngleP; currentAngleY = startAngleY; currentQuaternion = startQuaternion; SetupCurrentConfiguration(); HelixViewportLeft.Children.Add(frameEuler); HelixViewportRight.Children.Add(frameQuaternion); startTime = DateTime.Now; dispatcherTimer.Start(); return; } timeDelay += DateTime.Now - stopTime; dispatcherTimer.Start(); }
private void GetInterpolatedPositions(Configuration interpolatedConfiguration) { var eulerTransformGroup = new Transform3DGroup(); Position p0, p1, p2, p3, p4, p5; p0 = new Position(0,0,0); p1 = new Position(p0); Vector3D dir, pos; dir = GetZ(eulerTransformGroup); pos = GetPosition(eulerTransformGroup); RotateInDirection(dir, pos, interpolatedConfiguration.A1, ref eulerTransformGroup); dir = GetZ(eulerTransformGroup); TranslateInDirection(dir, components[1].Length, ref eulerTransformGroup); p2 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ); dir = GetY(eulerTransformGroup); pos = GetPosition(eulerTransformGroup); RotateInDirection(dir, pos, interpolatedConfiguration.A2, ref eulerTransformGroup); dir = GetX(eulerTransformGroup); TranslateInDirection(dir, interpolatedConfiguration.Q2, ref eulerTransformGroup); p3 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ); dir = GetY(eulerTransformGroup); pos = GetPosition(eulerTransformGroup); RotateInDirection(dir, pos, interpolatedConfiguration.A3, ref eulerTransformGroup); dir = GetZ(eulerTransformGroup); TranslateInDirection(dir, -components[3].Length, ref eulerTransformGroup); p4 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ); dir = GetZ(eulerTransformGroup); pos = GetPosition(eulerTransformGroup); RotateInDirection(dir, pos, interpolatedConfiguration.A4, ref eulerTransformGroup); dir = GetX(eulerTransformGroup); TranslateInDirection(dir, components[4].Length, ref eulerTransformGroup); p5 = new Position(eulerTransformGroup.Value.OffsetX, eulerTransformGroup.Value.OffsetY, eulerTransformGroup.Value.OffsetZ); dir = GetX(eulerTransformGroup); pos = GetPosition(eulerTransformGroup); RotateInDirection(dir, pos, interpolatedConfiguration.A5, ref eulerTransformGroup); joints[0].Frame.P = p0; joints[1].Frame.P = p1; joints[2].Frame.P = p2; joints[3].Frame.P = p3; joints[4].Frame.P = p4; joints[5].Frame.P = p5; currentFrame.Transform = eulerTransformGroup; }
public void SetupEffectorPosQuat(Position position, Quaternion quaternion) { joints[5].Frame.P = position; var m = converter.BuildMatrix3DFromQuaternion(quaternion); joints[5].Frame.X = new Vector3D(m.M11, m.M12, m.M13); joints[5].Frame.Y = new Vector3D(m.M21, m.M22, m.M23); joints[5].Frame.Z = new Vector3D(m.M31, m.M32, m.M33); joints[5].Frame.X.Normalize(); joints[5].Frame.Y.Normalize(); 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); }
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 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); }
public Position(Position other) { X = other.X; Y = other.Y; Z = other.Z; }
public void CalculateCurrentPosition(ref Position currentPosition, double normalizedTime) { currentPosition.X = StartPositionX + normalizedTime * (EndPositionX - StartPositionX); currentPosition.Y = StartPositionY + normalizedTime * (EndPositionY - StartPositionY); currentPosition.Z = StartPositionZ + normalizedTime * (EndPositionZ - StartPositionZ); }
public Frame(Frame other) { X = new Vector3D(other.X.X, other.X.Y, other.X.Z); Y = new Vector3D(other.Y.X, other.Y.Y, other.Y.Z); Z = new Vector3D(other.Z.X, other.Z.Y, other.Z.Z); P = new Position(other.P); }
public Frame() { X = new Vector3D(1.0f, 0, 0); Y = new Vector3D(0, 1.0f, 0.0f); Z = new Vector3D(0, 0, 1.0f); P = new Position(); }