protected override void OnDoWork(DoWorkEventArgs e) { DateTime start = new DateTime(); DateTime stop = new DateTime(); while (dotracking) { start = DateTime.Now; if (trackForceSensor) { if (!errorStateActive && UniversalRobotHelperFunctions.IsForceOverLimit()) { errorStateActive = true; Vector3D forceDirection = UniversalRobotHelperFunctions.GetForceDirection(); UniversalRobotController.URRobotCoOrdinate currentRobotCoord = App.Current.URController.GetCurrentLocation(); Point3D currentCamSetPoint = new Point3D( currentRobotCoord.x, currentRobotCoord.y, currentRobotCoord.z); Vector3D lookDirection = new Vector3D( currentRobotCoord.qx, currentRobotCoord.qy, currentRobotCoord.qz ); Point3D safePoint = UniversalRobotHelperFunctions.AdjustLocationToForceSensor(currentCamSetPoint, lookDirection, forceDirection); App.Current.URController.SetVirtualEStopOverride(true, safePoint.X, safePoint.Y, safePoint.Z); } } stop = DateTime.Now; Thread.Sleep(20); } }
// Implemented RPY UniversalRobotController.URRobotCoOrdinate ConvertRigidBodyIntoURSpace(UniversalRobotController.URRobotCoOrdinate polaris) { //positive x is toward the camera //positive y is to the right of the camera (camera pov) //camera z is out toward the robot //z -950 at closent limit to cam, 2400 is outer limit //camera x is up negative up //camera y is left and right positive right //on rigid bodies ndi must be on top //camera y positive is left and right RigidBodyDeltas converted = new RigidBodyDeltas(); converted.xDelta = (polaris.z / 1000); converted.yDelta = (polaris.y / 1000); converted.zDelta = (polaris.x / 1000);// ((-polaris.xDelta) / 1000); RPY_RigidBodyDeltas rpy_converted = new RPY_RigidBodyDeltas(); rpy_converted.rDelta = (polaris.qz); rpy_converted.pDelta = (polaris.qy); rpy_converted.yDelta = (polaris.qx); UniversalRobotController.URRobotCoOrdinate convertedDelta = new UniversalRobotController.URRobotCoOrdinate(); convertedDelta.x = converted.xDelta; convertedDelta.y = converted.yDelta; convertedDelta.z = converted.zDelta; convertedDelta.qx = rpy_converted.rDelta; convertedDelta.qy = rpy_converted.pDelta; convertedDelta.qz = rpy_converted.yDelta; return(convertedDelta); }
//Implemented RPY to check the tolerance (TODO) bool IsWithinRangeForLazerTracking(UniversalRobotController.URRobotCoOrdinate lastDeltas, UniversalRobotController.URRobotCoOrdinate currentDeltas) { double tolerance = 0.0002; if (Math.Abs(Math.Abs(currentDeltas.x) - Math.Abs(lastDeltas.x)) < tolerance && Math.Abs(Math.Abs(currentDeltas.y) - Math.Abs(lastDeltas.y)) < tolerance && Math.Abs(Math.Abs(currentDeltas.z) - Math.Abs(lastDeltas.z)) < tolerance && Math.Abs(Math.Abs(currentDeltas.qx) - Math.Abs(lastDeltas.qx)) < tolerance && Math.Abs(Math.Abs(currentDeltas.qy) - Math.Abs(lastDeltas.qy)) < tolerance && Math.Abs(Math.Abs(currentDeltas.qz) - Math.Abs(lastDeltas.qz)) < tolerance) { return(true); } return(false); }
// TODO: Implement for qx, qy and qz after defining setPoint properly bool HasRobotReachedCameraSetpoint(UniversalRobotController.URRobotCoOrdinate currentRobotCoord, Point3D setPoint, Point3D rpy_setPoint) { double tolerance = 0.0002; if (Math.Abs(Math.Abs(currentRobotCoord.x) - Math.Abs(setPoint.X)) < tolerance && Math.Abs(Math.Abs(currentRobotCoord.y) - Math.Abs(setPoint.Y)) < tolerance && Math.Abs(Math.Abs(currentRobotCoord.z) - Math.Abs(setPoint.Z)) < tolerance && Math.Abs(Math.Abs(currentRobotCoord.qx) - Math.Abs(rpy_setPoint.X)) < tolerance && Math.Abs(Math.Abs(currentRobotCoord.qy) - Math.Abs(rpy_setPoint.Y)) < tolerance && Math.Abs(Math.Abs(currentRobotCoord.qz) - Math.Abs(rpy_setPoint.Z)) < tolerance) { return(true); } return(false); }
void SetCameraSetpoints() { rigidBodySetpoints = new PolarisCameraController.PolarisRigidBody(); if (currentRigidBody == RigidBodyOptions.One) { rigidBodySetpoints = polarisController.GetUserRigidBodyOne(); } else if (currentRigidBody == RigidBodyOptions.Two) { rigidBodySetpoints = polarisController.GetUserRigidBodyTwo(); } universalRobotCameraSetpoint = urController.GetCurrentLocation(); hasDeltaSetpointsBeenSet = true; }
UniversalRobotController.URRobotCoOrdinate GetRigidBodyDeltas() { RigidBodyDeltas delta = new RigidBodyDeltas(); RPY_RigidBodyDeltas RPYdelta = new RPY_RigidBodyDeltas(); Vector3D rpycurrentlocations = new Vector3D(); Vector3D rpyrigidbodysetpoints = new Vector3D(); UniversalRobotController.URRobotCoOrdinate convertedDelta = new UniversalRobotController.URRobotCoOrdinate(); PolarisCameraController.PolarisRigidBody currentLocations = new PolarisCameraController.PolarisRigidBody(); if (currentRigidBody == RigidBodyOptions.One) { currentLocations = polarisController.GetUserRigidBodyOne(); } else if (currentRigidBody == RigidBodyOptions.Two) { currentLocations = polarisController.GetUserRigidBodyTwo(); } Console.WriteLine("x:" + currentLocations.x.ToString() + ", y:" + currentLocations.y.ToString() + ", z:" + currentLocations.z.ToString() + ", avg: " + currentLocations.numberOfAverages.ToString()); delta.xDelta = currentLocations.x - rigidBodySetpoints.x; delta.yDelta = currentLocations.y - rigidBodySetpoints.y; delta.zDelta = currentLocations.z - rigidBodySetpoints.z; //Converting current location and rigibody set points from quartenion to rpy rpycurrentlocations = coordTrans.QuaternionToRPY(currentLocations.qo, currentLocations.qx, currentLocations.qy, currentLocations.qz); rpyrigidbodysetpoints = coordTrans.QuaternionToRPY(rigidBodySetpoints.qo, rigidBodySetpoints.qx, rigidBodySetpoints.qy, rigidBodySetpoints.qz); //Calculating RPYdelta RPYdelta.rDelta = rpycurrentlocations.X - rpyrigidbodysetpoints.X; RPYdelta.pDelta = rpycurrentlocations.Y - rpyrigidbodysetpoints.Y; RPYdelta.yDelta = rpycurrentlocations.Z - rpyrigidbodysetpoints.Z; convertedDelta.x = delta.xDelta; convertedDelta.y = delta.yDelta; convertedDelta.z = delta.zDelta; convertedDelta.qx = RPYdelta.rDelta; convertedDelta.qy = RPYdelta.pDelta; convertedDelta.qz = RPYdelta.yDelta; return(ConvertRigidBodyIntoURSpace(convertedDelta)); }
public TrackWindow() { InitializeComponent(); this.DataContext = this; robotTrack = new Controls.TrackCameraWithRobot( App.Current.URController, App.Current.URSecondController, App.Current.PolarisController, App.Current.ApplicationSettings, App.Current.CoordinateTranslator, App.Current.TorqueSensorTracking, _AccelerationSpeed ); robotStartLocation = App.Current.URController.GetCurrentLocation(); RobotStartLocationString = robotStartLocation.ToString(); //TODO: Get position of marker }
private void UiTimer_Tick(object sender, EventArgs e) { this.Dispatcher.BeginInvoke(DispatcherPriority.Normal, (Action)(() => { Controls.UniversalRobotController.URRobotCoOrdinate current = App.Current.URController.GetCurrentLocation(); double xOffset = robotTrack.CurrentSetPoint.x - current.x; double yOffset = robotTrack.CurrentSetPoint.y - current.y; xOffset = Math.Round(xOffset, 4); yOffset = Math.Round(yOffset, 4); xOffset *= 1000; yOffset *= 1000; TargetFollower.Margin = new Thickness( centerPixelOffset + xOffset, centerPixelOffset + yOffset, 0, 0); Controls.UniversalRobotController.URRobotCoOrdinate deltaRobot = robotStartLocation - current; DeltaRobotLocationString = deltaRobot.ToString(); })); }