Exemplo n.º 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;
}
Exemplo n.º 2
0
/*******************************************************************************
 Checks the state of the gimbal button and gets the position of the device.
*******************************************************************************/
HDCallbackCode HDCALLBACK updateDeviceCallback(void *pUserData)
{   
    int nButtons = 0;

    hdBeginFrame(hdGetCurrentDevice());

    /* Retrieve the current button(s). */
    hdGetIntegerv(HD_CURRENT_BUTTONS, &nButtons);
    
    /* In order to get the specific button 1 state, we use a bitmask to
       test for the HD_DEVICE_BUTTON_1 bit. */
    gServoDeviceData.m_buttonState = 
        (nButtons & HD_DEVICE_BUTTON_1) ? HD_TRUE : HD_FALSE;
        
    /* Get the current location of the device (HD_GET_CURRENT_POSITION)
       We declare a vector of three doubles since hdGetDoublev returns 
       the information in a vector of size 3. */
    hdGetDoublev(HD_CURRENT_POSITION, gServoDeviceData.m_devicePosition);

    /* Also check the error state of HDAPI. */
    gServoDeviceData.m_error = hdGetError();

    /* Copy the position into our device_data tructure. */
    hdEndFrame(hdGetCurrentDevice());

    return HD_CALLBACK_CONTINUE;    
}
Exemplo n.º 3
0
	HDCallbackCode HDCALLBACK CybPhantom::setForce(void *pUserData) {
		//cout << "setForce()" << endl;

		hduVector3Dd position;//buffer para a recepção de dados do socket
		hduVector3Dd force;


		HDdouble forceClamp;
		static const HDdouble kAnchorStiffness = 0.1;

		hdBeginFrame(hdGetCurrentDevice()); /*Começa o frame.*/

		hdGetDoublev(HD_CURRENT_POSITION, position);

		//cout << anchorPosition[0] << " " << anchorPosition[1] <<  " " << anchorPosition[2] << endl;

		hduVecSubtract(force, anchor, position);
		hduVecScaleInPlace(force, kAnchorStiffness);
		//hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &forceClamp);
		/*if (hduVecMagnitude(force) > forceClamp)
		{
			hduVecNormalizeInPlace(force);
			hduVecScaleInPlace(force, forceClamp);
		}*/
		//cout << force[0] << " " << force[1] <<  " " << force[2] << endl;
		hdSetDoublev(HD_CURRENT_FORCE, force);

		hdEndFrame(hdGetCurrentDevice());

		return HD_CALLBACK_CONTINUE;

	}
