Ejemplo n.º 1
0
        // 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);
        }
Ejemplo n.º 2
0
        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));
        }