//=========================================================================== 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; }