#define _USE_MATH_DEFINES
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#if defined(WIN32) || defined(WIN64)
#define NOMINMAX
#include "windows.h"
#endif
#ifdef MACOSX
#include "OpenGL/glu.h"
#else
#include "GL/glu.h"
#endif
#include <GLFW/glfw3.h>
#include "CMatrixGL.h"
#include "FontGL.h"
const Eigen::Vector3d SpherePosition(0.0, 0.0, 0.0);
constexpr double SphereRadius = 0.03;
constexpr double ToolRadius = 0.005;
constexpr double LinearStiffness = 1000.0;
constexpr int SwapInterval = 1;
bool simulationRunning = true;
bool simulationFinished = false;
int toolCount = 0;
Eigen::Vector3d toolPosition[2];
bool useRotation = false;
bool useGripper = false;
bool showRefreshRate = true;
GLFWwindow* window = nullptr;
int windowWidth = 0;
int windowHeight = 0;
int updateGraphics()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
double deviceRotation[3][3] = {};
{
return -1;
}
cMatrixGL matrix;
matrix.set(SpherePosition);
matrix.glMatrixPushMultiply();
GLUquadricObj* sphere = gluNewQuadric();
glEnable(GL_COLOR_MATERIAL);
glColor3f(0.1f, 0.3f, 0.5f);
gluSphere(sphere, SphereRadius, 32, 32);
matrix.glMatrixPop();
for (int index = 0; index < toolCount; index++)
{
matrix.set(toolPosition[index], deviceRotation);
matrix.glMatrixPushMultiply();
sphere = gluNewQuadric();
glColor3f(0.8f, 0.8f, 0.8f);
gluSphere(sphere, ToolRadius, 32, 32);
if (useRotation)
{
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(0.45f, 0.45f, 0.45f);
glVertex3d(0.00, 0.000, 0.000);
glVertex3d(0.02, 0.000, 0.000);
glVertex3d(0.02, -0.004, 0.000);
glVertex3d(0.02, 0.004, 0.000);
glVertex3d(0.02, 0.000, -0.004);
glVertex3d(0.02, 0.000, 0.004);
glEnd();
glEnable(GL_LIGHTING);
}
matrix.glMatrixPop();
}
if (showRefreshRate)
{
static double lastFrequencyUpdateTime =
dhdGetTime();
static char frequencyString[16] = "0.000 kHz";
if (time - lastFrequencyUpdateTime > 0.1)
{
lastFrequencyUpdateTime = time;
snprintf(frequencyString, 10, "%0.03f kHz", frequency);
}
glDisable(GL_LIGHTING);
glColor3f(1.0, 1.0, 1.0);
glRasterPos3f(0.0f, -0.01f, -0.1f);
for (char* character = frequencyString; *character != '\0'; character++)
{
render_character(*character, HELVETICA12);
}
glEnable(GL_LIGHTING);
}
GLenum err = glGetError();
if (err != GL_NO_ERROR)
{
return -1;
}
return 0;
}
void* hapticsLoop(void* a_userData)
{
Eigen::Vector3d force;
force.setZero();
Eigen::Vector3d forceTool[2];
forceTool[0].setZero();
forceTool[1].setZero();
{
std::cout <<
"error: failed to enable force rendering (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
return nullptr;
}
while (simulationRunning)
{
double px, py, pz;
if (useGripper)
{
{
std::cout << std::endl <<
"error: failed to read gripper thumb position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
toolPosition[0] << px, py, pz;
{
std::cout << std::endl <<
"error: failed to read gripper finger position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
toolPosition[1] << px, py, pz;
}
else
{
{
std::cout << std::endl <<
"error: failed to read position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
toolPosition[0] << px, py, pz;
}
for (int index = 0; index < toolCount; index++)
{
Eigen::Vector3d direction = (toolPosition[index] - SpherePosition).normalized();
double penetrationDistance = (toolPosition[index] - SpherePosition).norm() - SphereRadius - ToolRadius;
if (penetrationDistance < 0.0)
{
forceTool[index] = -1.0 * penetrationDistance * LinearStiffness * direction;
}
else
{
forceTool[index].setZero();
}
}
double gripperForceMagnitude = 0.0;
if (!useGripper)
{
force = forceTool[0];
}
else
{
force = forceTool[0] + forceTool[1];
Eigen::Vector3d gripperDirection = toolPosition[1] - toolPosition[0];
if (gripperDirection.norm() > 0.00001)
{
gripperDirection.normalize ();
Eigen::Vector3d gripperForce = (forceTool[1].dot(gripperDirection) / (gripperDirection.squaredNorm())) * gripperDirection;
gripperForceMagnitude = gripperForce.norm();
if (force.norm() > 0.001)
{
double cosAngle = gripperDirection.dot(gripperForce) / (gripperDirection.norm() * gripperForce.norm());
cosAngle = std::min(1.0, cosAngle);
cosAngle = std::max(-1.0, cosAngle);
double angle = acos(cosAngle);
if ((angle > M_PI / 2.0) || (angle < -M_PI / 2.0))
{
gripperForceMagnitude = -gripperForceMagnitude;
}
}
}
{
gripperForceMagnitude = -gripperForceMagnitude;
}
}
static bool safeToRenderHaptics = false;
if (!safeToRenderHaptics)
{
if (force.norm() == 0.0 && gripperForceMagnitude == 0.0)
{
safeToRenderHaptics = true;
}
else
{
force.setZero();
gripperForceMagnitude = 0.0;
}
}
{
std::cout <<
"error: failed to render force (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
}
simulationRunning = false;
simulationFinished = true;
return nullptr;
}
void onExit()
{
simulationRunning = false;
while (!simulationFinished)
{
}
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return;
}
std::cout << "connection closed" << std::endl;
return;
}
void onWindowResized(GLFWwindow* a_window,
int a_width,
int a_height)
{
windowWidth = a_width;
windowHeight = a_height;
double glAspect = (static_cast<double>(a_width) / static_cast<double>(a_height));
glViewport(0, 0, a_width, a_height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, glAspect, 0.01, 10);
gluLookAt(0.2, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void onKeyPressed(GLFWwindow* a_window,
int a_key,
int a_scanCode,
int a_action,
int a_modifiers)
{
if (a_action != GLFW_PRESS)
{
return;
}
if ((a_key == GLFW_KEY_ESCAPE) || (a_key == GLFW_KEY_Q))
{
exit(0);
}
if (a_key == GLFW_KEY_R)
{
showRefreshRate = !showRefreshRate;
}
}
void onError(int a_error,
const char* a_description)
{
std::cout << "error: " << a_description << std::endl;
}
int initializeGLFW()
{
if (!glfwInit())
{
return -1;
}
glfwSetErrorCallback(onError);
const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor());
windowWidth = static_cast<int>(0.8 * mode->height);
windowHeight = static_cast<int>(0.5 * mode->height);
int x = static_cast<int>(0.5 * (mode->width - windowWidth));
int y = static_cast<int>(0.5 * (mode->height - windowHeight));
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_TRUE);
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_STEREO, GL_FALSE);
glfwWindowHint(GLFW_VISIBLE, GL_FALSE);
#ifdef MACOSX
glfwWindowHint(GLFW_COCOA_RETINA_FRAMEBUFFER, GL_FALSE);
#endif
window = glfwCreateWindow(windowWidth, windowHeight, "Force Dimension - OpenGL Sphere Example", nullptr, nullptr);
if (!window)
{
return -1;
}
glfwMakeContextCurrent(window);
glfwSetKeyCallback(window, onKeyPressed);
glfwSetWindowSizeCallback(window, onWindowResized);
glfwSetWindowPos(window, x, y);
glfwSwapInterval(SwapInterval);
glfwShowWindow(window);
onWindowResized(window, windowWidth, windowHeight);
GLfloat mat_ambient[] = { 0.5f, 0.5f, 0.5f };
GLfloat mat_diffuse[] = { 0.5f, 0.5f, 0.5f };
GLfloat mat_specular[] = { 0.5f, 0.5f, 0.5f };
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, mat_ambient);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, mat_diffuse);
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, mat_specular);
glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 1.0);
GLfloat ambient[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat diffuse[] = { 0.8f, 0.8f, 0.8f, 1.0f };
GLfloat specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, specular);
glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 1.0);
glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0);
glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, 0.0);
GLfloat lightPos[] = { 2.0, 0.0, 0.0, 1.0f };
GLfloat lightDir[] = { -1.0, 0.0, 0.0, 1.0f };
glLightfv(GL_LIGHT0, GL_POSITION, lightPos);
glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, lightDir);
glLightf(GL_LIGHT0, GL_SPOT_CUTOFF, 180);
glLightf(GL_LIGHT0, GL_SPOT_EXPONENT, 1.0);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glEnable(GL_DEPTH_TEST);
glClearColor(0.0, 0.0, 0.0, 1.0);
return 0;
}
int initializeHaptics()
{
{
return -1;
}
toolCount = (useGripper) ? 2 : 1;
return 0;
}
int initializeSimulation()
{
for (int index = 0; index < toolCount; index++)
{
toolPosition[index].setZero();
}
return 0;
}
int main(int argc,
char* argv[])
{
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
if (initializeHaptics() < 0)
{
std::cout << "error: failed to initialize haptics" << std::endl;
return -1;
}
if (initializeGLFW() < 0)
{
std::cout << "error: failed to initialize GLFW" << std::endl;
return -1;
}
if (initializeSimulation() < 0)
{
std::cout << "error: failed to initialize the simulation" << std::endl;
return -1;
}
#if defined(WIN32) || defined(WIN64)
DWORD threadHandle;
CreateThread(nullptr, 0, (LPTHREAD_START_ROUTINE)(hapticsLoop), nullptr, 0x0000, &threadHandle);
SetThreadPriority(&threadHandle, THREAD_PRIORITY_ABOVE_NORMAL);
#else
pthread_t threadHandle;
pthread_create(&threadHandle, nullptr, hapticsLoop, nullptr);
struct sched_param schedulerParameters;
memset(&schedulerParameters, 0, sizeof(struct sched_param));
schedulerParameters.sched_priority = 10;
pthread_setschedparam(threadHandle, SCHED_RR, &schedulerParameters);
#endif
atexit(onExit);
std::cout <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
std::cout << "press 'r' to toggle display of the haptic rate" << std::endl;
std::cout << " 'q' to quit" << std::endl << std::endl;
while (simulationRunning && !glfwWindowShouldClose(window))
{
if (updateGraphics() < 0)
{
std::cout << "error: failed to update graphics" << std::endl;
break;
}
glfwSwapBuffers(window);
glfwPollEvents();
}
glfwDestroyWindow(window);
glfwTerminate();
return 0;
}
double __SDK dhdGetComFreq(char ID=-1)
Definition: dhdc.cpp:4654
bool __SDK dhdHasGripper(char ID=-1)
Definition: dhdc.cpp:699
int __SDK dhdEnableForce(uchar val, char ID=-1)
Definition: dhdc.cpp:4378
bool __SDK dhdHasWrist(char ID=-1)
Definition: dhdc.cpp:641
int __SDK dhdClose(char ID=-1)
Definition: dhdc.cpp:443
int __SDK dhdGetGripperFingerPos(double *px, double *py, double *pz, char ID=-1)
Definition: dhdc.cpp:4621
void __SDK dhdSleep(double sec)
Definition: dhdc.cpp:4754
@ DHD_NO_ERROR
Definition: dhdc.h:52
int __SDK dhdSetForceAndGripperForce(double fx, double fy, double fz, double fg, char ID=-1)
Definition: dhdc.cpp:4678
const char *__SDK dhdGetSDKVersionStr()
Definition: dhdc.cpp:925
bool __SDK dhdIsLeftHanded(char ID=-1)
Definition: dhdc.cpp:576
int __SDK dhdOpen()
Definition: dhdc.cpp:265
#define DHD_ON
Definition: dhdc.h:128
int __SDK dhdGetPosition(double *px, double *py, double *pz, char ID=-1)
Definition: dhdc.cpp:1614
const char *__SDK dhdGetSystemName(char ID=-1)
Definition: dhdc.cpp:814
double __SDK dhdGetTime()
Definition: dhdc.cpp:190
int __SDK dhdGetOrientationFrame(double matrix[3][3], char ID=-1)
Definition: dhdc.cpp:4552
int __SDK dhdGetGripperThumbPos(double *px, double *py, double *pz, char ID=-1)
Definition: dhdc.cpp:4582
const char *__SDK dhdErrorGetLastStr()
Definition: dhdError.cpp:197