static void changeAleaForce(){ static HDdouble timer = 0; static const hduVector3Dd direction(0, 1, 0); HDdouble instRate; hduVector3Dd force; hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate); timer += 1.0 / instRate; hduVecScale(force, direction, 300 * sin(timer)); }
hduVector3Dd shakeBaby(){ static const hduVector3Dd direction(0, 1, 0); hduVector3Dd force; //current force applied to the arm HDdouble instRate; static HDdouble timer = 0; gVibrationFreq = (HDint) g_speed * 10; gVibrationFreq = gVibrationFreq < 0 ? 0 : gVibrationFreq; gVibrationFreq = gVibrationFreq > 500 ? 500 : gVibrationFreq; /* Use the reciprocal of the instantaneous rate as a timer. */ hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &instRate); timer += 1.0 / instRate; /* Apply a sinusoidal force in the direction of motion. */ hduVecScale(force, direction, gVibrationAmplitude * sin(timer * gVibrationFreq)); return force; }
HDCallbackCode BaseGeometryPatch::patchCalc(){ HDErrorInfo error; hduVector3Dd forceVec; hduVector3Dd targForceVec; hduVector3Dd loopForceVec; hduVector3Dd patchMinusDeviceVec; hduVector3Dd sensorVec; double sensorRadius = 1.0; if(simToPhantom == NULL){ return HD_CALLBACK_CONTINUE; } HHD hHD = hdGetCurrentDevice(); // reset all the variables that need to accumulate for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){ phantomToSim->forceMagnitude[sensi] = 0; phantomToSim->forceVec[sensi][0] = 0; phantomToSim->forceVec[sensi][1] = 0; phantomToSim->forceVec[sensi][2] = 0; } // Begin haptics frame. ( In general, all state-related haptics calls // should be made within a frame. ) hdBeginFrame(hHD); /* Get the current devicePos of the device. */ hdGetDoublev(HD_CURRENT_POSITION, devicePos); hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, deviceAngle); hdGetDoublev(HD_CURRENT_TRANSFORM, transformMat); forceVec[0] = 0; forceVec[1] = 0; forceVec[2] = 0; if(simToPhantom->numTargets > SimToPhantom::MAX_TARGETS) simToPhantom->numTargets = SimToPhantom::MAX_TARGETS; // clear all the data structures if(phantomToSim != NULL){ for(int i = 0; i < PhantomToSim::NUM_SENSORS; ++i){ phantomToSim->forceMagnitude[i] = 0; phantomToSim->forceVec[i][0] = 0; phantomToSim->forceVec[i][1] = 0; phantomToSim->forceVec[i][2] = 0; for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){ phantomToSim->targForceMagnitude[j][i] = 0; phantomToSim->targForceVec[j][i][0] = 0; phantomToSim->targForceVec[j][i][1] = 0; phantomToSim->targForceVec[j][i][2] = 0; } } for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){ phantomToSim->targForcesMagnitude[j] = 0; } } /***/ //printf("BaseGeometryPatch::patchCalc() numtargets = %d\n", simToPhantom->numTargets); for(int targi = 0; targi < simToPhantom->numTargets; ++targi){ patchPos[0] = simToPhantom->targetX[targi]; patchPos[1] = simToPhantom->targetY[targi]; patchPos[2] = simToPhantom->targetZ[targi]; patchRadius = simToPhantom->targetRadius[targi]; targForceVec[0] = 0; targForceVec[1] = 0; targForceVec[2] = 0; for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){ loopForceVec[0] = 0; loopForceVec[1] = 0; loopForceVec[2] = 0; sensorVec[0] = simToPhantom->sensorX[sensi] + devicePos[0]; sensorVec[1] = simToPhantom->sensorY[sensi] + devicePos[1]; sensorVec[2] = simToPhantom->sensorZ[sensi] + devicePos[2]; sensorRadius = simToPhantom->sensorRadius[sensi]; // patchMinusDeviceVec = patchPos-devicePos < // Create a vector from the device devicePos towards the sphere's center. //hduVecSubtract(patchMinusDeviceVec, patchPos, devicePos); hduVecSubtract(patchMinusDeviceVec, patchPos, sensorVec); hduVector3Dd dirVec; hduVecNormalize(dirVec, patchMinusDeviceVec); // If the device position is within the sphere's surface // center, apply a spring forceVec towards the surface. The forceVec // calculation differs from a traditional gravitational body in that the // closer the device is to the center, the less forceVec the well exerts; // the device behaves as if a spring were connected between itself and // the well's center. * double penetrationDist = patchMinusDeviceVec.magnitude() - (patchRadius+sensorRadius); if(penetrationDist < 0) { // > F = k * x < // F: forceVec in Newtons (N) // k: Stiffness of the well (N/mm) // x: Vector from the device endpoint devicePos to the center // of the well. hduVecScale(dirVec, dirVec, penetrationDist); hduVecScale(loopForceVec, dirVec, stiffnessK); /*** for(int i = 0; i < 3; ++i) if(loopForceVec[i] < 0) loopForceVec[i] *= 0.5; printf("targForceVec[%d][%d] = %.2f, %.2f, %.2f\r", targi, sensi, loopForceVec[0], loopForceVec[1], loopForceVec[2]); /****/ } if(phantomToSim != NULL){ phantomToSim->forceMagnitude[sensi] += hduVecMagnitude(loopForceVec); phantomToSim->forceVec[sensi][0] += loopForceVec[0]; phantomToSim->forceVec[sensi][1] += loopForceVec[1]; phantomToSim->forceVec[sensi][2] += loopForceVec[2]; phantomToSim->targForceMagnitude[targi][sensi] = hduVecMagnitude(loopForceVec); phantomToSim->targForceVec[targi][sensi][0] = loopForceVec[0]; phantomToSim->targForceVec[targi][sensi][1] = loopForceVec[1]; phantomToSim->targForceVec[targi][sensi][2] = loopForceVec[2]; } hduVecAdd(targForceVec, targForceVec, loopForceVec); hduVecAdd(forceVec, forceVec, loopForceVec); } if(phantomToSim != NULL){ phantomToSim->targForcesMagnitude[targi] = hduVecMagnitude(targForceVec); } } if(phantomToSim != NULL){ for(int i = 0; i < 16; ++i){ phantomToSim->matrix[i] = transformMat[i]; } } // divide the forceVec the number of times that a force was added? /* Send the forceVec to the device. */ if(simToPhantom->enabled){ hdSetDoublev(HD_CURRENT_FORCE, forceVec); } /* End haptics frame. */ hdEndFrame(hHD); /* Check for errors and abort the callback if a scheduler error is detected. */ if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "BaseGeometryPatch.calcPatch():\n"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } /* Signify that the callback should continue running, i.e. that it will be called again the next scheduler tick. */ return HD_CALLBACK_CONTINUE; }
/******************************************************************************* Servo callback. Called every servo loop tick. Simulates a gravity well, which sucks the device towards its center whenever the device is within a certain range. *******************************************************************************/ HDCallbackCode HDCALLBACK gravityWellCallback(void *data) { //const HDdouble kStiffness = 0.075; /* N/mm */ const HDdouble kStiffness = 0.045; /* N/mm */ const HDdouble kGravityWellInfluence = 400; /* mm */ /* This is the position of the gravity well in cartesian (i.e. x,y,z) space. */ static const hduVector3Dd wellPos = {0,0,0}; hduVector3Dd wellPos2 = {0.0, 0.0, 0.0}; HDErrorInfo error; hduVector3Dd position; hduVector3Dd force; hduVector3Dd positionTwell; HHD hHD = hdGetCurrentDevice(); /* Begin haptics frame. ( In general, all state-related haptics calls should be made within a frame. ) */ WaitForSingleObject( hIOMutex, INFINITE ); hdBeginFrame(hHD); /* Get the current position of the device. */ hdGetDoublev(HD_CURRENT_POSITION, position); memset(force, 0, sizeof(hduVector3Dd)); /* > positionTwell = wellPos-position < Create a vector from the device position towards the gravity well's center. */ count++; //wellPos2[0] = (count%5000)/100; wellPos2[1] = 20*sin((count)*6.28/360); hduVecSubtract(positionTwell, wellPos2, position); /* If the device position is within some distance of the gravity well's center, apply a spring force towards gravity well's center. The force calculation differs from a traditional gravitational body in that the closer the device is to the center, the less force the well exerts; the device behaves as if a spring were connected between itself and the well's center. */ if (hduVecMagnitude(positionTwell) < kGravityWellInfluence) { /* > F = k * x < F: Force in Newtons (N) k: Stiffness of the well (N/mm) x: Vector from the device endpoint position to the center of the well. */ hduVecScale(force, positionTwell, kStiffness); } /* Send the force to the device. */ hdSetDoublev(HD_CURRENT_FORCE, force); /* Get data for logging */ hdGetDoublev(HD_CURRENT_POSITION, global_position); hdGetDoublev(HD_CURRENT_FORCE, global_force); hdGetDoublev(HD_CURRENT_JOINT_ANGLES, global_joint_angles); hdGetDoublev(HD_CURRENT_JOINT_TORQUE, global_joint_torque); /* End haptics frame. */ hdEndFrame(hHD); ReleaseMutex( hIOMutex ); /* Check for errors and abort the callback if a scheduler error is detected. */ if (HD_DEVICE_ERROR(error = hdGetError())) { hduPrintError(stderr, &error, "Error detected while rendering gravity well\n"); if (hduIsSchedulerError(&error)) { return HD_CALLBACK_DONE; } } /* Signify that the callback should continue running, i.e. that it will be called again the next scheduler tick. */ return HD_CALLBACK_CONTINUE; }