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