#include <atomic>
#include <chrono>
#include <iostream>
#include <iomanip>
#include <thread>
#if defined(WIN32) | defined(WIN64)
#include <windows.h>
#endif
#include "drdc.h"
#include <GLFW/glfw3.h>
#ifdef MACOSX
#include "OpenGL/glu.h"
#else
#include "GL/glu.h"
#endif
#include "Robot.h"
#include "Transform.h"
std::atomic<bool> simulationRunning = true;
std::atomic<bool> engageOverride = false;
Transform masterCurrent;
Transform slaveTarget;
Transform slaveCurrent;
Robot slaveRobot;
int updateGraphics()
{
constexpr float HapticSphereRadius = 0.01f;
constexpr float TargetSphereRadius = 0.005f;
constexpr float RobotSphereRadius = 0.01f;
constexpr float HapticFrameSize = 0.04f;
constexpr float TargetFrameSize = 0.02f;
constexpr float RobotFrameSize = 0.04f;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glClearColor(0.0, 0.0, 0.0, 1.0);
auto renderSphere = [](double* a_transform,
float a_red, float a_green, float a_blue,
float a_radius,
float a_frameSize)
{
glPushMatrix();
glMultMatrixd(a_transform);
glEnable(GL_COLOR_MATERIAL);
glColor3f(a_red, a_green, a_blue);
GLUquadricObj* sphere = gluNewQuadric();
gluSphere(sphere, a_radius, 32, 32);
gluDeleteQuadric(sphere);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(a_frameSize, 0.00, 0.00);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(0.00, a_frameSize, 0.00);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(0.00, 0.00, a_frameSize);
glEnd();
glEnable(GL_LIGHTING);
glPopMatrix();
};
renderSphere(masterCurrent.matrix().data(), 0.5f, 0.5f, 0.5f, HapticSphereRadius, HapticFrameSize);
renderSphere(slaveTarget.matrix().data(), 0.2f, 0.2f, 0.2f, TargetSphereRadius, TargetFrameSize);
renderSphere(slaveCurrent.matrix().data(), 0.5f, 0.5f, 0.5f, RobotSphereRadius, RobotFrameSize);
glFinish();
GLenum err = glGetError();
if (err != GL_NO_ERROR)
{
return -1;
}
return 0;
}
void teleoperationControlLoop(int a_deviceId)
{
constexpr double LinearScaling = 0.5;
constexpr double AngularScaling = 0.5;
constexpr double LinearDamping = 10.0;
constexpr double AngularDamping = 0.01;
constexpr double LinearStiffness = 4000.0 / LinearScaling;
constexpr double AngularStiffness = 4.0 / AngularScaling;
std::cout << "teleoperation thread running" << std::endl;
double masterX = 0.0;
double masterY = 0.0;
double masterZ = 0.0;
double masterRotation[3][3] = {};
double masterLinearVelocityX = 0.0;
double masterLinearVelocityY = 0.0;
double masterLinearVelocityZ = 0.0;
double masterAngularVelocityX = 0.0;
double masterAngularVelocityY = 0.0;
double masterAngularVelocityZ = 0.0;
bool teleoperationEngaged = false;
Eigen::Vector3d masterStartPosition;
Eigen::Matrix3d masterStartRotation;
Eigen::Vector3d slaveStartPosition;
Eigen::Matrix3d slaveStartRotation;
Eigen::Vector3d force;
Eigen::Vector3d torque;
Eigen::AngleAxisd previousMasterAngularMove;
if (drdStop(true, a_deviceId) < 0)
{
std::cout <<
"error: failed to stop robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
return;
}
{
std::cout <<
"error: failed to enable button emulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
return;
}
while (simulationRunning)
{
{
std::cout <<
"error: failed to get haptic position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
break;
}
if (
dhdGetLinearVelocity(&masterLinearVelocityX, &masterLinearVelocityY, &masterLinearVelocityZ, a_deviceId) < 0)
{
std::cout <<
"error: failed to retrieve master linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
break;
}
if (
dhdGetAngularVelocityRad(&masterAngularVelocityX, &masterAngularVelocityY, &masterAngularVelocityZ, a_deviceId) < 0)
{
std::cout <<
"error: failed to retrieve master angular velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
break;
}
masterCurrent.set(masterX, masterY, masterZ, masterRotation);
Eigen::Vector3d masterLinearVelocity { masterLinearVelocityX, masterLinearVelocityY, masterLinearVelocityZ };
Eigen::Vector3d masterAngularVelocity { masterAngularVelocityX, masterAngularVelocityY, masterAngularVelocityZ };
slaveCurrent = slaveRobot.current();
if (buttonEngaged && !teleoperationEngaged)
{
masterStartPosition = masterCurrent.position();
masterStartRotation = masterCurrent.rotation();
slaveStartPosition = slaveCurrent.position();
slaveStartRotation = slaveCurrent.rotation();
teleoperationEngaged = true;
}
else if (!buttonEngaged && teleoperationEngaged)
{
teleoperationEngaged = false;
}
if (!teleoperationEngaged)
{
force.setZero();
torque.setZero();
}
else
{
Eigen::Vector3d masterLinearMove = masterCurrent.position() - masterStartPosition;
Eigen::Vector3d slaveLinearMove = LinearScaling * masterLinearMove;
Eigen::AngleAxisd masterAngularMove = Transform::angleAxis(masterCurrent.rotation() * masterStartRotation.transpose());
Eigen::AngleAxisd masterAngularMoveNoFlip = Transform::noFlip(masterAngularMove, previousMasterAngularMove);
Eigen::Matrix3d slaveAngularMove { Eigen::AngleAxisd { AngularScaling * masterAngularMoveNoFlip.angle(), masterAngularMoveNoFlip.axis() } };
slaveTarget.setPosition(slaveStartPosition + slaveLinearMove);
slaveTarget.setRotation(slaveAngularMove * slaveStartRotation);
slaveRobot.setTarget(slaveTarget);
Eigen::Vector3d linearSpringLength = slaveTarget.position() - slaveCurrent.position();
Eigen::Vector3d linearSpringForce = -1.0 * LinearStiffness * linearSpringLength;
Eigen::Matrix3d angularSpringLength = slaveTarget.rotation() * slaveCurrent.rotation().transpose();
Eigen::Vector3d angularSpringTorque = -1.0 * AngularStiffness * Transform::angles(angularSpringLength);
Eigen::Matrix3d masterSpringLength = Eigen::Matrix3d { masterAngularMove } * Eigen::Matrix3d { masterAngularMoveNoFlip }.transpose();
Eigen::Vector3d masterSpringTorque = -1.0 * AngularStiffness * Transform::angles(masterSpringLength);
Eigen::Vector3d dampingForce = -1.0 * LinearDamping * masterLinearVelocity;
Eigen::Vector3d dampingTorque = -1.0 * AngularDamping * masterAngularVelocity;
force = linearSpringForce + dampingForce;
torque = angularSpringTorque + masterSpringTorque + dampingTorque;
}
{
std::cout <<
"error: failed to set haptic force (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
break;
}
}
simulationRunning = false;
std::cout << "teleoperation thread stopped" << std::endl;
}
void onWindowResized(GLFWwindow* a_window,
int a_width,
int 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;
}
switch (a_key)
{
case GLFW_KEY_ESCAPE:
case GLFW_KEY_Q:
{
simulationRunning = false;
break;
}
case GLFW_KEY_SPACE:
{
engageOverride = !engageOverride;
break;
}
}
}
void onError(int a_error,
const char* a_description)
{
std::cout << "error: " << a_description << std::endl;
}
void userInterfaceLoop()
{
if (!glfwInit())
{
std::cout << "error: failed to initialized GLFW" << std::endl;
simulationRunning = false;
return;
}
GLFWwindow *window = NULL;
glfwSetErrorCallback(onError);
const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor());
int width = static_cast<int>(0.8 * mode->height);
int height = static_cast<int>(0.5 * mode->height);
int x = static_cast<int>(0.5 * (mode->width - width));
int y = static_cast<int>(0.5 * (mode->height - height));
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(width, height, "Force Dimension - Virtual Teleoperation Example", NULL, NULL);
if (!window)
{
std::cout << "error: failed create GLFW window" << std::endl;
simulationRunning = false;
return;
}
glfwMakeContextCurrent(window);
glfwSetKeyCallback(window, onKeyPressed);
glfwSetWindowSizeCallback(window, onWindowResized);
glfwSetWindowPos(window, x, y);
glfwSwapInterval(1);
glfwShowWindow(window);
onWindowResized(window, width, height);
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);
while (simulationRunning && !glfwWindowShouldClose(window))
{
std::this_thread::sleep_for(std::chrono::milliseconds(25));
updateGraphics();
glfwSwapBuffers(window);
glfwPollEvents();
}
glfwDestroyWindow(window);
glfwTerminate();
simulationRunning = false;
}
int main()
{
std::cout << std::endl;
std::cout <<
"Force Dimension - Virtual Teleoperation Example " <<
dhdGetSDKVersionStr() <<
")" << std::endl;
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
int deviceId = drdOpen();
if (deviceId < 0)
{
std::cout <<
"error: failed to open haptic device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
uint16_t serialNumber = 0;
{
std::cout <<
"warning: failed to retrieve device serial number (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
}
std::cout << "initializing haptic device...\r";
std::cout.flush();
if (!drdIsInitialized(deviceId) && drdAutoInit(deviceId) < 0)
{
std::cout <<
"error: failed to initialize device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout <<
"connected to " <<
dhdGetSystemName(deviceId) <<
" S/N " << std::setw(5) << std::setfill(
'0') << serialNumber << std::endl;
if (!drdIsRunning(deviceId) && drdStart(deviceId) < 0)
{
std::cout <<
"error: failed to start robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << std::endl;
std::cout << "Hold the user button on the haptic device to engage teleoperation, or" << std::endl;
std::cout << "press ' ' in the UI window to engage/disengage teleoperation, or" << std::endl;
std::cout << "press 'q' in the UI window to quit." << std::endl;
std::cout << std::endl;
if (drdMoveTo(initialPosition, true, deviceId) < 0)
{
std::cout <<
"error: failed to align master with slave (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::thread teleoperationThread(teleoperationControlLoop, deviceId);
userInterfaceLoop();
teleoperationThread.join();
if (drdClose(deviceId) < 0)
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connection closed" << std::endl;
return 0;
}
#define DHD_MAX_DOF
Definition: dhdc.h:133
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID=-1)
Definition: dhdc.cpp:2040
int __SDK dhdEnableForce(uchar val, char ID=-1)
Definition: dhdc.cpp:4378
int __SDK dhdGetLinearVelocity(double *vx, double *vy, double *vz, char ID=-1)
Definition: dhdc.cpp:2191
int __SDK dhdGetSerialNumber(ushort *sn, char ID=-1)
Definition: dhdc.cpp:540
int __SDK dhdGetPositionAndOrientationFrame(double *px, double *py, double *pz, double matrix[3][3], char ID=-1)
Definition: dhdc.cpp:1923
bool __SDK dhdHasActiveGripper(char ID=-1)
Definition: dhdc.cpp:726
int __SDK dhdGetButton(int index, char ID=-1)
Definition: dhdc.cpp:1195
int __SDK dhdEmulateButton(uchar val, char ID=-1)
Definition: dhdc.cpp:2941
#define DHD_OFF
Definition: dhdc.h:129
const char *__SDK dhdGetSDKVersionStr()
Definition: dhdc.cpp:925
#define DHD_ON
Definition: dhdc.h:128
int __SDK dhdGetAngularVelocityRad(double *wx, double *wy, double *wz, char ID=-1)
Definition: dhdc.cpp:2270
const char *__SDK dhdGetSystemName(char ID=-1)
Definition: dhdc.cpp:814
const char *__SDK dhdErrorGetLastStr()
Definition: dhdError.cpp:197