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