void updateHaptics(void)
{
// reset clock
simClock.reset();
// main haptic simulation loop
while(simulationRunning)
{
// stop the simulation clock
simClock.stop();
// read the time increment in seconds
double timeInterval = simClock.getCurrentTimeSeconds();
if (timeInterval > 0.001) { timeInterval = 0.001; }
// restart the simulation clock
simClock.reset();
simClock.start();
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
for (int y=0; y<10; y++)
{
for (int x=0; x<10; x++)
{
cVector3d nodePos = nodes[x][y]->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
cVector3d tmpfrc = cNegate(f);
nodes[x][y]->setExternalForce(tmpfrc);
force.add(f);
}
}
// integrate dynamics
defWorld->updateDynamics(timeInterval);
// scale force
force.mul(deviceForceScale);
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// reset simulation clock
simClock.reset();
simClock.start();
// main haptic simulation loop
while(simulationRunning)
{
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setPos(pos);
// init temp variable
cVector3d force;
force.zero();
// compute reaction forces
list<cGELMesh*>::iterator i;
// model water level
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
if (nodePos.z < level)
{
double depth = nodePos.z - level;
force.add(cVector3d(0,0,-100*depth));
}
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
if ((nodePos.z-radius) < level)
{
double depth = (nodePos.z-radius) - level;
force.add(cVector3d(0,0,-1.0 * depth));
node->m_vel.mul(0.95);
}
node->setExternalForce(force);
}
}
}
// compute haptic feedback
for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
{
cGELMesh *nextItem = *i;
if (nextItem->m_useMassParticleModel)
{
int numVertices = nextItem->m_gelVertices.size();
for (int i=0; i<numVertices; i++)
{
cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
}
force.add(cMul(1.0, f));
}
}
if (nextItem->m_useSkeletonModel)
{
list<cGELSkeletonNode*>::iterator i;
for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
{
cGELSkeletonNode* node = *i;
cVector3d nodePos = node->m_pos;
double radius = node->m_radius;
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
if (f.lengthsq() > 0)
{
cVector3d tmpfrc = cNegate(f);
node->setExternalForce(tmpfrc);
}
force.add(cMul(4.0, f));
}
}
}
// integrate dynamics
double interval = simClock.stop();
simClock.reset();
simClock.start();
if (interval > 0.001) { interval = 0.001; }
defWorld->updateDynamics(interval);
// scale force
force.mul(0.6 * deviceForceScale);
// water viscosity
if ((pos.z - deviceRadius) < level)
{
// read damping properties of haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
double Kv = 0.8 * info.m_maxLinearDamping;
// read device velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// compute a scale factor [0,1] proportional to percentage
// of tool volume immersed in the water
double val = (level - (pos.z - deviceRadius)) / (2.0 * deviceRadius);
double scale = cClamp(val, 0.1, 1.0);
// compute force
cVector3d forceDamping = cMul(-Kv * scale, linearVelocity);
force.add(forceDamping);
}
// send forces to haptic device
hapticDevice->setForce(force);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// simulation clock
cPrecisionClock simClock;
simClock.start(true);
// reset haptics activation clock
startHapticsClock.reset();
startHapticsClock.start();
bool hapticsReady = false;
// main haptic simulation loop
while(simulationRunning)
{
// wait for some time before enabling haptics
if (!hapticsReady)
{
if (startHapticsClock.getCurrentTimeSeconds() > 3.0)
{
hapticsReady = true;
}
}
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// check if the tool is touching an object
cGenericObject* object = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
// read user switch status
bool userSwitch = tool->getUserSwitch(0);
// if the tool is currently grasping an object we simply update the interaction grasp force
// between the tool and the object (modeled by a virtual spring)
if (graspActive && userSwitch)
{
// retrieve latest position and orientation of grasped ODE object in world coordinates
cMatrix3d globalGraspObjectRot = graspObject->getGlobalRot();
cVector3d globalGraspObjectPos = graspObject->getGlobalPos();
// compute the position of the grasp point on object in global coordinates
cVector3d globalGraspPos = globalGraspObjectPos + cMul(globalGraspObjectRot, graspPos);
// retrieve the position of the tool in global coordinates
cVector3d globalToolPos = tool->getProxyGlobalPos();
// compute the offset between the tool and grasp point on the object
cVector3d offset = globalToolPos - globalGraspPos;
// model a spring between both points
double STIFFNESS = 4;
cVector3d force = STIFFNESS * offset;
// apply attraction force (grasp) onto object
graspObject->addGlobalForceAtGlobalPos(force, globalGraspPos);
// scale magnitude and apply opposite force to haptic device
tool->m_lastComputedGlobalForce.add(cMul(-1.0, force));
// update both end points of the line which is used for display purposes only
graspLine->m_pointA = globalGraspPos;
graspLine->m_pointB = globalToolPos;
}
// the user is not or no longer currently grasping the object
else
{
// was the user grasping the object at the previous simulation loop
if (graspActive)
{
// we disable grasping
graspActive = false;
// we hide the virtual line between the tool and the grasp point
graspLine->setShowEnabled(false);
// we enable haptics interaction between the tool and the previously grasped object
if (graspObject != NULL)
{
graspObject->m_imageModel->setHapticEnabled(true, true);
}
}
// the user is touching an object
if (object != NULL)
{
// check if object is attached to an external ODE parent
cGenericType* externalParent = object->getExternalParent();
cODEGenericBody* ODEobject = dynamic_cast<cODEGenericBody*>(externalParent);
if (ODEobject != NULL)
{
// get position of tool
cVector3d pos = tool->m_proxyPointForceModel->m_contactPoint0->m_globalPos;
// check if user has enabled the user switch to gras the object
if (userSwitch)
{
// a new object is being grasped
graspObject = ODEobject;
// retrieve the grasp position on the object in local coordinates
graspPos = tool->m_proxyPointForceModel->m_contactPoint0->m_localPos;
// grasp in now active!
graspActive = true;
// enable small line which display the offset between the tool and the grasp point
graspLine->setShowEnabled(true);
// disable haptic interaction between the tool and the grasped device.
// this is performed for stability reasons.
graspObject->m_imageModel->setHapticEnabled(false, true);
}
// retrieve the haptic interaction force being applied to the tool
cVector3d force = tool->m_lastComputedGlobalForce;
// apply haptic force to ODE object
cVector3d tmpfrc = cNegate(force);
if (hapticsReady)
{
ODEobject->addGlobalForceAtGlobalPos(tmpfrc, pos);
}
}
}
}
// send forces to device
tool->applyForces();
// retrieve simulation time and compute next interval
double time = simClock.getCurrentTimeSeconds();
double nextSimInterval = cClamp(time, 0.001, 0.004);
// reset clock
simClock.reset();
simClock.start();
// update simulation
ODEWorld->updateDynamics(nextSimInterval);
}
// exit haptics thread
simulationFinished = true;
}
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;
}
void updateHaptics(void)
{
double timeV = 0.0;
cLabel* label = new cLabel();
cLabel* label2 = new cLabel();
rootLabels->addChild(label);
rootLabels->addChild(label2);
label->setPos(0, 0, 0);
label2->setPos(0, -20, 0);
label->m_fontColor.set(1.0, 1.0, 1.0);
label2->m_fontColor.set(1.0, 1.0, 1.0);
// main haptic simulation loop
while(simulationRunning)
{
float deltaTime = pClock.getCurrentTimeSeconds() - lastTime;
lastTime = pClock.getCurrentTimeSeconds();
cVector3d equilibrium (0.0, 0.0, 0.0);
cVector3d pos0 (0.0, 0.0, 0.0), pos1 (0.0, 0.0, 0.0);
// for each device
int i=0;
while (i < numHapticDevices)
{
// read position of haptic device
cVector3d newPosition;
hapticDevices[i]->getPosition(newPosition);
newPosition = deviceToWorld(newPosition, i);
((i == 0)? pos0 : pos1) = newPosition;
// read orientation of haptic device
cMatrix3d newRotation;
hapticDevices[i]->getRotation(newRotation);
// update position and orientation of cursor
cursors[i]->setPos(newPosition);
cursors[i]->setRot(newRotation);
// read linear velocity from device
cVector3d linearVelocity;
hapticDevices[i]->getLinearVelocity(linearVelocity);
// update arrow
// velocityVectors[i]->m_pointA = newPosition;
// velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity);
// read user button status
bool buttonStatus;
hapticDevices[i]->getUserSwitch(0, buttonStatus);
// adjustthe color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (i == 0)
cursors[i]->m_material = matCursor2;
else
cursors[i]->m_material = matCursor1;
// increment counter
i++;
}
double f0, f1;
f0 = pos0.length();
f1 = pos1.length();
f0 = f0/(f0 + f1);
f1 = 1.0 - f0;
equilibrium = pos1 + (pos0 - pos1)*f0;
// Update the position of the sun
cVector3d dir = pos1 - pos0;
double dist = dir.length();
dir.normalize();
double vibrationSpeed = 20.0;
double vibrationAmount = 0.02;
//sun->setPos(sun->getPos()*(1.0 - deltaTime*speed) + equilibrium*(deltaTime*speed));
//timeV += deltaTime*vibrationSpeed*(0.7 - cAbs(f0 - 0.5)*2.0);
sun->setPos(equilibrium /*+ vibrationAmount*dir*cSinRad(timeV)*/);
// Update logic
if (!calibrationFinished) {
label->m_string = "Calibrating, please move the haptic devices around in order to determine their limitations in movement.";
label2->m_string = "Press 'c' to finish calibration.";
if (sun->getPos().x < min.x)
min.x = sun->getPos().x;
if (sun->getPos().y < min.y)
min.y = sun->getPos().y;
if (sun->getPos().z < min.z)
min.z = sun->getPos().z;
if (sun->getPos().x > max.x)
max.x = sun->getPos().x;
if (sun->getPos().y > max.y)
max.y = sun->getPos().y;
if (sun->getPos().z > max.z)
max.z = sun->getPos().z;
} else if (logic->isReady()) {
if (logic->gameIsOver() && !scoreDisplayed) {
std::stringstream strs;
strs << logic->playTime();
std::string playString = strs.str();
// define its position, color and string message
label->m_string = "Congratulation! Your Time: " + playString;
label2->m_string = "Press 'r' to restart!";
for (i = 0; i < numHapticDevices; i++) {
cVector3d zero(0.0, 0.0, 0.0);
hapticDevices[i]->setForce(zero);
}
scoreDisplayed = true;
} else if (!scoreDisplayed) {
label->m_string = "";
label2->m_string = "";
logic->update(deltaTime);
}
for (i = 0; i < numHapticDevices; i++) {
// compute a reaction force
cVector3d newForce (0,0,0);
cVector3d devicePosition;
hapticDevices[i]->getPosition(devicePosition);
devicePosition = deviceToWorld(devicePosition, i);
double k = 0.4;
if (test == 1) k = 0.3;
double dist = (devicePosition - sun->getPos()).length();
//dist=dist-0.1;
newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist);
//double intensity = (newForce.length())*1.0;
//newForce.normalize();
//newForce *= intensity;
// newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist);
if (i == 0) { // Device on positive X (RIGHT)
newForce.x *= -1.0;
newForce.y *= -1.0;
}
// send computed force to haptic device
// bool status = true;
// if (hapticDevices[i]->getUserSwitch(0))
// printf("button pressed\n");
// Check if the sphere is in the target area. If so, vibrate
cVector3d vibrationForce(0.0, 0.0, 0.0);
if (logic->sphereInTarget() && !logic->gameIsOver()) {
Cube* target = logic->getTarget();
double dist = target->getPos().distance(sun->getPos());
double factor = 1.0 - dist/(target->size/2.0);
timeV += deltaTime * (0.5 + factor/2.0);
double f = 2*cSinRad(40*timeV);
vibrationForce = cVector3d(f, f, f);
}
newForce += vibrationForce;
if (test <= 2 || i == 0)
hapticDevices[i]->setForce(newForce);
else {
cVector3d zero;
zero.zero();
hapticDevices[i]->setForce(zero);
}
}
}
}
// exit haptics thread
simulationFinished = true;
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("Be God\n");
printf ("DH2660 Haptics\n");
printf ("-----------------------------------\n");
printf ("\n\n");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
pClock.start(true);
lastTime = pClock.getCurrentTimeSeconds();
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
// the color is defined by its (R,G,B) components.
world->setBackgroundColor(0.25, 0.55, 0.8);
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (-1.0, 0.0, 0.3), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the "up" vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
camera->enableMultipassTransparency(true);
// create a light source and attach it to the camera
light = new cLight(world);
camera->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setPos(cVector3d( 2.0, 0.5, 1.0)); // position the light source
light->setDir(cVector3d(-2.0, 0.5, 1.0)); // define the direction of the light beam
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// read the number of haptic devices currently connected to the computer
numHapticDevices = handler->getNumDevices();
// Check if two devices are plugged in.
if(numHapticDevices != 2) {
std::cout << "Application shut down: Two falcon devices are needed." << std::endl;
exit(0);
}
// create a node on which we will attach small labels that display the
// position of each haptic device
rootLabels = new cGenericObject();
camera->m_front_2Dscene.addChild(rootLabels);
// create a small label as title
// cLabel* titleLabel = new cLabel();
// rootLabels->addChild(titleLabel);
// // define its position, color and string message
// titleLabel->setPos(0, 30, 0);
// titleLabel->m_fontColor.set(1.0, 1.0, 1.0);
// titleLabel->m_string = "Haptic Device Pos [mm]:";
// for each available haptic device, create a 3D cursor
// and a small line to show velocity
int i = 0;
while (i < numHapticDevices)
{
// get a handle to the next haptic device
cGenericHapticDevice* newHapticDevice;
handler->getDevice(newHapticDevice, i);
// open connection to haptic device
newHapticDevice->open();
// initialize haptic device
newHapticDevice->initialize();
// store the handle in the haptic device table
hapticDevices[i] = newHapticDevice;
// retrieve information about the current haptic device
cHapticDeviceInfo info = newHapticDevice->getSpecifications();
// create a cursor by setting its radius
cShapeSphere* newCursor = new cShapeSphere(0.01);
// add cursor to the world
world->addChild(newCursor);
// add cursor to the cursor table
cursors[i] = newCursor;
// create a small line to illustrate velocity
// cShapeLine* newLine = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
// velocityVectors[i] = newLine;
// // add line to the world
// world->addChild(newLine);
// create a string that concatenates the device number and model name.
// string strID;
// cStr(strID, i);
// string strDevice = "#" + strID + " - " +info.m_modelName;
// attach a small label next to the cursor to indicate device information
// cLabel* newLabel = new cLabel();
// newCursor->addChild(newLabel);
// newLabel->m_string = strDevice;
// newLabel->setPos(0.00, 0.02, 0.00);
// newLabel->m_fontColor.set(1.0, 1.0, 1.0);
// if the device provided orientation sensing (stylus), a reference
// frame is displayed
if (info.m_sensedRotation == true)
{
// display a reference frame
newCursor->setShowFrame(true);
// set the size of the reference frame
newCursor->setFrameSize(0.05, 0.05);
}
// crate a small label to indicate the position of the device
// cLabel* newPosLabel = new cLabel();
// rootLabels->addChild(newPosLabel);
// newPosLabel->setPos(0, -20 * i, 0);
// newPosLabel->m_fontColor.set(0.6, 0.6, 0.6);
// labels[i] = newPosLabel;
// increment counter
i++;
}
// here we define the material properties of the cursor when the
// user button of the device end-effector is engaged (ON) or released (OFF)
// a light orange material color
matCursor1.m_ambient.set(0.5, 0.2, 0.0);
matCursor1.m_diffuse.set(1.0, 0.5, 0.0);
matCursor1.m_specular.set(1.0, 1.0, 1.0);
// a blue material color
matCursor2.m_ambient.set(0.1, 0.1, 0.4);
matCursor2.m_diffuse.set(0.3, 0.3, 0.8);
matCursor2.m_specular.set(1.0, 1.0, 1.0);
room = new Room(world, 0.5);
matSun.m_ambient.set(0.4, 0.3, 0.0);
matSun.m_diffuse.set(1.0, 0.7, 0.0);
matSun.m_specular.set(1.0, 1.0, 1.0);
sun = new Sphere(world, 0.05, matSun, -room->getHeight()/2);
max = cVector3d(-DBL_MAX, -DBL_MAX, -DBL_MAX);
min = cVector3d(DBL_MAX, DBL_MAX, DBL_MAX);
logic = new Logic(world, sun);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("Haptics - BG");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
/*!
* \brief Callback for receiving a point cloud.
*
* This description is displayed lower in the doxygen as an extended description along with
* the above brief description.
*/
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input)
{
// const std::string& publisher_name = event.getPublisherName();
// ros::M_string connection_header = event.getConnectionHeader();
// ros::Time receipt_time = event.getReceiptTime();
// const std::string topic = connection_header["topic"];
// const sensor_msgs::PointCloud2& input = event.getMessage();
std::string topic = "none";
int cloud_size = input->width*input->height;
ROS_DEBUG_NAMED("haptics", "Got a cloud on topic %s in frame %s with %d points!",
topic.c_str(),
input->header.frame_id.c_str(),
cloud_size);
if(cloud_size == 0) return;
pcl::fromROSMsg(*input, last_cloud_);
if(last_cloud_.header.frame_id.compare("/tool_frame"))
{
// ROS_INFO("Transforming cloud with %d points from %s to %s.",
// (int)last_cloud_.points.size(), last_cloud_.header.frame_id.c_str(),
// "/world");
if(!tfl_.waitForTransform("/tool_frame", last_cloud_.header.frame_id,
last_cloud_.header.stamp, ros::Duration(2.0)) )
{
ROS_ERROR("Couldn't get transform for cloud, returning FAILURE!");
return;
}
pcl_ros::transformPointCloud("/tool_frame", last_cloud_, last_cloud_, tfl_);
}
//ROS_INFO("m_shape is %d", m_shape);
//if(new_object->m_shape == hviz::Haptics_CLOUD)
{
boost::mutex::scoped_lock lock(mutex_);
if(object->m_shape == hviz::Haptics_CLOUD)
object->createFromCloud(last_cloud_);
}
if(!got_first_cloud_){
got_first_cloud_ = true;
ROS_INFO("Got first cloud!");
}
static bool firstTime = true;
static int counter = 0;
static cPrecisionClock pclock;
float sample_period = 2.0;
if(firstTime) // start a clock to estimate the rate
{
pclock.setTimeoutPeriodSeconds(sample_period);
pclock.start(true);
firstTime = false;
}
// estimate the refresh rate and publish
++counter;
if (pclock.timeoutOccurred()) {
pclock.stop();
float rate = counter/sample_period;
counter = 0;
pclock.start(true);
std_msgs::String msg;
char status_string[256];
sprintf(status_string, "Cloud rate: %.3f",
rate );
msg.data = status_string;
pub_status_.publish(msg);
}
}
/*!
* \brief The timer callback sends graphical output to rviz.
*
* This description is displayed lower in the doxygen as an extended description along with
* the above brief description.
*/
void timerCallback()
{
if(!tool) return;
ros::Time time_now = ros::Time::now(); // use single time for all output
static bool firstTime = true;
static int counter = 0;
static cPrecisionClock pclock;
if(firstTime) // start a clock to estimate the rate
{
pclock.setTimeoutPeriodSeconds(1.0);
pclock.start(true);
firstTime = false;
}
// estimate the refresh rate and publish
++counter;
if (pclock.timeoutOccurred()) {
pclock.stop();
graphicRateEstimate = counter;
counter = 0;
pclock.start(true);
std_msgs::String msg;
char status_string[256];
sprintf(status_string, "Haptic rate: %.3f Graphics Rate: %.3f",
rateEstimate, graphicRateEstimate);
msg.data = status_string;
pub_status_.publish(msg);
}
// Transmit the visualizations of the tool/proxy
float proxy_radius = config_.tool_radius;
cVector3d pos = object->m_interactionProjectedPoint; //tool->getDeviceGlobalPos();
cVector3d HIP = tool->getDeviceGlobalPos();
cMatrix3d tool_rotation = tool->m_deviceGlobalRot;
if(false && device_info.m_sensedRotation)
{
object_manipulator::shapes::Mesh mesh;
mesh.dims = tf::Vector3(0.5, 0.5, 0.5);
mesh.frame.setRotation(chai_tools::cMatrixToTFQuaternion(tool_rotation));
mesh.frame.setOrigin(tf::Vector3(pos.x, pos.y, pos.z));
mesh.header.stamp = time_now;
mesh.header.frame_id = "/tool_frame";
mesh.use_embedded_materials = true;
mesh.mesh_resource = std::string("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
// std::string proximal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger.dae");
// std::string distal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger_tip.dae");
object_manipulator::drawMesh(pub_marker_, mesh, "gripper", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));
}
else
{
object_manipulator::shapes::Sphere sphere;
sphere.dims = tf::Vector3(2*proxy_radius, 2*proxy_radius, 2*proxy_radius);
sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(pos.x, pos.y, pos.z));
sphere.header.frame_id = "/tool_frame";
sphere.header.stamp = time_now;
object_manipulator::drawSphere(pub_marker_, sphere, "proxy", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));
//object_manipulator::shapes::Sphere sphere;
sphere.dims = tf::Vector3(1.9*proxy_radius, 1.9*proxy_radius, 1.9*proxy_radius);
sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(HIP.x, HIP.y, HIP.z));
sphere.header.frame_id = "/tool_frame";
sphere.header.stamp = time_now;
object_manipulator::drawSphere(pub_marker_, sphere, "HIP", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.0, 0.0, 0.6));
}
object_manipulator::shapes::Cylinder box;
tf::Quaternion quat = chai_tools::cMatrixToTFQuaternion(object->tPlane->getRot());
box.frame.setRotation(quat);
box.frame.setOrigin(chai_tools::cVectorToTF(object->tPlane->getPos()) - 0.5*proxy_radius*box.frame.getBasis().getColumn(2));
box.dims = tf::Vector3(5*proxy_radius, 5*proxy_radius, 0.0015);
box.header.frame_id = "/tool_frame";
box.header.stamp = time_now;
if(object->m_interactionInside){
object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), false);
}
else
object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), true);
boost::mutex::scoped_lock lock(mutex_);
if(config_.publish_cloud)
{
// if(object->last_normals->points.size() == object->last_points->points.size())
// {
// visualization_msgs::Marker marker;
// marker.header = object->last_points->header;
// marker.ns = "cloud";
// marker.id = 0;
// marker.type = visualization_msgs::Marker::SPHERE_LIST; // CUBE, SPHERE, ARROW, CYLINDER
// marker.action = false?((int32_t)visualization_msgs::Marker::DELETE):((int32_t)visualization_msgs::Marker::ADD);
// marker.lifetime = ros::Duration();
// float scale = object->m_active_radius/2;
// marker.scale = object_manipulator::msg::createVector3Msg(scale, scale, scale);
// marker.color = object_manipulator::msg::createColorMsg(0.5, 0.5, 0.5,1.0);
// float angle = time_now.toSec();
// angle = config_.light_angle * M_PI/180.0;
// tf::Vector3 light_source = tf::Vector3(0.1*cos(angle), 0.1*sin(angle), 0.3);
// object_manipulator::shapes::Sphere sphere;
// sphere.dims = tf::Vector3(0.02, 0.02, 0.02);
// sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), light_source);
// sphere.header.frame_id = "/tool_frame";
// sphere.header.stamp = marker.header.stamp;
// object_manipulator::drawSphere(pub_marker_, sphere, "light", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 1.0, 1.0, 0.5));
// for(int i = 0; i < object->last_points->points.size(); i++)
// {
// const PointT &pt = object->last_points->points[i];
// const pcl::Normal &nl = object->last_normals->points[i];
// tf::Vector3 point = tf::Vector3(pt.x, pt.y, pt.z);
// tf::Vector3 N_vec = tf::Vector3(nl.normal[0], nl.normal[1], nl.normal[2]).normalized();
// tf::Vector3 L_vec = (light_source - point).normalized();
// tf::Vector3 color = fabs(L_vec.dot(N_vec))*tf::Vector3(1.0, 1.0, 1.0) + tf::Vector3(0.2, 0.2, 0.2);
// //tf::Vector3 color = tf::Vector3(pt.x*pt.x, pt.y*pt.y, 1.0);
// marker.colors.push_back(object_manipulator::msg::createColorMsg(color.x(), color.y(), color.z(), 1.0));;
// marker.points.push_back(object_manipulator::msg::createPointMsg(pt.x, pt.y, pt.z));
// }
// //ROS_INFO("Publishing marker cloud with %d points!", marker.points.size());
// pub_marker_.publish(marker);
// }
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*(object->last_points), msg);
msg.header.frame_id = "/tool_frame";
msg.header.stamp = time_now;
pub_cloud_.publish(msg);
}
}
void hapticsLoop(void* a_pUserData)
{
// read the position of the haptic device
cursor->updatePose();
// compute forces between the cursor and the environment
cursor->computeForces();
// stop the simulation clock
g_clock.stop();
// read the time increment in seconds
double increment = g_clock.getCurrentTime() / 1000000.0;
// restart the simulation clock
g_clock.initialize();
g_clock.start();
// get position of cursor in global coordinates
cVector3d cursorPos = cursor->m_deviceGlobalPos;
// compute velocity of cursor;
timeCounter = timeCounter + increment;
if (timeCounter > 0.01)
{
cursorVel = (cursorPos - lastCursorPos) / timeCounter;
lastCursorPos = cursorPos;
timeCounter = 0;
}
// get position of torus in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute the velocity of the sphere at the contact point
cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
if (rotVelocity.length() > CHAI_SMALL)
{
cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
cVector3d vpc = cursorPos - projection;
if (vpc.length() > CHAI_SMALL)
{
contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
}
}
// get the last force applied to the cursor in global coordinates
cVector3d cursorForce = cursor->m_lastComputedGlobalForce;
// compute friction force
cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));
// add friction force to cursor
cursor->m_lastComputedGlobalForce.add(friction);
// update rotational velocity
if (friction.length() > CHAI_SMALL)
{
rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
}
// add some damping...
//rotVelocity.mul(1.0 - increment);
// compute the next rotation of the torus
if (rotVelocity.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
}
// send forces to haptic device
cursor->applyForces();
}