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