void CybPhantom::updateState(int _flag)
{
states.flagState = _flag;
hdScheduleSynchronous(getStateCB, &states, HD_MIN_SCHEDULER_PRIORITY);
}
HDboolean CybPhantom::CheckCalibration()
{
HDErrorInfo error;
HDenum status = GetCalibrationStatus(&stateCalibration);
if(status == HD_CALIBRATION_OK)
{
return HD_TRUE;
}
else if (status == HD_CALIBRATION_NEEDS_MANUAL_INPUT)
{
printf("Calibration requires manual input...\n");
return HD_FALSE;
}
else if (status == HD_CALIBRATION_NEEDS_UPDATE)
{
hdScheduleSynchronous(UpdateCalibrationCallback, &stateCalibration,HD_DEFAULT_SCHEDULER_PRIORITY);
if(HD_DEVICE_ERROR(error = hdGetError()))
{
printf("\nFailed to update Calibration!\n");
return HD_FALSE;
}
else
cout<<"Calibration sucessfully"<<endl;
}
else
{
assert(!"Unknow Calibration status");
return HD_FALSE;
}
}
// boucle de manipulation ??
void startManipulation(){
ghHD = HD_INVALID_HANDLE;
gSchedulerCallback = HD_INVALID_HANDLE;
bool returnValue = true;
int devideID = 0;
HDErrorInfo error;
// Initialize the device. This needs to be called before any actions on the
// device.
ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
hdEnable(HD_FORCE_OUTPUT);
hdEnable(HD_MAX_FORCE_CLAMPING);
// initialize amplitude for vibration
// TODO
// mise en attente du device
hdScheduleSynchronous(startManipulationCallBack, 0,
HD_DEFAULT_SCHEDULER_PRIORITY);
// synchronization du scheduer avec les "actions" ???
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
returnValue = false;
atexit(exit);
}
/*******************************************************************************
Graphics main loop function.
*******************************************************************************/
void displayFunction(void)
{
// Setup model transformations.
glMatrixMode(GL_MODELVIEW);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
setupGraphicsState();
drawAxes(sphereRadius*3.0);
// Draw the fixed sphere.
static const hduVector3Dd fixedSpherePosition(0, 0, 0);
static const float fixedSphereColor[4] = {.2, .8, .8, .8};
GLUquadricObj* pQuadObj = gluNewQuadric();
drawSphere(pQuadObj, fixedSpherePosition, fixedSphereColor, sphereRadius);
// Get the current position of end effector.
DeviceDisplayState state;
hdScheduleSynchronous(DeviceStateCallback, &state,
HD_MIN_SCHEDULER_PRIORITY);
// Draw a sphere to represent the haptic cursor and the dynamic
// charge.
static const float dynamicSphereColor[4] = { .8, .2, .2, .8 };
drawSphere(pQuadObj,
state.position,
dynamicSphereColor,
sphereRadius);
// Create the force vector.
hduVector3Dd forceVector = 400.0 * forceField(state.position);
drawForceVector(pQuadObj,
state.position,
forceVector,
sphereRadius*.1);
gluDeleteQuadric(pQuadObj);
glPopMatrix();
glutSwapBuffers();
}
void HapticDevice::run()
{
hdScheduleSynchronous(sScheduleOut, &m_hss, HD_DEFAULT_SCHEDULER_PRIORITY);
for(unsigned int i=0;i<m_nbDevices;i++)
{
if(m_constraints[i] != NULL)
{
btTransform myTrans;
btVector3 origin((HDfloat)m_hss[i].m_free.m_position[0],(HDfloat)m_hss[i].m_free.m_position[1],(HDfloat)m_hss[i].m_free.m_position[2]-OFFSET_TO_CAMERA);
origin*=SCALE_DEVICE_TO_WORLD;
m_cameraViews[i]->setBasis(m_cameraViews[i]->getBasis().transpose());
btMatrix3x3 basis;
btScalar m[16];
for(int j=0;j<16;j++)
m[j] = (float) m_hss[i].m_free.m_transform[j];
basis.setFromOpenGLSubMatrix(m);
//put device position/orientation into camera/world referencial
myTrans.setOrigin(origin);
myTrans.setBasis(basis);
myTrans.mult(*(m_cameraViews[i]),myTrans);
btTransform offset(btMatrix3x3::getIdentity(),btVector3(0,0,0.5));
m_effectors[i].setOrigin(origin);
m_effectors[i].setBasis(basis);
m_effectors[i].mult(m_effectors[i],offset);
m_effectors[i].mult(*m_cameraViews[i],m_effectors[i]);
//correction cursor orientation
m_effectors[i].getBasis()*=btMatrix3x3(1,0,0, 0,-1,0, 0,0,-1);
m_constraints[i]->getFrameOffsetA() = myTrans;
//if(m_itsConstraints[i]!=NULL)
// ((btGeneric6DofConstraint*)m_itsConstraints[i])->getFrameOffsetA() = myTrans;
}
}
}
void HapticDevice::feedback(btDynamicsWorld &dynamic)
{
for(unsigned int i=0;i<m_nbDevices;i++)
{
bool ground_collide = false;
// free move
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 )// (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2))
{
m_hss[i].m_free.m_done = true;
//m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
}
if(m_constraints[i] != NULL)
{
btRigidBody * myBody = &m_constraints[i]->getRigidBodyB();
btTransform myTrans = myBody->getWorldTransform();
m_effRenderPos = myTrans.getOrigin();
//Check collision
if(m_constraints[i]->getUserConstraintPtr() != NULL)
{
//std::cout<< " se cas la " <<std::endl;
m_hss[i].m_free.m_nbCollision = 1;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
btCollisionObject * object = static_cast<btCollisionObject *>(m_constraints[i]->getUserConstraintPtr());
if(object->getInternalType()== btCollisionObject::CO_RIGID_BODY)
{
btRigidBody * collideBody = static_cast<btRigidBody *>(object);
// collide with ground
if(collideBody == m_ground)
{
m_hss[i].m_free.m_nbCollision = 2;
ground_collide = true;
}
// collide with other object
if(collideBody->getInvMass()!=0 && collideBody != m_ground)
{
if(m_itsConstraints[i] == NULL )
{
// catch it if colide with it
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) == 0 )
{
//create constraint
btTransform bodyTrans = collideBody->getWorldTransform();
m_itsConstraints[i] = createConstraint(*myBody,*collideBody);
dynamic.addConstraint(m_itsConstraints[i],true);
m_newConstraint(m_ptr,collideBody,i);
m_caught = collideBody;
m_coll = true;
m_devine = false;
showTarget(collideBody);
deactivateMove();
}
}else
// realise it when button 1 pressed
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
//remove constraint
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_deleteConstraint(m_ptr,collideBody,i);
//m_hss[i].setThrown(NULL);
m_hss[i].m_free.m_done = true;
showTarget(collideBody);
m_variator = 0;
m_coll = false;
deactivateMove();
}
}
}
}
else
{
m_hss[i].m_free.m_nbCollision = 0;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
if(m_itsConstraints[i] != NULL )
{
//remove constraint
m_deleteConstraint(m_ptr,&m_itsConstraints[i]->getRigidBodyB(),i);
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_hss[i].m_free.m_done = true;
if(m_caught != NULL)
showTarget(m_caught);
m_coll = false;
deactivateMove();
}
}
}
if(m_itsConstraints[i]!=NULL )
m_hss[i].m_free.m_nbCollision = 1;
else
// launch an other target
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_2) == 0)
{
//m_canLaunch = true;
}
m_oldButtons[i]=m_hss[i].m_free.m_buttons;
//put back cursor world position into device referencial <<<<< ------------------------------
btTransform offset(btMatrix3x3::getIdentity(),btVector3(0,0,-0.5));
m_effectors[i].setOrigin(myTrans.getOrigin());
m_effectors[i].mult(m_effectors[i],offset);
btVector3 pos = m_cameraViews[i]->inverse()(myTrans.getOrigin());
pos*=SCALE_WORLD_TO_DEVICE;
m_hss[i].m_free.m_realPosition.set(pos.getX(),pos.getY(),pos.getZ()+OFFSET_TO_CAMERA);
}
btTransform camInv = m_cameraViews[i]->inverse();
// compute feed back for ground
if(ground_collide)
{
m_hss[i].m_free.m_force = groundForce(true,&(m_hss[i].m_free.m_position),&camInv);
}else
m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
// detecte the direction
HDdouble deplacement = betweenTwoPoints(m_hss[i].m_free.m_atThrowPos,m_hss[i].m_free.m_position);
HDdouble distanceMax = Distance_max;
hduVector3Dd move = m_hss[i].m_free.m_oldPosition - m_hss[i].m_free.m_position;
HDdouble selectedDistance = 0;
if( m_devine && m_thrownRigids != NULL && m_thrownRigids->size()>0 && deplacement > distanceMax && m_sible == 0 /* && !m_targetChoosen */){
HDdouble distance = Quick_Distance_max;
//unsigned int targ = 0;
for(unsigned int j = 0; j<m_thrownRigids->size(); j++){
HDdouble d = distanceToTrajectory(m_hss[i].m_free.m_position,j,&camInv);
if((d)<distance){
if(m_Thrown != (*m_thrownRigids)[j]){
m_hss[i].m_free.m_currentThrown = (*m_thrownRigids)[j];
distance = d;
m_Thrown = (*m_thrownRigids)[j];
m_impactPos = m_possibleImpact[j];
m_targetChoosen = true;
if(m_Feedback){
activateMove();
}
selectedDistance = d;
m_index = j;
m_sible = Nbr_frame_wait;
}
}
}
if(m_targetChoosen){
//showTarget(m_Thrown);
addPrevious(m_Thrown);
}
else
m_sible = 0;
//m_time = Time;
}
if(m_canLaunch){
//m_time = Time;
m_hss[i].m_free.m_done = true;
m_variator = 0.001;
m_sible = 0;
m_devine = true;
deactivateMove();
cleanHistory();
m_hss[i].m_free.m_atThrowPos = m_hss[i].m_free.m_position;
}
m_hss[i].m_free.m_oldPosition = m_hss[i].m_free.m_position ;
if(m_hss[i].m_free.m_currentThrown != NULL && m_posSet && m_Feedback ){
hduVector3Dd impact = invertTransform(m_impactPos, &camInv);
hduVector3Dd pos(m_hss[i].m_free.m_position);
btVector3 balltest = ((*m_thrownRigids)[0])->getWorldTransform().getOrigin();
hduVector3Dd test = invertTransform(&balltest, &camInv);
m_velocity = m_hss[i].m_free.m_velocity.magnitude();
if(pos[2]-5 >= test[2]){
if(!m_hss[i].m_free.m_done){
//hduVector3Dd helpForce = ForceToImpact(&pos,&impact);
hduVector3Dd helpForce = magneticForce(&pos, &impact, &camInv);
//hduVector3Dd helpForce = atomeForce(&pos, &impact, &camInv);
if(!helpForce.isZero(EPSILON)){
hduVector3Dd force = 1.0 * helpForce;
m_hss[i].m_free.m_force = force;
m_Force = force;
}else
{
//m_hss[i].m_free.m_force = m_Force;
}
}
}
}
hdScheduleSynchronous(sScheduleIn, &m_hss, HD_DEFAULT_SCHEDULER_PRIORITY);
m_selectedDistance = selectedDistance;
if(m_sible > 0)
m_sible--;
}
}
/******************************************************************************
Collects statistics about the update rate of the haptic device.
******************************************************************************/
int main(int argc, char* argv[])
{
HDErrorInfo error;
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "Failed to initialize haptic device");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return -1;
}
HDstring model = hdGetString(HD_DEVICE_MODEL_TYPE);
std::cout << "Initialized: " << model << std::endl;
HDSchedulerHandle hServoCallback = hdScheduleAsynchronous(
ServoSchedulerCallback, 0, HD_MAX_SCHEDULER_PRIORITY);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to schedule servoloop callback" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdSetSchedulerRate(TARGET_SERVOLOOP_RATE);
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to set servoloop rate" << std::endl;
hdDisableDevice(hHD);
return -1;
}
hdDisable(HD_FORCE_OUTPUT);
hdStartScheduler();
if (HD_DEVICE_ERROR(error = hdGetError()))
{
std::cerr << error << std::endl;
std::cerr << "Failed to start servoloop" << std::endl;
hdDisableDevice(hHD);
return -1;
}
std::cout << "Printing servoloop rate stats. All numbers are in Hz units" << std::endl;
std::cout << std::endl;
char fileName[256];
sprintf(fileName, "%s Rate Stats.txt", model);
std::ofstream fout(fileName, std::ios::out | std::ios::app);
for (int nRuns = 0; nRuns < 10 && !_kbhit(); nRuns++)
{
Sleep(1000);
hdScheduleSynchronous(CopyUpdateRateStats, 0, HD_MIN_SCHEDULER_PRIORITY);
// Prints some stats about the rate as well as log it to file.
PrintUpdateRateStats(std::cout);
PrintUpdateRateStats(fout);
if (!hdWaitForCompletion(hServoCallback, HD_WAIT_CHECK_STATUS))
{
std::cerr << "Error occurred during main loop" << std::endl;
std::cerr << "Press any key to quit." << std::endl;
getch();
break;
}
}
hdStopScheduler();
hdUnschedule(hServoCallback);
hdDisableDevice(hHD);
return 0;
}
HDenum CybPhantom::GetCalibrationStatus(CalibrationState *pState)
{
hdScheduleSynchronous(calibrationServoLoop, pState, HD_MIN_SCHEDULER_PRIORITY);
return pState->calibrationStatus;
}
/*******************************************************************************
This routine allows the device to provide information about the current
location of the stylus, and contains a mechanism for terminating the
application.
Pressing the button causes the application to display the current location
of the device.
Holding the button down for N iterations causes the application to exit.
*******************************************************************************/
void mainLoop(void)
{
static const int kTerminateCount = 1000;
int buttonHoldCount = 0;
/* Instantiate the structure used to capture data from the device. */
DeviceData currentData;
DeviceData prevData;
/* Perform a synchronous call to copy the most current device state. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData, HD_MIN_SCHEDULER_PRIORITY);
memcpy(&prevData, ¤tData, sizeof(DeviceData));
printHelp();
/* Run the main loop until the gimbal button is held. */
while (1)
{
/* Perform a synchronous call to copy the most current device state.
This synchronous scheduler call ensures that the device state
is obtained in a thread-safe manner. */
hdScheduleSynchronous(copyDeviceDataCallback,
¤tData,
HD_MIN_SCHEDULER_PRIORITY);
/* If the user depresses the gimbal button, display the current
location information. */
if (currentData.m_buttonState && !prevData.m_buttonState)
{
fprintf(stdout, "Current position: (%g, %g, %g)\n",
currentData.m_devicePosition[0],
currentData.m_devicePosition[1],
currentData.m_devicePosition[2]);
}
else if (currentData.m_buttonState && prevData.m_buttonState)
{
/* Keep track of how long the user has been pressing the button.
If this exceeds N ticks, then terminate the application. */
buttonHoldCount++;
if (buttonHoldCount > kTerminateCount)
{
/* Quit, since the user held the button longer than
the terminate count. */
break;
}
}
else if (!currentData.m_buttonState && prevData.m_buttonState)
{
/* Reset the button hold count, since the user stopped holding
down the stylus button. */
buttonHoldCount = 0;
}
/* Check if an error occurred. */
if (HD_DEVICE_ERROR(currentData.m_error))
{
hduPrintError(stderr, ¤tData.m_error, "Device error detected");
if (hduIsSchedulerError(¤tData.m_error))
{
/* Quit, since communication with the device was disrupted. */
fprintf(stderr, "\nPress any key to quit.\n");
getch();
break;
}
}
/* Store off the current data for the next loop. */
memcpy(&prevData, ¤tData, sizeof(DeviceData));
}
}
/******************************************************************************
Main loop.
Detects and interprets keypresses. Monitors and initiates error recovery if
necessary.
******************************************************************************/
void mainLoop()
{
HDdouble stiffness = gSpringStiffness;
int keypress;
printf("\nPress button and drag to feel anchored spring force.\n");
printf("Press +/- to modify spring stiffness or press Q to quit.\n\n");
printf("Spring stiffness: %.3f N/mm\n", stiffness);
while (1==1)
{
/* Check for keyboard input. */
if (_kbhit())
{
keypress = getch();
keypress = toupper(keypress);
if (keypress == 'Q')
{
return;
}
else if (keypress == '+' || keypress == '=')
{
stiffness += gStiffnessIncrement;
if (stiffness > gMaxStiffness)
stiffness = gMaxStiffness;
/* Make a thread-safe call to change the spring stiffness
being used for rendering forces. */
hdScheduleSynchronous(SetSpringStiffnessCallback, &stiffness,
HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Spring stiffness: %.3f N/mm\n", stiffness);
}
else if (keypress == '-' || keypress == '_')
{
stiffness -= gStiffnessIncrement;
if (stiffness < 0)
stiffness = 0;
/* Make a thread-safe call to change the spring stiffness
being used for rendering forces. */
hdScheduleSynchronous(SetSpringStiffnessCallback, &stiffness,
HD_DEFAULT_SCHEDULER_PRIORITY);
printf("Spring stiffness: %.3f N/mm\n", stiffness);
}
else
{
printf("\nInvalid keypress\n");
printf("Press +/- to modify spring stiffness or press Q to quit.\n\n");
}
}
/* Check if the main scheduler callback has exited. */
if (!hdWaitForCompletion(gCallbackHandle, HD_WAIT_CHECK_STATUS))
{
fprintf(stderr, "\nThe main scheduler callback has exited\n");
fprintf(stderr, "\nPress any key to quit.\n");
getch();
return;
}
}
}
