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