# Life can be a drag sometimes

• Cleaning up commands. Mostly done
• While testing the “test” part of the app, I’m realizing that my “ratio” calculations have some issues. Before tying to fix them directly, I’m going to try just making a different gripper that has three “sensor spheres” on each finger. Then I can just let my “drag-based” physics to the whole job. Finished. That’s much better
• Quick! What’s wrong with the following code?
```	for(int i = 0; i < 3; ++i){
position[i] += velocityVec[i];
if(velocityVec[i] > drag*ratio){
velocityVec[i] -= drag*ratio;
}else{
velocityVec[i] = 0;
}
}```
• Yep, the drag is only being applied for objects moving in a positive direction. This is a problem that has been driving me crazy for days. I thought is was some artifact of the communication between the Phantom control loop (1k hz) and the simulation loop (100 hz). Nope. Simple math mistake. Facepalm.
• Pretty picture for the day. Notice that the grippers now have multiple points of contact:

• I’ve also started to notice how feedback changes the speed that you can perform the task. Haptic and tactor seem pretty close. Open loop is much worse, at least subjectively. Let’s see what the data says.

# Achilles continues to chase the tortoise

Just plugging along today. Slow but good progress:

• Switching between test types.
• Since session is now integrated in the testing, I need eliminate session switches
• Need to pull the “general” logic out of the “simulation” command processing.

# Zeno’s development schedule

Ok, it’s not as bad as that paradox, but the amount of work remaining always grows when you get closer to the end. Still, I think I’ll be ready by the end of the week.

• Goal Box – done. I had to add collision detection for determining if a TargetSphere was touching (for setup) or inside (achieving the goal). I basically wrote an axis-aligned bounding box where the radius of the TargetSphere was either added (inside) to or subtracted from (touching) the size of the GoalBox. I’m not calculating penetration, just looking for sign change on the line segment.
• Added <map> of UI_cmd to handle commands coming from the control system and results coming from the sim. Worked right the first time. Yay, C++ templates!
• Enable/disable haptics/tactors – done. It’s interesting to see how the behavior and feel of the system changes with the different capabilities enabled/disabled.
• Started working on the experiment session management

Picture for the day:

# Closing in on Version 1.0

Getting sounds associated with TargetSpheres – done!

Added TestManager and TestResult from VibrotactileHeadsetFLTK. Now making changes to accomodate the types of testes we’ll be doing with this application.

Adding write setup file – done!

Progress for today:

# Blew my hand off for a while

I’m in the process of turning the Phantom testbed code into a research tool. This means that a lot of items that have been #defines now need to be variables and such.

One of the mechanisms that the shared memory app uses to communicate is a char[255] message. I basically sprintf whatever I want into that, and I can then debug both applications simultaneously.

However, after checking to see that some data were coming across correctly, I took the formatting argument out of the sprintf statement and left the value in. Suddenly I was overflowing the 255 limit and causing all kinds of havoc. Took a few hours to chase that one down. That’s what you get for playing with C/C++. Moving on.

Anyway, I now have an event handling loop, and am able to load target spheres into the application and associate them with a sound file. Tommorrow we’ll try getting the sounds associated with the targets to play. There are some issues, primarily that the gripper can touch multiple targets simultaneously. Still, it looks pretty straightforward. After that I’ll start to roll in the TestManager and TestResults classes into the application.

The other thing to do for the day is to check out the headset code with Brian this evening in the lab and see if the output file bug has either disappeared or can be replicated.

I was just asked to see how many hours I have left for working this research. It turns out at the rate I’m going, that I can continue until mid-October. This is basically a big shout-out to Novetta, who has granted a continuation of my 20% time that was originally a hiring condition when I went to work for Edge. Thanks. And if you’d like a programming job in the DC area that supports creativity, give them a call.

I just can’t make the audio code break in writing out results. Odd. Maybe a corrupt input file can have unforeseen effects? Regardless, I’m going to stop pursuing this particular bug without more information

Fixing the state problem. Done.

Fixing the saving issue. Also changing the naming of the speakers to reflect Dolby or not. Done.

New version release built and deployed.

And back to Phantom++

I started to add in the user interface that will support experiments. Since it was already done, I pulled in most of the Fluid code from the Vibrotactile headset, which made things pretty easy. I needed to add an enclosing control system class that can move commande between the various pieces.

I’ve also decided that each sound will have an associated object with it. This allows each object to have a simple “acoustic” texture that doesn’t require any fancy data structure.

At this point, I’m estimating that the first version of the test program should be ready by Friday.

# Sounds like Deja Vu.

Adding custom speaker number and placement as per Dr. Kuber’s request.

Looks like dot product should do the trick:

Done! With only a couple of string compare issues. I also had to make the speaker index jump around the subwoofer channel until I can work out how to set the EQ.

And it looks like there are bugs in the code. It seems that you cannot do zero speed sessions. And the writing out of results with multiple sound files looks pretty confused. I’m not sure if extra CRs are being put in there or if some of the data isn’t being written out. Need to run some more examples.

# Moving beyond PoC

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.

# Proof of concept!

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.

• Need to add walls around the work environment so that the targets can’t get out of reach.
• Need to add the metal standoffs for the speakers. I was thinking about ordering some box tubing, but the sizes weren’t optimal. I’ll bend up some metal tonight.
• Need to start adding in the code that will support the experiments
• Feedback options
• Position and pressure only
• Position, force feedback and pressure
• Position, pressure and vibration
• Position, force feedback, pressure and vibration.
• I also need to extend the audio feedback so that arbitrary speakers can be used and positioned without adhering to Dolby positioning rules. I’ll get back to that after getting the metal speaker supports attached to the interface.

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;

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];

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];

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. */
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];
}
}
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);

}```

# And yet more…

Got the wiring cleaned up.

Integrating collision response with the targetSphere. The math is looking reasonably good.