コード例 #1
0
/******************************************************************************
 The main servo loop scheduler callback.  Samples the haptic frame rate.
******************************************************************************/
HDCallbackCode HDCALLBACK ServoSchedulerCallback(void *pUserData)
{
    hdBeginFrame(hdGetCurrentDevice());

    HDdouble rate;
    hdGetDoublev(HD_INSTANTANEOUS_UPDATE_RATE, &rate);

    hdEndFrame(hdGetCurrentDevice());

    gUpdateRateServo.sample(rate);

    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        std::cerr << std::endl;
        std::cerr << error << std::endl;

        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
        }
    }

    return HD_CALLBACK_CONTINUE;
}
コード例 #2
0
ファイル: main.cpp プロジェクト: oliriley/PhantomController
/*******************************************************************************
 Main callback that calculates and sets the force.
*******************************************************************************/
HDCallbackCode HDCALLBACK CoulombCallback(void *data)
{
    HHD hHD = hdGetCurrentDevice();

    hdBeginFrame(hHD);

    hduVector3Dd pos;
    hdGetDoublev(HD_CURRENT_POSITION,pos);
    hduVector3Dd forceVec;
    forceVec = forceField(pos);
    hdSetDoublev(HD_CURRENT_FORCE, forceVec);
        
    hdEndFrame(hHD);

    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error during scheduler callback");
        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
        }
    }

    return HD_CALLBACK_CONTINUE;
}
コード例 #3
0
/*******************************************************************************
 Haptic sphere callback.  
 The sphere is oriented at 0,0,0 with radius 40, and provides a repelling force 
 if the device attempts to penetrate through it. 
*******************************************************************************/
HDCallbackCode HDCALLBACK FrictionlessSphereCallback(void *data)
{
    const double sphereRadius = 10.0;
    const hduVector3Dd spherePosition(0,50,0);

    // Stiffness, i.e. k value, of the sphere.  Higher stiffness results
    // in a harder surface.
    const double sphereStiffness = .01;

    hdBeginFrame(hdGetCurrentDevice());
   
    // Get the position of the device.
    hduVector3Dd position;
    hdGetDoublev(HD_CURRENT_POSITION, position);
    
    // Find the distance between the device and the center of the
    // sphere.
    double distance = (position-spherePosition).magnitude();

    // If the user is within the sphere -- i.e. if the distance from the user to 
    // the center of the sphere is less than the sphere radius -- then the user 
    // is penetrating the sphere and a force should be commanded to repel him 
    // towards the surface.
    if (distance > sphereRadius)
    {
        // Calculate the penetration distance.
        double penetrationDistance = sphereRadius-distance;

        // Create a unit vector in the direction of the force, this will always 
        // be outward from the center of the sphere through the user's 
        // position.
        hduVector3Dd forceDirection = (position-spherePosition)/distance;

        // Use F=kx to create a force vector that is away from the center of 
        // the sphere and proportional to the penetration distance, and scsaled 
        // by the object stiffness.  
        // Hooke's law explicitly:
        double k = sphereStiffness;
        hduVector3Dd x = penetrationDistance*forceDirection;
        hduVector3Dd f = k*x;
        hdSetDoublev(HD_CURRENT_FORCE, f);
    }

    hdEndFrame(hdGetCurrentDevice());

    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error during main scheduler callback\n");

        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
        }        
    }

    return HD_CALLBACK_CONTINUE;
}
コード例 #4
0
ファイル: omni.cpp プロジェクト: danepowell/phantom_omni
HDCallbackCode HDCALLBACK omni_state_callback(void *pUserData) {
	OmniState *omni_state = static_cast<OmniState *>(pUserData);
	if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE) {
	  ROS_DEBUG("Updating calibration...");
	    hdUpdateCalibration(calibrationStyle);
	  }
	hdBeginFrame(hdGetCurrentDevice());
	//Get angles, set forces
	hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, omni_state->rot);
	hdGetDoublev(HD_CURRENT_POSITION, omni_state->position);
	hdGetDoublev(HD_CURRENT_JOINT_ANGLES, omni_state->joints);

	hduVector3Dd vel_buff(0, 0, 0);
	vel_buff = (omni_state->position * 3 - 4 * omni_state->pos_hist1
			+ omni_state->pos_hist2) / 0.002;  //mm/s, 2nd order backward dif
	omni_state->velocity = (.2196 * (vel_buff + omni_state->inp_vel3)
			+ .6588 * (omni_state->inp_vel1 + omni_state->inp_vel2)) / 1000.0
			- (-2.7488 * omni_state->out_vel1 + 2.5282 * omni_state->out_vel2
					- 0.7776 * omni_state->out_vel3);  //cutoff freq of 20 Hz
	omni_state->pos_hist2 = omni_state->pos_hist1;
	omni_state->pos_hist1 = omni_state->position;
	omni_state->inp_vel3 = omni_state->inp_vel2;
	omni_state->inp_vel2 = omni_state->inp_vel1;
	omni_state->inp_vel1 = vel_buff;
	omni_state->out_vel3 = omni_state->out_vel2;
	omni_state->out_vel2 = omni_state->out_vel1;
	omni_state->out_vel1 = omni_state->velocity;
	if (omni_state->lock == true) {
		omni_state->force = 0.04 * (omni_state->lock_pos - omni_state->position)
				- 0.001 * omni_state->velocity;
	}

	hdSetDoublev(HD_CURRENT_FORCE, omni_state->force);

	//Get buttons
	int nButtons = 0;
	hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
	omni_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0;
	omni_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0;

	hdEndFrame(hdGetCurrentDevice());

	HDErrorInfo error;
	if (HD_DEVICE_ERROR(error = hdGetError())) {
		hduPrintError(stderr, &error, "Error during main scheduler callback");
		if (hduIsSchedulerError(&error))
			return HD_CALLBACK_DONE;
	}

	float t[7] = { 0., omni_state->joints[0], omni_state->joints[1],
			omni_state->joints[2] - omni_state->joints[1], omni_state->rot[0],
			omni_state->rot[1], omni_state->rot[2] };
	for (int i = 0; i < 7; i++)
		omni_state->thetas[i] = t[i];
	return HD_CALLBACK_CONTINUE;
}
コード例 #5
0
HDCallbackCode HDCALLBACK HapticDevice::aSchedule(void *pUserData)
{
	HapticSynchronizer * hs = (HapticSynchronizer *) pUserData;

	for(unsigned int i=0;i<NB_DEVICES_MAX;i++)
	{
		if(hs[i].m_data!=NULL)
		{
			HapticData * data = hs[i].m_data;

			hduVector3Dd force( 0, 0, 0 );				
			hdBeginFrame(hs[i].m_data->m_id);		

			HDdouble forceClamp;
			hduVector3Dd distance = data->m_realPosition - data->m_position;

			force = data->m_force;
			/* if we are nearly at the center don't recalculate anything,
				since the center is a singular point. */
			if(data->m_nbCollision == 1)
			{
				if(distance.magnitude() > EPSILON && data->m_ready)
				{   

					/* force is calculated from F=kx; k is the stiffness
						and x is the vector magnitude and direction.  In this case,
						k = STIFFNESS, and x is the penetration vector from 
						the actual position to the desired position. */
														
					force = STIFFNESS*distance;						
					force *=  BALL_MASS;					
					force[0] = 0;
				} 
			}
			
			// Check if we need to clamp the force. 
			hdGetDoublev(HD_NOMINAL_MAX_FORCE, &forceClamp);
			if (hduVecMagnitude(force) > forceClamp)
			{
				hduVecNormalizeInPlace(force);
				hduVecScaleInPlace(force, forceClamp);
			}
			

			hdSetDoublev(HD_CURRENT_FORCE, force);
			
			hdEndFrame(data->m_id);
		}
		
	}

	
    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
       // hduPrintError(stderr, &error, "Error during scheduler callback");

        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
		}
    }

	return HD_CALLBACK_CONTINUE;
}
コード例 #6
0
/*******************************************************************************
 Haptic plane callback.  The plane is oriented along Y=0 and provides a 
 repelling force if the device attempts to penetrates through it.
*******************************************************************************/
HDCallbackCode HDCALLBACK FrictionlessPlaneCallback(void *data)
{
    // Stiffnes, i.e. k value, of the plane.  Higher stiffness results
    // in a harder surface.
    const double planeStiffness = .25;

    // Amount of force the user needs to apply in order to pop through
    // the plane.
    const double popthroughForceThreshold = 5.0;
    
    // Plane direction changes whenever the user applies sufficient
    // force to popthrough it.
    // 1 means the plane is facing +Y.
    // -1 means the plane is facing -Y.
    static int directionFlag = 1;

    hdBeginFrame(hdGetCurrentDevice());

    // Get the position of the device.
    hduVector3Dd position;
    hdGetDoublev(HD_CURRENT_POSITION, position);

    // If the user has penetrated the plane, set the device force to 
    // repel the user in the direction of the surface normal of the plane.
    // Penetration occurs if the plane is facing in +Y and the user's Y position
    // is negative, or vice versa.

    if ((position[1] <= 0 && directionFlag > 0) || 
        (position[1] > 0) && (directionFlag < 0))
    {
        // Create a force vector repelling the user from the plane proportional
        // to the penetration distance, using F=kx where k is the plane 
        // stiffness and x is the penetration vector.  Since the plane is 
        // oriented at the Y=0, the force direction is always either directly 
        // upward or downward, i.e. either (0,1,0) or (0,-1,0).
        double penetrationDistance = fabs(position[1]);
        hduVector3Dd forceDirection(0,directionFlag,0);

        // Hooke's law explicitly:
        double k = planeStiffness;
        hduVector3Dd x = penetrationDistance*forceDirection;
        hduVector3Dd f = k*x;

        // If the user applies sufficient force, pop through the plane
        // by reversing its direction.  Otherwise, apply the repel
        // force.
        if (f.magnitude() > popthroughForceThreshold)
        {
            f.set(0.0,0.0,0.0);
            directionFlag = -directionFlag;
        }

        hdSetDoublev(HD_CURRENT_FORCE, f);
    }

    hdEndFrame(hdGetCurrentDevice());

    // In case of error, terminate the callback.
    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error detected during main scheduler callback\n");

        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;  
        }
    }

    return HD_CALLBACK_CONTINUE;
}
コード例 #7
0
/******************************************************************************
 * Main scheduler callback for rendering the anchored spring force.
 *****************************************************************************/
