//=========================================================================== bool cEffectStickSlip::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { // check if history for this IDN exists if (a_toolID < CHAI_EFFECT_MAX_IDN) { if (m_parent->m_interactionInside) { // check if a recent valid point has been stored previously if (!m_history[a_toolID].m_valid) { m_history[a_toolID].m_currentStickPosition = a_toolPos; m_history[a_toolID].m_valid = true; } // read parameters for stick and slip model double stiffness = m_parent->m_material.getStickSlipStiffness(); double forceMax = m_parent->m_material.getStickSlipForceMax(); // compute current force between last stick position and current tool position double distance = cDistance(a_toolPos, m_history[a_toolID].m_currentStickPosition); double forceMag = distance * stiffness; if (forceMag > 0) { // if force above threshold, slip... if (forceMag > forceMax) { m_history[a_toolID].m_currentStickPosition = a_toolPos; a_reactionForce.zero(); } // ...otherwise stick else { a_reactionForce = (forceMag / distance) * cSub(m_history[a_toolID].m_currentStickPosition, a_toolPos); } } else { a_reactionForce.zero(); } return (true); } else { // the tool is located outside the object, so zero reaction force m_history[a_toolID].m_valid = false; a_reactionForce.zero(); return (false); } } else { a_reactionForce.zero(); return (false); } }
//=========================================================================== bool cEffectVibration::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { if (m_parent->m_interactionInside) { // read vibration parameters double vibrationFrequency = m_parent->m_material.getVibrationFrequency(); double vibrationAmplitude = m_parent->m_material.getVibrationAmplitude(); // read time double time = clock.getCurrentTimeSeconds(); // compute force magnitude double forceMag = vibrationAmplitude * sin(2.0 * CHAI_PI *vibrationFrequency * time); a_reactionForce = cMul(forceMag, cVector3d(1, 0, 0)); return (true); } else { // the tool is located outside the object, so zero reaction force a_reactionForce.zero(); return (false); } }
//============================================================================== bool cEffectMagnet::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { // compute distance from object to tool double distance = cDistance(a_toolPos, m_parent->m_interactionPoint); // get parameters of magnet double magnetMaxForce = m_parent->m_material->getMagnetMaxForce(); double magnetMaxDistance = m_parent->m_material->getMagnetMaxDistance(); double stiffness = m_parent->m_material->getStiffness(); double forceMagnitude = 0; if (m_enabledInside || (!m_parent->m_interactionInside)) { if ((distance < magnetMaxDistance) && (stiffness > 0)) { double d = magnetMaxForce / stiffness; if (distance < d) { forceMagnitude = stiffness * distance; } else { double dx = (magnetMaxDistance - d); if (dx > 0) { double k = magnetMaxForce / dx; forceMagnitude = k * (magnetMaxDistance - distance); } } // compute reaction force int sign = -1; if (m_parent->m_interactionInside) { sign = 1; } a_reactionForce = cMul(sign * forceMagnitude, m_parent->m_interactionNormal); return (true); } else { return (false); } } else { // the tool is located outside the magnet zone a_reactionForce.zero(); return (false); } }
//=========================================================================== bool cEffectViscosity::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { if (m_parent->m_interactionInside) { // the tool is located inside the object. double viscosity = m_parent->m_material.getViscosity(); a_reactionForce = cMul(-viscosity, a_toolVel); return (true); } else { // the tool is located outside the object, so zero reaction force. a_reactionForce.zero(); return (false); } }
//=========================================================================== bool cEffectSurface::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { if (m_parent->m_interactionInside) { // the tool is located inside the object, // we compute a reaction force using Hooke's law double stiffness = m_parent->m_material.getStiffness(); a_reactionForce = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos)); return (true); } else { // the tool is located outside the object, so zero reaction force a_reactionForce.zero(); return (false); } }
//=========================================================================== bool cEffectPotentialField::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { if (m_parent->m_interactionInside) { // the tool is located inside the object, so zero reaction force a_reactionForce.zero(); return (false); } else { // the tool is located outside the object, // we compute a reaction force using Hooke's law double stiffness = m_parent->m_material->getStiffness(); double maxForce = m_parent->m_material->getMagnetMaxForce(); cVector3d force = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos)); if (maxForce > 0.0) { double ratio = force.length() / maxForce; if (ratio > 1.0) { force = (1.0 / ratio) * force; } } else { force.zero(); } a_reactionForce = force; return (true); } }
void updateHaptics(void) { // reset clock cPrecisionClock clock; clock.reset(); // main haptic simulation loop while(simulationRunning) { ///////////////////////////////////////////////////////////////////// // SIMULATION TIME ///////////////////////////////////////////////////////////////////// // stop the simulation clock clock.stop(); // read the time increment in seconds double timeInterval = clock.getCurrentTimeSeconds(); // restart the simulation clock clock.reset(); clock.start(); // update frequency counter frequencyCounter.signal(1); ///////////////////////////////////////////////////////////////////// // HAPTIC FORCE COMPUTATION ///////////////////////////////////////////////////////////////////// // compute global reference frames for each object world->computeGlobalPositions(true); // update position and orientation of tool tool->updatePose(); // compute interaction forces tool->computeInteractionForces(); // send forces to device tool->applyForces(); ///////////////////////////////////////////////////////////////////// // HAPTIC SIMULATION ///////////////////////////////////////////////////////////////////// // get position of cursor in global coordinates cVector3d toolPos = tool->getDeviceGlobalPos(); // get position of object in global coordinates cVector3d objectPos = object->getGlobalPos(); // compute a vector from the center of mass of the object (point of rotation) to the tool cVector3d v = cSub(toolPos, objectPos); // compute angular acceleration based on the interaction forces // between the tool and the object cVector3d angAcc(0,0,0); if (v.length() > 0.0) { // get the last force applied to the cursor in global coordinates // we negate the result to obtain the opposite force that is applied on the // object cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce); // compute the effective force that contributes to rotating the object. cVector3d force = toolForce - cProject(toolForce, v); // compute the resulting torque cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force)); // update rotational acceleration const double INERTIA = 0.4; angAcc = (1.0 / INERTIA) * torque; } // update rotational velocity angVel.add(timeInterval * angAcc); // set a threshold on the rotational velocity term const double MAX_ANG_VEL = 10.0; double vel = angVel.length(); if (vel > MAX_ANG_VEL) { angAcc.mul(MAX_ANG_VEL / vel); } // add some damping too const double DAMPING = 0.1; angVel.mul(1.0 - DAMPING * timeInterval); // if user switch is pressed, set velocity to zero if (tool->getUserSwitch(0) == 1) { angVel.zero(); } // compute the next rotation configuration of the object if (angVel.length() > C_SMALL) { object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length()); } } // exit haptics thread simulationFinished = true; }
//=========================================================================== bool cEffectMagnet::computeForce(const cVector3d& a_toolPos, const cVector3d& a_toolVel, const unsigned int& a_toolID, cVector3d& a_reactionForce) { // compute distance from object to tool double distance = cDistance(a_toolPos, m_parent->m_interactionProjectedPoint); // get parameters of magnet double magnetMaxForce = m_parent->m_material.getMagnetMaxForce(); double magnetMaxDistance = m_parent->m_material.getMagnetMaxDistance(); double stiffness = m_parent->m_material.getStiffness(); double forceMagnitude = 0; if ((distance > 0) && (distance < magnetMaxDistance) && (stiffness > 0)) { double limitLinearModel = magnetMaxForce / stiffness; cClamp(limitLinearModel, 0.0, 0.5 * distance); if (distance < limitLinearModel) { // apply local linear model near magnet forceMagnitude = stiffness * distance; } else { // compute quadratic model cMatrix3d sys; sys.m[0][0] = limitLinearModel * limitLinearModel; sys.m[0][1] = limitLinearModel; sys.m[0][2] = 1.0; sys.m[1][0] = magnetMaxDistance * magnetMaxDistance; sys.m[1][1] = magnetMaxDistance; sys.m[1][2] = 1.0; sys.m[2][0] = 2.0 * limitLinearModel; sys.m[2][1] = 1.0; sys.m[2][2] = 0.0; sys.invert(); cVector3d param; sys.mulr(cVector3d(magnetMaxForce, 0.0, -1.0), param); // apply quadratic model double val = distance - limitLinearModel; forceMagnitude = param.x * val * val + param.y * val + param.z; } // compute magnetic force a_reactionForce = cMul(forceMagnitude, cNormalize(cSub(m_parent->m_interactionProjectedPoint, a_toolPos))); // add damping component double viscosity = m_parent->m_material.getViscosity(); cVector3d viscousForce = cMul(-viscosity, a_toolVel); a_reactionForce.add(viscousForce); return (true); } else { // the tool is located outside the magnet zone a_reactionForce.zero(); return (false); } }
void updateHaptics(void) { // reset clock simClock.reset(); // main haptic simulation loop while(simulationRunning) { // compute global reference frames for each object world->computeGlobalPositions(true); // update position and orientation of tool tool->updatePose(); // compute interaction forces tool->computeInteractionForces(); // send forces to device tool->applyForces(); // stop the simulation clock simClock.stop(); // read the time increment in seconds double timeInterval = simClock.getCurrentTimeSeconds(); // restart the simulation clock simClock.reset(); simClock.start(); // temp variable to compute rotational acceleration cVector3d rotAcc(0,0,0); // check if tool is touching an object cGenericObject* objectContact = tool->m_proxyPointForceModel->m_contactPoint0->m_object; if (objectContact != NULL) { // retrieve the root of the object mesh cGenericObject* obj = objectContact->getSuperParent(); // get position of cursor in global coordinates cVector3d toolPos = tool->m_deviceGlobalPos; // get position of object in global coordinates cVector3d objectPos = obj->getGlobalPos(); // compute a vector from the center of mass of the object (point of rotation) to the tool cVector3d vObjectCMToTool = cSub(toolPos, objectPos); // compute acceleration based on the interaction forces // between the tool and the object if (vObjectCMToTool.length() > 0.0) { // get the last force applied to the cursor in global coordinates // we negate the result to obtain the opposite force that is applied on the // object cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce); // compute effective force to take into account the fact the object // can only rotate around a its center mass and not translate cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool); // compute the resulting torque cVector3d torque = cMul(vObjectCMToTool.length(), cCross( cNormalize(vObjectCMToTool), effectiveForce)); // update rotational acceleration const double OBJECT_INERTIA = 0.4; rotAcc = (1.0 / OBJECT_INERTIA) * torque; } } // update rotational velocity rotVel.add(timeInterval * rotAcc); // set a threshold on the rotational velocity term const double ROT_VEL_MAX = 10.0; double velMag = rotVel.length(); if (velMag > ROT_VEL_MAX) { rotVel.mul(ROT_VEL_MAX / velMag); } // add some damping too const double DAMPING_GAIN = 0.1; rotVel.mul(1.0 - DAMPING_GAIN * timeInterval); // if user switch is pressed, set velocity to zero if (tool->getUserSwitch(0) == 1) { rotVel.zero(); } // compute the next rotation configuration of the object if (rotVel.length() > CHAI_SMALL) { object->rotate(cNormalize(rotVel), timeInterval * rotVel.length()); } } // exit haptics thread simulationFinished = true; }