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