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
  • Task code
  • 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;
	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);

}