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