private OptiTrackSystem InitializeOptiTrackSystem()
{
OptiTrackSystem optiTrack = new OptiTrackSystem();
optiTrack.Initialize();
return(optiTrack);
}
public CalibrationTool(OptiTrackSystem optiTrack)
{
this.optiTrack = optiTrack;
worker = new BackgroundWorker()
{
WorkerSupportsCancellation = true
};
optiTrackPoints = new List <Vector <double> >();
kukaRobotPoints = new List <Vector <double> >();
calibrationPoints = new List <Vector <double> >();
Transformation transformation = null;
worker.DoWork += (s, e) => {
Start?.Invoke();
// Move robot to first calibration point and wait
//MoveRobotToPoint(robot, calibrationPoints[0], robot.Limits.MaxVelocity.XYZ / 3.0);
robot.ForceMoveTo(new RobotVector(calibrationPoints[0], robot.Position.ABC), RobotVector.Zero, 15.0);
for (int i = 0; i < calibrationPoints.Count; i++)
{
// Move robot to next calibration point and wait
//MoveRobotToPoint(robot, calibrationPoints[i], robot.Limits.MaxVelocity.XYZ / 3.0);
robot.ForceMoveTo(new RobotVector(calibrationPoints[i], robot.Position.ABC), RobotVector.Zero, 4.0);
// Add robot XYZ position to list
var kukaPoint = robot.Position.XYZ;
kukaRobotPoints.Add(kukaPoint);
// Gen n samples from optitrack system and add average ball position to the list
var optiTrackPoint = optiTrack.GetAveragePosition(samplesPerPoint);
optiTrackPoints.Add(optiTrackPoint);
// Calculate new transformation
int progress = i * 100 / (calibrationPoints.Count - 1);
transformation = new Transformation(optiTrackPoints, kukaRobotPoints);
ProgressChanged?.Invoke(progress, transformation);
}
//MoveRobotToPoint(robot, robot.HomePosition.XYZ, robot.Limits.MaxVelocity.XYZ / 3.0);
//robot.ForceMoveTo(robot.HomePosition, 6.0);
};
worker.RunWorkerCompleted += (s, e) => {
if (e.Error != null)
{
throw e.Error;
}
else
{
Completed?.Invoke(transformation);
}
};
}
public OptiTrackPanel()
{
InitializeComponent();
InitializeCharts();
OptiTrack = new OptiTrackSystem();
OptiTrack.Initialized += (s, e) => {
Dispatcher.Invoke(() => {
hostApp.Text = OptiTrack.ServerDescription.HostApp;
hostName.Text = OptiTrack.ServerDescription.HostComputerName;
hostAdress.Text = OptiTrack.ServerDescription.HostComputerAddress.ToString();
natnetVersion.Text = OptiTrack.ServerDescription.NatNetVersion.ToString();
connectBtn.IsEnabled = false;
disconnectBtn.IsEnabled = true;
});
OptiTrack.FrameReceived += UpdateOptiTrackUI;
if (robot1Transformation != null)
{
OptiTrack.FrameReceived += UpdateRobot1UI;
}
if (robot2Transformation != null)
{
OptiTrack.FrameReceived += UpdateRobot2UI;
}
};
OptiTrack.Uninitialized += (s, e) => {
Dispatcher.Invoke(() => {
connectBtn.IsEnabled = true;
disconnectBtn.IsEnabled = false;
});
};
connectBtn.Click += Connect;
disconnectBtn.Click += Disconnect;
freezeBtn.Click += FreezeCharts;
fitToDataBtn.Click += FitChartsToData;
resetZoomBtn.Click += ResetChartsZoom;
screenshotBtn.Click += TakeChartScreenshot;
activeChart = positionChart;
tabControl.SelectionChanged += (s, e) => {
activeChart = (LiveChart)tabControl.SelectedContent;
};
// CTRL + S -> save active chart to png image
Loaded += (s, e) => Focus();
KeyDown += (s, e) => {
if (e.Key == Key.S && Keyboard.IsKeyDown(Key.LeftCtrl))
{
TakeChartScreenshot(null, null);
}
};
}
public CommandArgs(Dictionary<string, ICommand> registeredCommands, CommandLineWindow cmdWindow,
KUKARobot robot1, KUKARobot robot2, OptiTrackSystem optiTrack, string[] userArgs) {
RegisteredCommands = registeredCommands;
CommandLine = cmdWindow;
Robot1 = robot1;
Robot2 = robot2;
OptiTrack = optiTrack;
UserArgs = userArgs;
}
public CalibrationWindow(OptiTrackSystem optiTrack, KUKARobot robot1, KUKARobot robot2)
{
calibrationTool = new CalibrationTool(optiTrack);
InitializeComponent();
Text = title;
MinimumSize = new Size(Width, Height);
MaximumSize = new Size(Width, Height);
BindingList <KUKARobot> robotsList = new BindingList <KUKARobot>();
robotSelect.DropDown += (s, e) => {
if (robotsList.Contains(robot1))
{
if (!robot1.IsInitialized())
{
robotsList.Remove(robot1);
}
}
else
{
if (robot1.IsInitialized())
{
robotsList.Add(robot1);
}
}
if (robotsList.Contains(robot2))
{
if (!robot2.IsInitialized())
{
robotsList.Remove(robot2);
}
}
else
{
if (robot2.IsInitialized())
{
robotsList.Add(robot2);
}
}
};
robotSelect.TextChanged += (s, e) => {
selectedRobot = (KUKARobot)robotSelect.SelectedItem;
if (selectedRobot != null)
{
startBtn.Enabled = true;
}
else
{
startBtn.Enabled = false;
}
};
robotSelect.DataSource = robotsList;
robotSelect.Text = "- Select robot -";
FormClosing += (s, e) => calibrationTool.Cancel();
startBtn.Click += (s, e) => {
calibrationTool.Calibrate(selectedRobot, (int)intermediatePoints.Value, (int)samplesPerPoint.Value);
};
stopBtn.Click += (s, e) => {
Text = title;
robotSelect.Enabled = true;
startBtn.Enabled = true;
calibrationTool.Cancel();
};
calibrationTool.Start += () => {
UpdateUI(() => {
Text = $"{title} (0%)";
robotSelect.Enabled = false;
startBtn.Enabled = false;
});
};
calibrationTool.ProgressChanged += (progress, transformation) => {
selectedRobot.OptiTrackTransformation = transformation;
UpdateUI(() => {
Text = $"{title} ({progress}%)";
progressBar.Value = progress;
m11.Text = transformation[0, 0].ToString("F3");
m12.Text = transformation[0, 1].ToString("F3");
m13.Text = transformation[0, 2].ToString("F3");
m14.Text = transformation[0, 3].ToString("F3");
m21.Text = transformation[1, 0].ToString("F3");
m22.Text = transformation[1, 1].ToString("F3");
m23.Text = transformation[1, 2].ToString("F3");
m24.Text = transformation[1, 3].ToString("F3");
m31.Text = transformation[2, 0].ToString("F3");
m32.Text = transformation[2, 1].ToString("F3");
m33.Text = transformation[2, 2].ToString("F3");
m34.Text = transformation[2, 3].ToString("F3");
m41.Text = transformation[3, 0].ToString("F3");
m42.Text = transformation[3, 1].ToString("F3");
m43.Text = transformation[3, 2].ToString("F3");
m44.Text = transformation[3, 3].ToString("F3");
});
};
calibrationTool.Completed += (transformation) => {
selectedRobot.OptiTrackTransformation = transformation;
UpdateUI(() => {
robotSelect.Enabled = true;
startBtn.Enabled = true;
});
};
}
public CalibrationTool(Robot robot, OptiTrackSystem optiTrack)
{
this.robot = robot;
this.optiTrack = optiTrack;
worker = new BackgroundWorker()
{
WorkerSupportsCancellation = true
};
optiTrackPoints = new List <Vector <double> >();
kukaRobotPoints = new List <Vector <double> >();
calibrationPoints = new List <Vector <double> >();
worker.DoWork += (s, e) => {
Start?.Invoke();
// Move robot to first calibration point and wait
MoveRobotToCalibrationPoint(calibrationPoints[0], 100);
for (int i = 0; i < calibrationPoints.Count; i++)
{
if (worker.CancellationPending)
{
break;
}
// Move robot to the next calibration point and wait
MoveRobotToCalibrationPoint(calibrationPoints[i], 150);
// Add robot XYZ position to list
var kukaPoint = robot.Position.XYZ;
kukaRobotPoints.Add(kukaPoint);
// Gen n samples from optitrack system and add average ball position to the list
var optiTrackFrames = optiTrack.WaitForFrames(samplesPerPoint);
var optiTrackPoint = Vector <double> .Build.Dense(3);
optiTrackFrames.ForEach(frame => {
optiTrackPoint += frame.BallPosition;
});
optiTrackPoints.Add(optiTrackPoint / optiTrackFrames.Count);
// Calculate new transformation
int progress = i * 100 / (calibrationPoints.Count - 1);
var transformation = new Transformation(optiTrackPoints, kukaRobotPoints);
ProgressChanged?.Invoke(progress, transformation);
}
};
worker.RunWorkerCompleted += (s, e) => {
if (e.Error != null)
{
throw e.Error;
}
else
{
Completed?.Invoke();
}
};
}
public CalibrationWindow(Robot robot, OptiTrackSystem optiTrack)
{
InitializeComponent();
Transformation currentTransformation = null;
calibrationTool = new CalibrationTool(robot, optiTrack);
calibrationTool.Start += () => {
Dispatcher.Invoke(() => {
progressBar.Value = 0;
progressBarLabel.Content = "0%";
startBtn.IsEnabled = false;
cancelBtn.IsEnabled = true;
});
};
calibrationTool.ProgressChanged += (progress, transformation) => {
currentTransformation = transformation;
Dispatcher.Invoke(() => {
progressBar.Value = progress;
progressBarLabel.Content = $"{progress}%";
t00.Text = transformation[0, 0].ToString("F3");
t01.Text = transformation[0, 1].ToString("F3");
t02.Text = transformation[0, 2].ToString("F3");
t03.Text = transformation[0, 3].ToString("F3");
t10.Text = transformation[1, 0].ToString("F3");
t11.Text = transformation[1, 1].ToString("F3");
t12.Text = transformation[1, 2].ToString("F3");
t13.Text = transformation[1, 3].ToString("F3");
t20.Text = transformation[2, 0].ToString("F3");
t21.Text = transformation[2, 1].ToString("F3");
t22.Text = transformation[2, 2].ToString("F3");
t23.Text = transformation[2, 3].ToString("F3");
t30.Text = transformation[3, 0].ToString("F3");
t31.Text = transformation[3, 1].ToString("F3");
t32.Text = transformation[3, 2].ToString("F3");
t33.Text = transformation[3, 3].ToString("F3");
});
};
calibrationTool.Completed += () => {
Completed?.Invoke(currentTransformation);
Close();
};
startBtn.Click += (s, e) => {
try {
calibrationTool.Calibrate(10, 200);
} catch (Exception ex) {
MainWindow.ShowErrorDialog("Unable to start calibration.", ex);
}
};
cancelBtn.Click += (s, e) => {
calibrationTool.Cancel();
};
Closing += (s, e) => {
calibrationTool.Cancel();
};
robotIpAdress.Content += robot.ToString();
}
public MainWindow()
{
InitializeComponent();
mainWindowHanlde = this;
// Set timers resolution to 1ms (default resolution is 15.6ms)
WinApi.TimeBeginPeriod(1);
// Force change number separator to dot
CultureInfo culuteInfo = new CultureInfo("en-US");
culuteInfo.NumberFormat.NumberDecimalSeparator = ".";
Thread.CurrentThread.CurrentCulture = culuteInfo;
Thread.CurrentThread.CurrentUICulture = Thread.CurrentThread.CurrentCulture;
Loaded += (s, e) => {
robot1Panel.MainWindowHandle = this;
robot1Panel.OptiTrack = optiTrackPanel.OptiTrack;
robot2Panel.MainWindowHandle = this;
robot2Panel.OptiTrack = optiTrackPanel.OptiTrack;
try {
robot1Panel.LoadConfig(Path.Combine(App.ConfigDir, "robot1.config.json"));
} catch (Exception) {
// Ignore exception
}
try {
robot2Panel.LoadConfig(Path.Combine(App.ConfigDir, "robot2.config.json"));
} catch (Exception) {
// Ignore exception
}
Robot robot1 = robot1Panel.Robot;
Robot robot2 = robot2Panel.Robot;
OptiTrackSystem optiTrack = optiTrackPanel.OptiTrack;
optiTrackPanel.Initialize(robot1, robot2);
pingPanel.Initialize(robot1, robot2, optiTrack);
robot1.ErrorOccured += (sender, args) => {
robot1Panel.ForceFreezeCharts();
robot2Panel.ForceFreezeCharts();
optiTrackPanel.ForceFreezeCharts();
pingPanel.ForceFreezeCharts();
ShowErrorDialog($"An exception was raised on the robot ({args.RobotIp}) thread.", args.Exception);
};
robot2.ErrorOccured += (sender, args) => {
robot1Panel.ForceFreezeCharts();
robot2Panel.ForceFreezeCharts();
optiTrackPanel.ForceFreezeCharts();
pingPanel.ForceFreezeCharts();
ShowErrorDialog($"An exception was raised on the robot ({args.RobotIp}) thread.", args.Exception);
};
pingPanel.Started += () => {
robot1Panel.DisableUIUpdates();
robot2Panel.DisableUIUpdates();
optiTrackPanel.DisableUIUpdates();
};
pingPanel.Stopped += () => {
robot1Panel.EnableUIUpdates();
robot2Panel.EnableUIUpdates();
optiTrackPanel.EnableUIUpdates();
};
};
// Shrink window if it's too wide or too high
double windowWidth = Width;
double windowHeight = Height;
double screenWidth = SystemParameters.PrimaryScreenWidth;
double screenHeight = SystemParameters.PrimaryScreenHeight;
if (windowWidth >= screenWidth)
{
Width = MinWidth = screenWidth - 100;
}
if (windowHeight >= screenHeight)
{
Height = MinHeight = screenHeight - 100;
}
// Robots configuration files directory
Directory.CreateDirectory(App.ConfigDir);
// Chart screenshots directory
Directory.CreateDirectory(App.ScreenshotsDir);
}
public CORTester(OptiTrackSystem optiTrack)
{
InitializeComponent();
this.optiTrack = optiTrack;
startBtn.Click += (s, e) => optiTrack.FrameReceived += ProcessFrame;
clearBtn.Click += (s, e) => {
zValues.Clear();
chart.Series[0].Points.Clear();
chart.Annotations.Clear();
};
calculateBtn.Click += (s, e) => {
int from = (int)fromSampleInput.Value;
int to = (int)toSampleInput.Value;
var peeks = FindPeeks(from, to);
var CORs = new List <double>();
for (int i = 0; i < peeks.Count; i++)
{
if (i >= 1)
{
CORs.Add(Math.Sqrt(peeks[i] / peeks[i - 1]));
}
HorizontalLineAnnotation ann = new HorizontalLineAnnotation {
Y = peeks[i],
AxisY = chart.ChartAreas[0].AxisY,
AxisX = chart.ChartAreas[0].AxisX,
LineColor = Color.Red,
LineWidth = 1,
LineDashStyle = ChartDashStyle.Dash,
IsInfinitive = true
};
chart.Annotations.Add(ann);
}
double averageCOR = 0.0;
for (int i = 0; i < CORs.Count; i++)
{
averageCOR += CORs[i];
}
averageCOR /= Math.Max(CORs.Count, 1.0);
averageCORText.Text = averageCOR.ToString("F3");
};
fromSampleInput.ValueChanged += (s, e) => chart.ChartAreas[0].AxisX.Minimum = (int)fromSampleInput.Value;
toSampleInput.ValueChanged += (s, e) => chart.ChartAreas[0].AxisX.Maximum = (int)toSampleInput.Value;
int bounces = 6;
double cor = 0.8;
double zOnHit = 0.0;
double z0 = 1;
double t = 0.04;
double z = 1;
double g = 9.81;
int sample = 0;
Random rand = new Random();
while (z > zOnHit)
{
z = z0 - g * t * t / 2.0;
t += 0.004;
chart.Series[0].Points.AddXY(sample, z * 1000 + rand.NextDouble() * 32 - 16);
zValues.Add(z);
sample++;
}
double v = -g * t;
for (int i = 0; i < bounces; i++)
{
double v0 = Math.Abs(v * cor);
t = 0.004;
z = zOnHit + v0 * t - t * t * g / 2.0;
v = v0 - g * t;
while (z > zOnHit)
{
z = zOnHit + v0 * t - t * t * g / 2.0;
v = v0 - g * t;
t += 0.004;
chart.Series[0].Points.AddXY(sample, z * 1000 + rand.NextDouble() * 32 - 16);
zValues.Add(z);
sample++;
}
}
chart.ChartAreas[0].AxisX.Interval = zValues.Count / 20;
var peekss = FindPeeks(0, sample);
var CORss = new List <double>();
for (int i = 0; i < peekss.Count; i++)
{
HorizontalLineAnnotation ann = new HorizontalLineAnnotation {
Y = peekss[i] * 1000,
AxisY = chart.ChartAreas[0].AxisY,
AxisX = chart.ChartAreas[0].AxisX,
LineColor = Color.Red,
LineWidth = 1,
LineDashStyle = ChartDashStyle.Dash,
IsInfinitive = true
};
chart.Annotations.Add(ann);
}
}
public void Initialize(Robot robot1, Robot robot2, OptiTrackSystem optiTrack)
{
if (robot1 == robot2)
{
throw new ArgumentException("Cos tam ze instancje robota musza byc rozne");
}
this.robot1 = robot1;
this.robot2 = robot2;
this.optiTrack = optiTrack;
robot1PingApp = new PingApp(robot1, optiTrack, (ballPosition) => {
return(ballPosition[0] < 1000.0 && ballPosition[2] > 600.0);
});
bool checkboxChecked = false;
copyMovementsCheck.Checked += (s, e) => {
checkboxChecked = (bool)copyMovementsCheck.IsChecked;
};
robot1PingApp.DataReady += UpdateUI;
robot1PingApp.Started += (s, e) => {
Dispatcher.Invoke(() => {
startBtn.IsEnabled = false;
stopBtn.IsEnabled = true;
copyMovementsCheck.IsEnabled = false;
setBounceHeightBtn.IsEnabled = true;
setXRegBtn.IsEnabled = true;
setYRegBtn.IsEnabled = true;
});
if (checkboxChecked)
{
robot2.MovementChanged += CopyMovements;
}
Started?.Invoke();
};
robot1PingApp.Stopped += (s, e) => {
Dispatcher.Invoke(() => {
startBtn.IsEnabled = true;
stopBtn.IsEnabled = false;
copyMovementsCheck.IsEnabled = true;
setBounceHeightBtn.IsEnabled = false;
setXRegBtn.IsEnabled = false;
setYRegBtn.IsEnabled = false;
});
robot2.MovementChanged -= CopyMovements;
optiTrack.Uninitialize();
robot1.Uninitialize();
robot2.Uninitialize();
Stopped?.Invoke();
};
bool startBtnEnabled = false;
startBtn.Click += (s, e) => startBtnEnabled = startBtn.IsEnabled;
robot1.Initialized += (s, e) => {
if (!startBtnEnabled)
{
if (checkboxChecked)
{
if (robot2.IsInitialized())
{
robot1PingApp.Start();
}
}
else
{
robot1PingApp.Start();
}
}
};
robot2.Initialized += (s, e) => {
if (!startBtnEnabled)
{
if (checkboxChecked)
{
if (robot1.IsInitialized())
{
robot1PingApp.Start();
}
}
}
};
}
