//=========================================================================== void cShapeMatching3dofPointer::updatePose() { cVector3d pos, vel; // check if device is available if (m_device == NULL) { return; } // read device position m_device->getPosition(pos); // adjust for tool workspace scale factor m_deviceLocalPos = m_workspaceScaleFactor * pos; // update global position of tool cVector3d tPos; m_globalRot.mulr(m_deviceLocalPos, tPos); tPos.addr(m_globalPos, m_deviceGlobalPos); // read device orientation m_device->getRotation(m_deviceLocalRot); // update global orientation of tool m_deviceLocalRot.mulr(m_globalRot, m_deviceGlobalRot); // read switches m_userSwitch0 = getUserSwitch(0); // read velocity of the device in local coordinates m_device->getLinearVelocity(vel); // adjust for tool workspace scale factor m_deviceLocalVel = m_workspaceScaleFactor * vel; // update global velocity of tool m_globalRot.mulr(m_deviceLocalVel, m_deviceGlobalVel); }
//=========================================================================== int cDeltaDevice::command(int a_command, void* a_data) { // temp variables int result = CHAI_MSG_OK; double x=0.0,y=0.0,z=0.0; // check if device is open if (m_systemReady) { switch (a_command) { // read position of delta device case CHAI_CMD_GET_POS_3D: { // read position from device dhdGetPosition(&x, &y, &z, m_deviceID); cVector3d* position = (cVector3d *) a_data; // Convert from m to mm position->mul(1000.0); position->set(x, y, z); } break; // read normalized position of the delta device case CHAI_CMD_GET_POS_NORM_3D: { // read position from device dhdGetPosition(&x, &y, &z, m_deviceID); // normalize position cVector3d* position = (cVector3d *) a_data; position->set(x, y, z); position->div(m_halfSizeWorkspace); } break; // read orientation angles of delta wrist case CHAI_CMD_GET_ROT_ANGLES: { cVector3d* angles = (cVector3d *) a_data; angles->set(0, 0, 0); dhdGetOrientationRad(&angles->x, &angles->y, &angles->z, m_deviceID); } break; // read orientation matrix of wrist case CHAI_CMD_GET_ROT_MATRIX: { cMatrix3d frame; frame.identity(); switch (m_deviceType) { // delta devices case (DHD_DEVICE_3DOF): case (DHD_DEVICE_6DOF): case (DHD_DEVICE_6DOF_500): { // read angles cVector3d angles; angles.set(0,0,0); dhdGetOrientationRad(&angles.x, &angles.y, &angles.z, m_deviceID); // compute rotation matrix angles.mul(1.5); frame.rotate(cVector3d(1,0,0), angles.x); frame.rotate(cVector3d(0,1,0), angles.y); frame.rotate(cVector3d(0,0,1), angles.z); } break; // omega devices case (DHD_DEVICE_OMEGA): case (DHD_DEVICE_OMEGA3): case (DHD_DEVICE_OMEGA33): case (DHD_DEVICE_OMEGA331): { // read rotation matrix double rot[3][3]; rot[0][0] = 1.0; rot[0][1] = 0.0; rot[0][2] = 0.0; rot[1][0] = 0.0; rot[1][1] = 1.0; rot[1][2] = 0.0; rot[2][0] = 0.0; rot[2][1] = 0.0; rot[2][2] = 1.0; dhdGetOrientationFrame(rot, m_deviceID); cMatrix3d result; frame.m[0][0] = rot[0][0]; frame.m[0][1] = rot[0][1]; frame.m[0][2] = rot[0][2]; frame.m[1][0] = rot[1][0]; frame.m[1][1] = rot[1][1]; frame.m[1][2] = rot[1][2]; frame.m[2][0] = rot[2][0]; frame.m[2][1] = rot[2][1]; frame.m[2][2] = rot[2][2]; } break; } // return result cMatrix3d* matrix = (cMatrix3d *) a_data; *matrix = frame; } break; // set force to device case CHAI_CMD_SET_FORCE_3D: { cVector3d* force = (cVector3d *) a_data; dhdSetForce(force->x, force->y, force->z, m_deviceID); } break; // set torque to device case CHAI_CMD_SET_TORQUE_3D: { cVector3d* torque = (cVector3d *) a_data; dhdSetTorque(torque->x, torque->y, torque->z, m_deviceID); } break; // read user switch from wrist case CHAI_CMD_GET_SWITCH_0: { int* result = (int *) a_data; *result = getUserSwitch(m_deviceID); } break; // read user switch from wrist case CHAI_CMD_GET_SWITCH_MASK: { int* result = (int *) a_data; // Force the result to be 0 or 1, since bit 0 should carry button 0's value *result = getUserSwitch(m_deviceID) ? 1 : 0; } break; // read scale factor from normalized coords to mm case CHAI_CMD_GET_NORMALIZED_SCALE_FACTOR: { double* scale = (double*)a_data; // Multiply by m_halfSizeWorkspace to get meters back *scale = m_halfSizeWorkspace; // Then multiply by 1000 to get millimeters *scale *= 1000.0; result = CHAI_MSG_OK; } break; // function is not implemented for delta devices default: result = CHAI_MSG_NOT_IMPLEMENTED; } } else { result = CHAI_MSG_SYSTEM_NOT_READY; } return (result); }