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