HDCallbackCode HDCALLBACK OmniForceCallback(void *pUserData/*, Omni device*/)
{
    static hduVector3Dd anchor;
    static HDboolean bRenderForce = FALSE;
    HDErrorInfo error;

    // HDint nCurrentButtons, nLastButtons;
    hduVector3Dd position;
    hduVector3Dd force;
	force[0] = 0;
	force[1] = 0;
	force[2] = 0;

    hdBeginFrame(hdGetCurrentDevice());

    hdGetDoublev(HD_CURRENT_POSITION, position);

	/*hdGetIntegerv(HD_CURRENT_BUTTONS, &nCurrentButtons);
    hdGetIntegerv(HD_LAST_BUTTONS, &nLastButtons);

    if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 &&
        (nLastButtons & HD_DEVICE_BUTTON_1) == 0)
    {
        * Detected button down *
        memcpy(anchor, position, sizeof(hduVector3Dd));
        bRenderForce = TRUE;
    }
    else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 &&
             (nLastButtons & HD_DEVICE_BUTTON_1) != 0)

    {
        * Detected button up *
        bRenderForce = FALSE;

        * Send zero force to the device, or else it will just continue
           rendering the last force sent *
        hdSetDoublev(HD_CURRENT_FORCE, force);
    }

    if (bRenderForce)
    {
        * Compute spring force as F = k * (anchor - pos), which will attract
           the device position towards the anchor position *
        hduVecSubtract(force, anchor, position);
        hduVecScaleInPlace(force, gSpringStiffness);
                
        hdSetDoublev(HD_CURRENT_FORCE, force);
    }*/

	if (count>MAX_COUNT)
	{
		device.updateUser();
		count -= MAX_COUNT;
	}

    hdEndFrame(hdGetCurrentDevice());

    /* Check if an error occurred while attempting to render the force */
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        if (hduIsForceError(&error))
        {
            bRenderForce = FALSE;
        }
        else if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;
        }
    }
	count++;
    return HD_CALLBACK_CONTINUE;
}	
コード例 #8
0
HDCallbackCode HDCALLBACK phantom_state_callback(void *pUserData)
{
  static bool lock_flag = true;
  PhantomState *phantom_state = static_cast<PhantomState *>(pUserData);

  hdBeginFrame(hdGetCurrentDevice());
  //Get angles, set forces
  hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, phantom_state->rot);
  hdGetDoublev(HD_CURRENT_POSITION, phantom_state->position);
  hdGetDoublev(HD_CURRENT_JOINT_ANGLES, phantom_state->joints);
  hdGetDoublev(HD_CURRENT_TRANSFORM, phantom_state->hd_cur_transform);

  hduVector3Dd vel_buff(0, 0, 0);
  vel_buff = (phantom_state->position * 3 - 4 * phantom_state->pos_hist1 + phantom_state->pos_hist2) / 0.002; //mm/s, 2nd order backward dif
  //	phantom_state->velocity = 0.0985*(vel_buff+phantom_state->inp_vel3)+0.2956*(phantom_state->inp_vel1+phantom_state->inp_vel2)-(-0.5772*phantom_state->out_vel1+0.4218*phantom_state->out_vel2 - 0.0563*phantom_state->out_vel3);    //cutoff freq of 200 Hz
  phantom_state->velocity = (.2196 * (vel_buff + phantom_state->inp_vel3)
      + .6588 * (phantom_state->inp_vel1 + phantom_state->inp_vel2)) / 1000.0
      - (-2.7488 * phantom_state->out_vel1 + 2.5282 * phantom_state->out_vel2 - 0.7776 * phantom_state->out_vel3); //cutoff freq of 20 Hz
  phantom_state->pos_hist2 = phantom_state->pos_hist1;
  phantom_state->pos_hist1 = phantom_state->position;
  phantom_state->inp_vel3 = phantom_state->inp_vel2;
  phantom_state->inp_vel2 = phantom_state->inp_vel1;
  phantom_state->inp_vel1 = vel_buff;
  phantom_state->out_vel3 = phantom_state->out_vel2;
  phantom_state->out_vel2 = phantom_state->out_vel1;
  phantom_state->out_vel1 = phantom_state->velocity;
  //	printf("position x, y, z: %f %f %f \node_", phantom_state->position[0], phantom_state->position[1], phantom_state->position[2]);
  //	printf("velocity x, y, z, time: %f %f %f \node_", phantom_state->velocity[0], phantom_state->velocity[1],phantom_state->velocity[2]);
  if (phantom_state->lock == true)
  {
    lock_flag = true;
    phantom_state->force = 0.04 * (phantom_state->lock_pos - phantom_state->position) - 0.001 * phantom_state->velocity;
  }
  else
  {
    if(lock_flag == true)
    {
      phantom_state->force.set(0.0, 0.0, 0.0);
      lock_flag = false;
    }
  }

  // Set force
  hdSetDoublev(HD_CURRENT_FORCE, phantom_state->force);
  // Set torque
  hdSetDoublev(HD_CURRENT_TORQUE, phantom_state->torque);

  //Get buttons
  int nButtons = 0;
  hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
  phantom_state->buttons[0] = (nButtons & HD_DEVICE_BUTTON_1) ? 1 : 0;
  phantom_state->buttons[1] = (nButtons & HD_DEVICE_BUTTON_2) ? 1 : 0;

  hdEndFrame(hdGetCurrentDevice());

  HDErrorInfo error;
  if (HD_DEVICE_ERROR(error = hdGetError()))
  {
    hduPrintError(stderr, &error, "Error during main scheduler callback\n");
    if (hduIsSchedulerError(&error))
      return HD_CALLBACK_DONE;
  }

  float t[7] = {0., phantom_state->joints[0], phantom_state->joints[1], phantom_state->joints[2] - phantom_state->joints[1],
                phantom_state->rot[0], phantom_state->rot[1], phantom_state->rot[2]};
  for (int i = 0; i < 7; i++)
    phantom_state->thetas[i] = t[i];
  return HD_CALLBACK_CONTINUE;
}
コード例 #9
0
ファイル: QueryDevice.c プロジェクト: danepowell/openhaptics
/*******************************************************************************
 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, 
        &currentData, HD_MIN_SCHEDULER_PRIORITY);

    memcpy(&prevData, &currentData, 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,
                              &currentData,
                              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, &currentData.m_error, "Device error detected");

            if (hduIsSchedulerError(&currentData.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, &currentData, sizeof(DeviceData));    
    }
}
コード例 #10
0
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;
}
コード例 #11
0
/*******************************************************************************
 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;
}