Switched out the old, glued together stack of sensors for a set of c-section parts that allow pressure on the sensor to be independent of the speaker. They keep falling off though.
Trying now with more glue and cure time. I also need to get some double-stick tape.
More glue worked!
Modified the code so that multiple targets can exist and experimented with turning forces off.
For the first time, all the important pieces are working together.
I added force interaction between gripper and target sphere, then determined how to make the ratio calculation work. If the sum of all of the magnitudes on the target is greater than zero, then the ratio equals the magnitude of the sum of the force vectors divided by the sum of each magnitude. A value of 1.0 means that there is no contact. A value of 0.0 is a perfectly opposing contact. As currently implemented, if the ratio is less than 0.5, then the position of the target sphere is set to the point that lies between the two grippers, otherwise the (summed) contact force vector is applied to the target.
After firing up the sim and trying it, the sense of “capture” works well and is intuitive.
Here’s the code that matters. First, the haptic code, then the sim code. In both cases, simToPhantom and phantomToSim are the structs that are used by the shared memory system:
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();
/* 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;
for(int targi = 0; targi < SimToPhantom::NUM_TARGETS; ++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;
sensorRadius = simToPhantom->sensorRadius[sensi];
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];
if(sensorVec[0] == 0.0){
continue;
}
/* > 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);
}
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(forceVec, forceVec, loopForceVec);
hduVecAdd(targForceVec, targForceVec, loopForceVec);
}
if(phantomToSim != NULL){
phantomToSim->targForcesMagnitude[targi] = hduVecMagnitude(targForceVec);
}
}
// divide the forceVec the number of times that a force was added?
/* Send the forceVec to the device. */
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;
}
}
if(phantomToSim != NULL){
for(int i = 0; i < 16; ++i){
phantomToSim->matrix[i] = transformMat[i];
}
}
/* Signify that the callback should continue running, i.e. that
it will be called again the next scheduler tick. */
return HD_CALLBACK_CONTINUE;
}
Next, the Sim code:
void TargetSphere::environmentCalc(){
double forces = phantomToSim->targForcesMagnitude[targIndex];
double sumVec[3];
double sumMag = 0;
for(int i = 0; i < 3; ++i){
constraintAnchorPos[i] = 0;
sumVec[i] = 0;
}
for(int i = 0; i < SimToPhantom::NUM_SENSORS; ++i){
constraintAnchorPos[0] += sensorPos[i][0];
constraintAnchorPos[1] += sensorPos[i][1];
constraintAnchorPos[2] += sensorPos[i][2];
double force = phantomToSim->targForceMagnitude[targIndex][i];
sumMag += force;
double forceVec[3];
forceVec[0] = phantomToSim->targForceVec[targIndex][i][0];
forceVec[1] = phantomToSim->targForceVec[targIndex][i][1];
forceVec[2] = phantomToSim->targForceVec[targIndex][i][2];
sumVec[0] += forceVec[0];
sumVec[1] += forceVec[1];
sumVec[2] += forceVec[2];
//Dprint::add("targ[%d] sens[%d] forceVec = (%.2f., %.2f, %.2f) force = %.2f, forces = %.2f",
// targIndex, i, forceVec[0], forceVec[1], forceVec[2], force, forces);
}
constraintAnchorPos[0] = constraintAnchorPos[0]/SimToPhantom::NUM_SENSORS;
constraintAnchorPos[1] = constraintAnchorPos[1]/SimToPhantom::NUM_SENSORS;
constraintAnchorPos[2] = constraintAnchorPos[2]/SimToPhantom::NUM_SENSORS;
//double mag = m3dGetMagnitude3(sumVec);
//Dprint::add("sumVec (%.2f., %.2f, %.2f) Mag = %.2f, sumForce = %.2f", sumVec[0], sumVec[1], sumVec[2], mag, forces);
double ratio = 1.0;
if(sumMag >0){
ratio = forces/sumMag;
if(ratio > 0.5){
double velocityScalar = 1.0; // should be time-based
for(int i = 0; i < 3; ++i){
velocityVec[i] += (-sumVec[i])*velocityScalar;
position[i] += velocityVec[i];
if(velocityVec[i] > 0.1){
velocityVec[i] -= 0.1;
}else{
velocityVec[i] = 0;
}
}
}else{
for(int i = 0; i < 3; ++i){
velocityVec[i] = 0;
position[i] = constraintAnchorPos[i];
}
}
}
Dprint::add("sumMag = %.2f, sumForce = %.2f, ratio = %.2f", sumMag, forces, ratio);
}
Got the wiring cleaned up.
Integrating collision response with the targetSphere. The math is looking reasonably good.
Added multi-target capability.
Added adjustable sensitivity to the pressure sensors. The pushing directly on the speakers is causing artifacts. I think I need to build small c-section angle that decouples the squeezing force from the Vibroacoustic feedback.
Working on constraint code. Got the framework done, but didn’t have enough sleep to be able to do the math involved. So instead I…
Got the actuators mounted on the Phantom! Aside from having one of the force sensors break during mounting, it went pretty smoothly. I may have to adjust the sensitivity of teh sensors so that you don’t have to press so hard on them. At the current setting the voice coils aren’t behaving at higher grip forces. But the ergonomics feel pretty good, so that’s nice.
Wow, the title sounds like a laundry list 🙂
Building a two-fingered gripper
Going to add sound class to SimpleSphere so that we know what sounds are coming from what collision. Didn’t do that’ but I’m associating the sounds by index, which is good enough for now
Need to calculate individual forces for each sphere in the Phantom and return them. Done.
To keep the oscillations at a minimum, I’m passing the offsets from the origin. That way the loop uses the device position as the basis for calculations within the haptic loop.
Here’s the result of today’s work:
Integrating all the pieces into one test platform. The test could be to move a collection of physically-based spheres (easy collision detect) from one area to another. Time would be recorded from the indication of a start and stop (spacebar, something in the sim, etc). Variations would be:
Probably only use two actuators for the simplicity of the test rig. It would bean that I could use the laptop’s headphone output. Need to test this by wiring up the actuators to a micro stereo plug. Radio Shack tonight.
Got two-way communication running between Phantom and sim.
Have force magnitude adjusting a volume.
Added a SimpleSphere class for most of the testing.
Had an idea to fix a messy bug.
Searched for “MSVC shared memory”
Got a useful hit in the MSDN database.
Implemented in a test loop. Worked the first time
Implemented in the project code. Worked the first time.
I think the world is about to end.
Today’s job is to integrate the Phantom code into the simulation.
Spent most of the day trying to figure out how to deal with geometry that has to be available to both the haptic and graphics subsystems. The haptics subsystem has to run fast – about 1000hz and gets its own callback-based loop from the HD haptic libraries. The graphics run as fast as they can, but they get bogged down.
So the idea for the day was to structure the code so that a stable geometry patch can be downloaded from the main system to the haptics subsystem. I’m thinking that they could be really simple, maybe just a plane and a concave/convex surface. I started by creating a BaseGeometryPatch class that takes care of all the basic setup and implements a sphere patch model. Other inheriting classes simple override the patchCalc() method and everything should work just fine.
I also built a really simple test main loop that runs at various rates using Sleep(). The sphere is nice and stable regardless of the main loop update rate, though the transitions as the position is updated can be a little sudden. It may make sense to add some interpolation rather than just jumping to the next position. But it works. The next thing will be to make the sphere work as a convex shape by providing either a flag or using a negative length. Once that’s done (with a possible detour into interpolation), I’ll try adding it to the graphics code. In the meanwhile, here’s a video of a dancing Phantom for your viewing pleasure:
I think I know what the vibroacoustic study should be. I put an actuator on the Phantom and drive wav files based on the material associated with the collision. I can use the built-in haptic pattern playback as a control. To make the wav files, it might be as simple as recording the word, or using a microphone to contact a material, move across it and lift off (personally, I like this because it mimics what could be done with telepresence. The use of multiple sensor/actuator pairs can be used in a later study.
Which means that I don’t actually need the Phidgets code in the new KF hand codebase. I’m going to include it anyway, simple because I’m so close and can use it later.
Come to think of it, I could put an actuator on a mouse as well and move over materials?
Tasks for today:
Phlog 