/*******************************************************************************
 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;
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
/*******************************************************************************
 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;
}
Exemplo n.º 7
0
/*************************************************************
ADDED
*************************************************************/
HDCallbackCode HDCALLBACK hdBeginCB(void *data)
{
// calling hdBeginFrame increments the HD frame counter.
    hdBeginFrame(hdGetCurrentDevice());

    HDErrorInfo error;

    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error in hdBeginCB\n");
        return HD_CALLBACK_DONE;
    }

    return HD_CALLBACK_CONTINUE;
}
Exemplo n.º 8
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;
}
Exemplo n.º 9
0
HDCallbackCode HDCALLBACK phantom_callback(void *pUserData)
{

	double	Acc=0;
	double  Mss = 0.0001;

	Xprv = X;
	hdBeginFrame(hdGetCurrentDevice());

	hdGetDoublev(HD_CURRENT_POSITION,posistion);
	if (start)
	{
		X0=posistion[0];
		start = false;
		hdEndFrame(Device);
		return HD_CALLBACK_CONTINUE;
	}

	X = posistion[0]-X0;
	V = X - Xprv;

	/*
	 * Velocity Command channel
	 */
	// Calculate Positive Energy
	if (Fs*Xm>0)
	{
		mst_Xm_P += Fs*Xm*Scale;
	}
	else
	{
		// Do nothing
	}
	//ROS_INFO("Xm channel Energy: %5f",mst_Xm_P);
	/*
	 * Velocity feedback channel
	 */
	Fve = -FveGain*(Xm*Scale-Vs);
	if (Fve*Vs>0)
	{
		mst_Vs_N -=Fve*Vs;
	}
	else
	{
		//Do nothing
	}
	//ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Vs_N,Vs,Fve);
	//PC
#if (PC_Fve_On)
	{
		if (mst_Vs_N+sla_Vs_P<0)
		{
			mst_Vs_N +=Fve*Vs; // backward 1 step

			// Modify Vs
			if (Fve*Fve>0)
			{
				Vs = (mst_Vs_N+sla_Vs_P)/Fve;
			}
			else
			{
				Vs = 0;
			}

			// update

			Fv = -FveGain*(Xm*Scale-Vs);
			mst_Vs_N -=Fv*Vs;
			ROS_INFO("PC");
		}
	}
#endif

	/*
	 * Obstacle force
	 */
	if (Fe*Vm>0)
	{
		mst_Fe_N-=Fe*Vm; //fe channel output energy
	}
	else
	{

	}
	//	ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe);
	//	fprintf(mstEnergy,"%.3f \n",mstE_N_fe);
	//	fprintf(slvEnergy,"%.3f \n",slaE_P_fe);
	//	ROS_INFO("Master fe Energy: %5f	",mstE_N_fe);

#if PC_Fe_On
	if (mst_Fe_N+sla_Fe_P<0) //output+input <0
	{
		mst_Fe_N+=Fe*Vm; //backward 1 step

		// Modify Fe
		if (Vm*Vm > 0.0)
			Fe = (mst_Fe_N+sla_Fe_P)/Vm;
		else
			Fe = 0;
		ROS_INFO("PC");
		// Update
		mst_Fe_N-=Fe*Vm;
	}
#endif

/*
 * Velocity Based force
 */
	if (Fv*Vm>0)
	{
		mst_Fv_N-=Fv*Vm; //fe channel output energy
	}
	else
	{

	}
	//	ROS_INFO("Energy - Velocity - Force: %5f - %5f - %5f",mst_Fe_N,Vm,Fe);
	//	fprintf(mstEnergy,"%.3f \n",mstE_N_fe);
	//	fprintf(slvEnergy,"%.3f \n",slaE_P_fe);
	//	ROS_INFO("Master fe Energy: %5f	",mstE_N_fe);

#if PC_Fv_On
	if (mst_Fv_N+sla_Fv_P<0) //output+input <0
	{
		mst_Fv_N+=Fv*Vm; //backward 1 step

		// Modify Fe
		if (Vm*Vm > 0.0)
			Fv = (mst_Fv_N+sla_Fv_P)/Vm;
		else
			Fv = 0;

		// Update
		mst_Fv_N-=Fv*Vm;
		ROS_INFO("PC");
	}
#endif

	/*
	 * Combination
	 */
#if !FeOn
	Fe = 0;
#endif
#if !FveOn
	Fve = 0;
#endif
#if !FvOn
	Fv = 0;
#endif

#if (PC_Fe_On||PC_Fve_On||PC_Fv_On)
	Fm = Fe + Fve + Fv;

	Acc = ( -1*K_Master*( Xm - X ) + Fm )/Mss;

	Vm += Acc*0.001;

	Xm += Vm*0.001;

	Fm = K_Master*(Xm - X);
#else
	Xm = X;
	Vm = V;
	Fm = Fe +Fve+Fv;
#endif
	//	ROS_INFO("Obstacle Feedback Force: %5f",Fe);
	//	ROS_INFO("Vsd: %5f",Xm*Scale);
	ROS_INFO("Force: %5f",Fm);
	force[2] = 0;
	force[1] = 0;
	force[0] = Fm;
	Saturation(force);
	hdSetDoublev(HD_CURRENT_FORCE, force);
	hdEndFrame(hdGetCurrentDevice());

	fprintf(data,"%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",Fm,Fve,Fe,Xm*Scale,Vs,mst_Fe_N,sla_Fe_P,mst_Vs_N,sla_Vs_P,mst_Fv_N,sla_Fv_P,Fv);

	MasToSlaDelay();
	return HD_CALLBACK_CONTINUE;
}
Exemplo n.º 10
0
HDCallbackCode HDCALLBACK hdEndCB(void *data)

{
    static const HDdouble kRampUpRate = 0.0001;
    static const HDdouble kImpulseLimit = 0.001;

    hdSetDoublev(HD_SOFTWARE_FORCE_IMPULSE_LIMIT,&kImpulseLimit);
    hdSetDoublev(HD_FORCE_RAMPING_RATE, &kRampUpRate );
    hdGetDoublev(HD_CURRENT_FORCE, force);

//Create custom haptic layers. There are two layers here
//at DOP = 0.1  and DOP = 0.35 .

    if (touchedHole && force[2]>=0.0)
    {
        if (probeDop > 0.0 && probeDop < 0.0125)
            force[2] = 0.25;
        else if (probeDop > 0.0 && probeDop < 0.025)
            force[2] = 0.5;
        else if (probeDop > 0.025 && probeDop < 0.05)
            force[2] = 0.75;
        else if (probeDop > 0.05 && probeDop < 0.075)
            force[2] = 1.0;
        else if (probeDop > 0.075 && probeDop < 0.09)
            force[2] = 1.5;
        else if (probeDop > 0.09 && probeDop < 0.1)
            force[2] = 0.0;
        else if (probeDop > 0.1 && probeDop < 0.2)
            force[2] = 0.25;
        else if (probeDop > 0.2 && probeDop < 0.225)
            force[2] = 0.5;
        else if (probeDop > 0.225 && probeDop < 0.25)
            force[2] = 0.75;
        else if (probeDop > 0.25 && probeDop < 0.275)
            force[2] = 0.0;
        else if (probeDop > 0.275 && probeDop < 0.3)
            force[2] = 0.0;
        else if (probeDop > 0.3 && probeDop < 0.35)
            force[2] = 0.25;
        else if (probeDop > 0.35 && probeDop < 0.4)
            force[2] = 0.5;
        else if (probeDop > 0.4)
            force[2] = 0.5;

        hdSetDoublev(HD_CURRENT_FORCE, force*forceScaler);
    }

// calling hdEndFrame decrements the frame counter.
// when the beginFrame counter reaches 0, forces are rendered.
// Note that HL makes calls to hdBeginFrame & hdEndFrame internally

    hdEndFrame(hdGetCurrentDevice());
    HDErrorInfo error;

    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error in hdEndCB\n");
    }

    return HD_CALLBACK_CONTINUE;
}
Exemplo n.º 11
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;
}	
Exemplo n.º 12
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;
}
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;
}