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();
        }
Exemple #2
0
        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;
        }
Exemple #3
0
 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();
 }
Exemple #4
0
        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);
        }
Exemple #5
0
 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();
 }
Exemple #6
0
        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);
        }
Exemple #7
0
 public Position(Position other)
 {
     X = other.X;
     Y = other.Y;
     Z = other.Z;
 }
Exemple #8
0
 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);
 }
Exemple #9
0
 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);
 }
Exemple #10
0
 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();
 }