public void EqualTest()
{
var robotVector1 = new RobotVector(ZeroZero(), Heading.North);
var robotVector2 = new RobotVector(ZeroZero(), Heading.North);
Assert.IsTrue(robotVector1 == robotVector2);
}
private void UpdateUI(object sender, KUKA.FrameReceivedEventArgs args)
{
RobotVector actualPosition = args.Position;
RobotVector targetPosition = Robot.TargetPosition;
RobotVector theoreticalPosition = Robot.TheoreticalPosition;
positionChart.Update(new double[] {
actualPosition.X, targetPosition.X, theoreticalPosition.X,
actualPosition.Y, targetPosition.Y, theoreticalPosition.Y,
actualPosition.Z, targetPosition.Z, theoreticalPosition.Z,
actualPosition.A, targetPosition.A, theoreticalPosition.A,
actualPosition.B, targetPosition.B, theoreticalPosition.B,
actualPosition.C, targetPosition.C, theoreticalPosition.C,
});
Dispatcher.Invoke(() => {
actualPositionX.Text = actualPosition.X.ToString("F3");
actualPositionY.Text = actualPosition.Y.ToString("F3");
actualPositionZ.Text = actualPosition.Z.ToString("F3");
actualPositionA.Text = actualPosition.A.ToString("F3");
actualPositionB.Text = actualPosition.B.ToString("F3");
actualPositionC.Text = actualPosition.C.ToString("F3");
targetPositionX.Text = targetPosition.X.ToString("F3");
targetPositionY.Text = targetPosition.Y.ToString("F3");
targetPositionZ.Text = targetPosition.Z.ToString("F3");
targetPositionA.Text = targetPosition.A.ToString("F3");
targetPositionB.Text = targetPosition.B.ToString("F3");
targetPositionC.Text = targetPosition.C.ToString("F3");
});
velocityChart.Update(Robot.Velocity.ToArray());
accelerationChart.Update(Robot.Acceleration.ToArray());
}
public void CanParseAStartingLocation(string startingLocation)
{
var locationParser = new LocationParser();
RobotVector location = locationParser.Parse(startingLocation);
Assert.That(location.ToString(), Is.EqualTo(startingLocation));
}
public void NotSameHeadingTests()
{
var robotVector1 = new RobotVector(ZeroZero(), Heading.North);
var robotVector2 = new RobotVector(ZeroZero(), Heading.South);
Assert.IsTrue(robotVector1 != robotVector2);
}
public void CanReadBackVectorValues()
{
var position = new Position(0, 0);
var heading = Heading.North;
var robotVector = new RobotVector(position, heading);
Assert.That(robotVector.Position, Is.EqualTo(position));
Assert.That(robotVector.Heading, Is.EqualTo(heading));
}
public void MoveForwardWhenFacingNorth(int startXCord, int startYCord)
{
const Heading startingHeading = Heading.North;
var command = new MoveForwards();
RobotVector result = command.GenerateNewVector(new RobotVector(new Position(startXCord, startYCord), startingHeading));
AssertHasOnlyMovedNorth(startXCord, startYCord, result, startingHeading);
}
public void TurnRightWhenFacingNorth(int startXCord, int startYCord)
{
const Heading startingHeading = Heading.North;
var command = new TurnRight();
RobotVector result = command.GenerateNewVector(new RobotVector(new Position(startXCord, startYCord), startingHeading));
DirectionAssertions.AssertHasTurnedToFaceEast(startXCord, startYCord, result);
}
public void TurnLeftWhenFacingWest(int startXCord, int startYCord)
{
const Heading startingHeading = Heading.West;
var command = new TurnLeft();
RobotVector result = command.GenerateNewVector(new RobotVector(new Position(startXCord, startYCord), startingHeading));
DirectionAssertions.AssertHasTurnedToFaceSouth(startXCord, startYCord, result);
}
public void StartAt(RobotVector staringPosition)
{
if (_arena == null)
{
throw new ArenaNotUploadedException();
}
_currentPosition = staringPosition;
_startLocationSet = true;
}
public void ExecuteCommandList(IEnumerable <RobotCommand> commandList)
{
if (!_startLocationSet)
{
throw new StartLocationNotSetException();
}
foreach (var robotCommand in commandList)
{
_currentPosition = robotCommand.GenerateNewVector(_currentPosition);
}
}
public void CanReportARobotsPosition()
{
var startPosition = SimpleVector();
var robot = new WarriorRobot();
robot.UploadArena(new Arena(1, 1));
robot.StartAt(startPosition);
RobotVector reportedPosition = robot.ReportPosition();
Assert.That(reportedPosition, Is.EqualTo(startPosition));
}
private void ResetMoveToFields(object sender, RoutedEventArgs e)
{
if (robot.IsInitialized())
{
RobotVector actualPosition = robot.Position;
moveToX.Text = actualPosition.X.ToString();
moveToY.Text = actualPosition.Y.ToString();
moveToZ.Text = actualPosition.Z.ToString();
moveToA.Text = actualPosition.A.ToString();
moveToB.Text = actualPosition.B.ToString();
moveToC.Text = actualPosition.C.ToString();
}
}
public override RobotVector GenerateNewVector(RobotVector currentPosition)
{
if (currentPosition.Heading == Heading.North)
{
return(new RobotVector(currentPosition.Position + new YDimensionUnit(1), Heading.North));
}
if (currentPosition.Heading == Heading.East)
{
return(new RobotVector(currentPosition.Position + new XDimensionUnit(1), Heading.East));
}
if (currentPosition.Heading == Heading.West)
{
return(new RobotVector(currentPosition.Position - new XDimensionUnit(1), Heading.West));
}
return(new RobotVector(currentPosition.Position - new YDimensionUnit(1), Heading.South));
}
public Ping_JuggleVertically(KUKARobot robot, Chart chart)
{
this.robot = robot;
this.chart = chart;
robot.FrameReceived += frame => {
if (robotMoved && robot.IsTargetPositionReached)
{
var robotPos = new RobotVector(robot.Position.X, robot.Position.Y, 290);
robot.MoveTo(robotPos, RobotVector.Zero, 1.5);
polyfitX.Values.Clear();
polyfitY.Values.Clear();
polyfitZ.Values.Clear();
timeOf3Pred.Clear();
}
};
}
public void ExecutingASingleCommandUpdatesThePositionAsExpected()
{
var startPosition = new RobotVector(new Position(0, 0), Heading.North);
var robot = new WarriorRobot();
robot.UploadArena(new Arena(1, 1));
robot.StartAt(startPosition);
var commandList = new List <RobotCommand>();
commandList.Add(new MoveForwards());
robot.ExecuteCommandList(commandList);
RobotVector reportedPosition = robot.ReportPosition();
Assert.That(startPosition, Is.Not.EqualTo(reportedPosition));
}
private void MoveTo(object sender, RoutedEventArgs e)
{
try {
double x = double.Parse(moveToX.Text);
double y = double.Parse(moveToY.Text);
double z = double.Parse(moveToZ.Text);
double a = double.Parse(moveToA.Text);
double b = double.Parse(moveToB.Text);
double c = double.Parse(moveToC.Text);
double movementTime = Math.Max(double.Parse(moveToTime.Text), 2);
moveToTime.Text = movementTime.ToString();
RobotVector targetPosition = new RobotVector(x, y, z, a, b, c);
robot.MoveTo(targetPosition, RobotVector.Zero, movementTime);
} catch (Exception ex) {
MainWindow.ShowErrorDialog("Unable to move robot to specified target position.", ex);
}
}
private void CopyMovements(object sender, MovementChangedEventArgs args)
{
RobotMovement[] movementsStack = new RobotMovement[args.MovementsStack.Length];
for (int i = 0; i < args.MovementsStack.Length; i++)
{
RobotMovement movement = args.MovementsStack[i];
RobotVector tPos = movement.TargetPosition;
RobotVector tVel = movement.TargetVelocity;
movementsStack[i] = new RobotMovement(
targetPosition: new RobotVector(tPos.Y, tPos.X / 2.0, tPos.Z, robot2.HomePosition.ABC),
targetVelocity: new RobotVector(0, 0, tVel.Z),
targetDuration: movement.TargetDuration
);
}
robot2.MoveTo(movementsStack);
}
private void Shift(object sender, RoutedEventArgs e)
{
try {
double x = double.Parse(shiftX.Text);
double y = double.Parse(shiftY.Text);
double z = double.Parse(shiftZ.Text);
double a = double.Parse(shiftA.Text);
double b = double.Parse(shiftB.Text);
double c = double.Parse(shiftC.Text);
double movementTime = Math.Max(double.Parse(shiftTime.Text), 0.5);
shiftTime.Text = movementTime.ToString();
RobotVector deltaPosition = new RobotVector(x, y, z, a, b, c);
robot.Shift(deltaPosition, RobotVector.Zero, movementTime);
ResetShiftFields(null, null);
} catch (Exception ex) {
MainWindow.ShowErrorDialog("Unable to shift robot by specified delta position.", ex);
}
}
private void Test()
{
RobotVector current = new RobotVector();
RobotVector target = new RobotVector(0, 0, 0);
var gen = new TrajectoryGenerator5v2(current);
Random rand = new Random();
gen.SetTargetPosition(current, target, new RobotVector(150, 0, 0), 3.0);
Task.Run(() => {
for (int i = 0; i < 2000; i++)
{
current = gen.GetNextValue(current);
if (!gen.IsTargetPositionReached)
{
//current += new RobotVector(rand.NextDouble() * 0.05, 0, 0);
}
//threadSafeChart1.AddPoint(current.X, gen.Velocity.X);
Thread.Sleep(4);
}
});
}
public void Execute(CommandArgs args)
{
if (args.UserArgs.Length != 3)
{
args.CommandLine.Error($"3 arguments expected, get {args.UserArgs.Length}");
return;
}
KUKARobot robot;
switch (args.UserArgs[0])
{
case "robot1":
robot = args.Robot1;
break;
case "robot2":
robot = args.Robot2;
break;
default:
args.CommandLine.Error($"'robot1' or 'robot2' expected, get '{args.UserArgs[0]}'");
return;
}
if (robot == null || !robot.IsInitialized())
{
args.CommandLine.Error($"Selected robot is not initialized");
return;
}
string[] positionSplit = args.UserArgs[1].Trim().Split(',');
RobotVector targetPosition;
if (positionSplit.Length == 3)
{
if (double.TryParse(positionSplit[0], out double x) &&
double.TryParse(positionSplit[1], out double y) &&
double.TryParse(positionSplit[2], out double z))
{
targetPosition = new RobotVector(x, y, z, robot.Position.ABC);
}
else
{
args.CommandLine.Error($"Invalid XYZ position");
return;
}
}
else if (positionSplit.Length == 6)
{
if (double.TryParse(positionSplit[0], out double x) &&
double.TryParse(positionSplit[1], out double y) &&
double.TryParse(positionSplit[2], out double z) &&
double.TryParse(positionSplit[3], out double a) &&
double.TryParse(positionSplit[4], out double b) &&
double.TryParse(positionSplit[5], out double c))
{
targetPosition = new RobotVector(x, y, z, a, b, c);
}
else
{
args.CommandLine.Error($"Invalid XYZ position or ABC angles");
return;
}
}
else
{
args.CommandLine.Error($"(x, y, z) or (x, y, z, a, b, c) position expected, get ({args.UserArgs[1]})");
return;
}
if (double.TryParse(args.UserArgs[2], out double movementDuration))
{
robot.MoveTo(targetPosition, RobotVector.Zero, movementDuration);
}
else
{
args.CommandLine.Error($"Invalid movement duration");
}
}
//Resharper generated
public bool Equals(RobotVector other)
{
return(_position.Equals(other._position) && _heading == other._heading);
}
public RobotMovement(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration)
{
TargetPosition = targetPosition;
TargetVelocity = targetVelocity;
TargetDuration = targetDuration;
}
private static void AssertHasOnlyMovedWest(int startXCord, int startYCord, RobotVector result, Heading startingHeading)
{
Assert.That(result.Position.YCord, Is.EqualTo(startYCord));
Assert.That(result.Position.XCord, Is.EqualTo(startXCord - 1));
Assert.That(result.Heading, Is.EqualTo(startingHeading));
}
public void ProcessOptiTrackData(OptiTrack.InputFrame data)
{
// Pozycja przekonwertowana z układu optitracka do układu odpowiedniej KUKI
var position = robot.OptiTrackTransformation.Convert(data.Position);
double ballX = position[0];
double ballY = position[1];
double ballZ = position[2];
if (ballZ < 0)
{
ballFell = true;
}
// Zamiast zabawy w te ify trzeba ogarnac LabeledMarkers w optitracku zeby wykryc kiedy dokladnie widzimy pileczke a kiedy nie
if (!ballFell && ballX < 1200 && ballX != 791.016 && ballY != 743.144 && ballZ != 148.319)
{
Console.WriteLine("cc");
if (polyfitZ.Values.Count == maxPolyfitPoints)
{
for (int i = 0; i < maxPolyfitPoints / 2; i++)
{
polyfitX.Values[i] = polyfitX.Values[2 * i];
polyfitY.Values[i] = polyfitY.Values[2 * i];
polyfitZ.Values[i] = polyfitZ.Values[2 * i];
}
polyfitX.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2);
polyfitY.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2);
polyfitZ.Values.RemoveRange(maxPolyfitPoints / 2, maxPolyfitPoints / 2);
}
polyfitX.AddPoint(elapsedTime, ballX);
polyfitY.AddPoint(elapsedTime, ballY);
polyfitZ.AddPoint(elapsedTime, ballZ);
var xCoeffs = polyfitX.CalculateCoefficients();
var yCoeffs = polyfitY.CalculateCoefficients();
var zCoeffs = polyfitZ.CalculateCoefficients();
var roots = QuadraticSolver.SolveReal(zCoeffs[2], zCoeffs[1], zCoeffs[0] - zPositionAtHit);
elapsedTime += data.FrameDeltaTime;
if (roots.Length != 2)
{
// No real roots
return;
}
double T = roots[1];
chart.AddPoint(T, T);
var ballTargetVelocity = Vector <double> .Build.DenseOfArray(new double[] { xCoeffs[1], yCoeffs[1], 2 * zCoeffs[2] * T + zCoeffs[1] });
Vector <double> paddleNormal = Normalize(reflectionVector) - Normalize(ballTargetVelocity);
if (IsTimeStable(T) && polyfitX.Values.Count >= 10)
{
double timeToHit = T - elapsedTime;
double predX = xCoeffs[1] * T + xCoeffs[0];
double predY = yCoeffs[1] * T + yCoeffs[0];
Console.WriteLine("T: " + T + " X: " + predX + " Y: " + predY);
if (!ballHit && timeToHit >= 0.05) // 0.1 DO SPRAWDZENIA!
{
double angleB = Math.Atan2(paddleNormal[0], paddleNormal[2]) * 180.0 / Math.PI;
double angleC = -90.0 - Math.Atan2(paddleNormal[1], paddleNormal[2]) * 180.0 / Math.PI;
RobotVector predictedHitPosition = new RobotVector(
predX, predY, zPositionAtHit, 0, angleB, angleC
);
//if (robot.Limits.WorkspaceLimits.CheckPosition(predictedHitPosition)) {
// //predkosc na osiach w [mm/s]
// // RobotVector velocity = new RobotVector(0, 0, 0);
// // Dla odwaznych:
// RobotVector velocity = new RobotVector(0, 0, 150);
// robot.MoveTo(predictedHitPosition, velocity, timeToHit);
// robotMoved = true;
//}
}
}
}
}
public override RobotVector GenerateNewVector(RobotVector currentPosition)
{
var newHeading = (currentPosition.Heading.GetHashCode() + 90) % 360;
return(new RobotVector(currentPosition.Position, (Heading)newHeading));
}
public void FormatsPositionCorrectly()
{
var vector = new RobotVector(new Position(1, 1), Heading.North);
Assert.That(vector.ToString(), Is.EqualTo("1 1 N"));
}
public void ToStringFormatsDirectionCorrectly(Heading heading, string expected)
{
var vector = new RobotVector(ZeroZero(), heading);
Assert.That(vector.ToString(), Is.EqualTo(expected));
}
public void ProcessOptiTrackData(OptiTrack.InputFrame data)
{
if (ballFell)
{
return;
}
if (ballSpotted)
{
elapsedTime += data.FrameDeltaTime;
}
var ballPosition = robot.OptiTrackTransformation.Convert(data.Position);
if (ballPosition[0] < 1200.0 &&
(ballPosition[0] != prevBallPosition[0] ||
ballPosition[1] != prevBallPosition[1] ||
ballPosition[2] != prevBallPosition[2])
)
{
ballSpotted = true;
if (ballPosition[2] <= ballFellHeight)
{
ballFell = true;
return;
}
polyfitX.AddPoint(elapsedTime, ballPosition[0]);
polyfitY.AddPoint(elapsedTime, ballPosition[1]);
polyfitZ.AddPoint(elapsedTime, ballPosition[2]);
if (polyfitZ.Values.Count == maxPolyfitPoints)
{
ShiftPolyfitPoints();
}
double T = CalculatePredictedTimeOfFlight();
chart.AddPoint(T, T);
if (!ballHit && T > 0.1 && IsPredictedTimeStable(T)) // 0.1 mozna zwiekszyc - do przetestowania
{
double timeToHit = T - elapsedTime;
if (timeToHit >= 0.05)
{
var xCoeffs = polyfitX.CalculateCoefficients();
var yCoeffs = polyfitY.CalculateCoefficients();
RobotVector predictedHitPosition = new RobotVector(
xCoeffs[1] * T + xCoeffs[0], // predicted x
yCoeffs[1] * T + yCoeffs[0], // predicted y
targetHitHeight,
robot.HomePosition.ABC
);
Console.WriteLine(predictedHitPosition);
if (robot.Limits.WorkspaceLimits.CheckPosition(predictedHitPosition))
{
robotMoved = true;
RobotVector velocity = new RobotVector(0, 0, 0);
//robot.MoveTo(predictedHitPosition, velocity, timeToHit);
}
}
}
}
prevBallPosition = ballPosition;
}
public static void AssertHasTurnedToFaceNorth(int startXCord, int startYCord, RobotVector result)
{
Assert.That(result.Position.YCord, Is.EqualTo(startYCord));
Assert.That(result.Position.XCord, Is.EqualTo(startXCord));
Assert.That(result.Heading, Is.EqualTo(Heading.North));
}
public abstract RobotVector GenerateNewVector(RobotVector currentPosition);
