//===========================================================================
void cODEGenericBody::createStaticPlane(const cVector3d& a_position,
const cVector3d& a_normal)
{
// object is static by default
m_static = true;
// temp variables
cVector3d normal = a_normal;
cVector3d offset(0,0,0);
// check normal
if (normal.length() == 0) { return; }
// compute parameters
normal.normalize();
double a = normal.x;
double b = normal.y;
double c = normal.z;
offset = cProject(a_position, normal);
double d = offset.length();
if (d > 0)
{
if (cAngle(offset, normal) > cDegToRad(90))
{
d = -d;
}
}
// build fixed plane
m_ode_geom = dCreatePlane(m_ODEWorld->m_ode_space, a, b, c, d);
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_PLANE;
// store dynamic model parameters
m_paramDynColModel0 = normal.x;
m_paramDynColModel1 = normal.y;
m_paramDynColModel2 = normal.z;
m_posOffsetDynColModel = a_position;
m_rotOffsetDynColModel.identity();
}
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;
}
//===========================================================================
void cProxyPointForceAlgo::updateForce()
{
// initialize variables
double stiffness = 0.0;
cVector3d normal;
normal.zero();
// if there are no contacts between proxy and environment, no force is applied
if (m_numContacts == 0)
{
m_lastGlobalForce.zero();
return;
}
//---------------------------------------------------------------------
// stiffness and surface normal estimation
//---------------------------------------------------------------------
else if (m_numContacts == 1)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() );
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
}
// if there are two contact points, the stiffness is the average of the
// stiffnesses of the two intersected triangles' meshes
else if (m_numContacts == 2)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() ) / 2.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.mul(1.0/2.0);
}
// if there are three contact points, the stiffness is the average of the
// stiffnesses of the three intersected triangles' meshes
else if (m_numContacts == 3)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint2->m_triangle->getParent()->m_material.getStiffness() ) / 3.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.add(m_contactPoint2->m_globalNormal);
normal.mul(1.0/3.0);
}
//---------------------------------------------------------------------
// computing a force (Hooke's law)
//---------------------------------------------------------------------
// compute the force by modeling a spring between the proxy and the device
cVector3d force;
m_proxyGlobalPos.subr(m_deviceGlobalPos, force);
force.mul(stiffness);
m_lastGlobalForce = force;
// compute tangential and normal forces
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
//---------------------------------------------------------------------
// force shading (optional)
//---------------------------------------------------------------------
if ((m_useForceShading) && (m_numContacts == 1))
{
// get vertices and normals related to contact triangle
cVector3d vertex0 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getPos()));
cVector3d vertex1 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getPos()));
cVector3d vertex2 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getPos()));
cVector3d normal0 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getNormal());
cVector3d normal1 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getNormal());
cVector3d normal2 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getNormal());
// compute angles between normals. If the angles are very different, then do not apply shading.
double angle01 = cAngle(normal0, normal1);
double angle02 = cAngle(normal0, normal2);
double angle12 = cAngle(normal1, normal2);
if ((angle01 < m_forceShadingAngleThreshold) || (angle02 < m_forceShadingAngleThreshold) || (angle12 < m_forceShadingAngleThreshold))
{
double a0 = 0;
double a1 = 0;
cProjectPointOnPlane(m_contactPoint0->m_globalPos, vertex0, vertex1, vertex2, a0, a1);
cVector3d normalShaded = cAdd(
cMul(0.5, cAdd(cMul(a0, normal1), cMul((1-a0), normal0))),
cMul(0.5, cAdd(cMul(a1, normal2), cMul((1-a1), normal0)))
);
normalShaded.normalize();
if (cAngle(normalShaded, normal) > 1.0)
{
normalShaded.negate();
}
if (cAngle(normal, normalShaded) < m_forceShadingAngleThreshold)
{
double forceMagnitude = m_normalForce.length();
force = cAdd( cMul(forceMagnitude, normalShaded), m_tangentialForce);
m_lastGlobalForce = force;
normal = normalShaded;
// update tangential and normal forces again
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
}
}
}
}
//==============================================================================
void cToolGripper::computeInteractionForces()
{
// convert the angle of the gripper into a position in device coordinates.
// this value is device dependent.
double gripperPositionFinger = 0.0;
double gripperPositionThumb = 0.0;
if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
// compute new position of thumb and finger
cVector3d lineFingerThumb = getGlobalRot().getCol1();
cVector3d pFinger = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionFinger * lineFingerThumb;
cVector3d pThumb = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionThumb * lineFingerThumb;
cVector3d posFinger, posThumb;
if (m_hapticDevice->m_specifications.m_rightHand)
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pThumb));
}
else
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pThumb));
}
// compute forces
cVector3d forceThumb = m_hapticPointThumb->computeInteractionForces(posThumb,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
cVector3d forceFinger = m_hapticPointFinger->computeInteractionForces(posFinger,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
// compute torques
double scl = 0.0;
double factor = m_gripperWorkspaceScale * m_workspaceScaleFactor;
if (factor > 0.0)
{
scl = 1.0 / factor;
}
cVector3d torque = scl * cAdd(cCross(cSub(posThumb, m_deviceGlobalPos), forceThumb), cCross(cSub(posFinger, m_deviceGlobalPos), forceFinger));
// compute gripper force
double gripperForce = 0.0;
if ((m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7) ||
(m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7))
{
cVector3d dir = posFinger - posThumb;
if (dir.length() > 0.00001)
{
dir.normalize ();
cVector3d force = cProject (forceFinger, dir);
gripperForce = force.length();
if (force.length() > 0.001)
{
double angle = cAngle(dir, force);
if ((angle > C_PI/2.0) || (angle < -C_PI/2.0)) gripperForce = -gripperForce;
}
}
}
// gripper damping
double gripperAngularVelocity = 0.0;
m_hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
double gripperDamping = -0.1 * m_hapticDevice->m_specifications.m_maxGripperAngularDamping * gripperAngularVelocity;
// finalize forces, torques and gripper force
m_lastComputedGlobalForce = forceThumb + forceFinger;
m_lastComputedGlobalTorque = torque;
m_lastComputedGripperForce = gripperForce + gripperDamping;
}
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;
}
