#include <cmath>
#include <iostream>
#include <iomanip>
int main()
{
std::cout.rdbuf()->pubsetbuf(nullptr, 512);
std::cout << std::nounitbuf;
std::ios_base::sync_with_stdio(false);
std::cout << std::endl;
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
if (deviceCount < 2)
{
std::cout << "error: not enough devices available" << std::endl;
return -1;
}
if (masterId < 0)
{
std::cout <<
"error: failed to open master device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
if (slaveId < 0)
{
std::cout <<
"error: failed to open slave device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to start robotic regulation on the slave device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
double positionCenter[
DHD_MAX_DOF] = {};
if (
drdMoveTo(positionCenter,
true, slaveId) < 0)
{
std::cout <<
"error: failed to move slave device to the center of its workspace (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to enable force on master device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to enable button emulation on master device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout <<
"using " <<
dhdGetSystemName(masterId) <<
" as the master device" << std::endl;
std::cout <<
"using " <<
dhdGetSystemName(slaveId) <<
" as the slave device" << std::endl << std::endl;
std::cout << "hold BUTTON to engage teleoperation, or" << std::endl;
std::cout << "press [space] to engage/disengage teleoperation, or" << std::endl;
std::cout << "press 'q' to quit." << std::endl << std::endl;
bool teleoperationEngaged = false;
bool engageOverride = false;
double pxMasterOrigin = 0.0;
double pyMasterOrigin = 0.0;
double pzMasterOrigin = 0.0;
double pxSlaveOrigin = 0.0;
double pySlaveOrigin = 0.0;
double pzSlaveOrigin = 0.0;
bool running = true;
while (running)
{
double pxMaster = 0.0;
double pyMaster = 0.0;
double pzMaster = 0.0;
{
std::cout <<
"error: failed to get master position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
double vxMaster = 0.0;
double vyMaster = 0.0;
double vzMaster = 0.0;
{
std::cout <<
"error: failed to retrieve master linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
double pxSlave = 0.0;
double pySlave = 0.0;
double pzSlave = 0.0;
{
std::cout <<
"error: failed to get slave position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
bool buttonEngaged = (
dhdGetButton(0, masterId) !=
DHD_OFF) || engageOverride;
if (buttonEngaged && !teleoperationEngaged)
{
pxMasterOrigin = pxMaster;
pyMasterOrigin = pyMaster;
pzMasterOrigin = pzMaster;
pxSlaveOrigin = pxSlave;
pySlaveOrigin = pySlave;
pzSlaveOrigin = pzSlave;
teleoperationEngaged = true;
std::cout << "teleoperation engaged \r";
}
else if (!buttonEngaged && teleoperationEngaged)
{
teleoperationEngaged = false;
std::cout << "teleoperation disengaged \r";
}
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
if (teleoperationEngaged)
{
constexpr double LinearScaling = 0.5;
constexpr double LinearDamping = 2.0;
constexpr double LinearStiffness = 1000.0 / LinearScaling;
double pxSlaveDesiredTarget = pxSlaveOrigin + LinearScaling * (pxMaster - pxMasterOrigin);
double pySlaveDesiredTarget = pySlaveOrigin + LinearScaling * (pyMaster - pyMasterOrigin);
double pzSlaveDesiredTarget = pzSlaveOrigin + LinearScaling * (pzMaster - pzMasterOrigin);
constexpr double SlaveWorkspaceRadius = 0.04;
double pxSlaveTarget = pxSlaveDesiredTarget;
double pySlaveTarget = pySlaveDesiredTarget;
double pzSlaveTarget = pzSlaveDesiredTarget;
double slaveDistanceFromCenter = std::sqrt(pxSlaveDesiredTarget * pxSlaveDesiredTarget + pySlaveDesiredTarget * pySlaveDesiredTarget + pzSlaveDesiredTarget * pzSlaveDesiredTarget);
if (slaveDistanceFromCenter > SlaveWorkspaceRadius)
{
double slaveScalingFactor = SlaveWorkspaceRadius / slaveDistanceFromCenter;
pxSlaveTarget *= slaveScalingFactor;
pySlaveTarget *= slaveScalingFactor;
pzSlaveTarget *= slaveScalingFactor;
}
if (
drdTrackPos(pxSlaveTarget, pySlaveTarget, pzSlaveTarget, slaveId) < 0)
{
std::cout <<
"error: failed to set slave tracking target (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
double fxSpring = -1.0 * LinearStiffness * (pxSlaveDesiredTarget - pxSlave);
double fySpring = -1.0 * LinearStiffness * (pySlaveDesiredTarget - pySlave);
double fzSpring = -1.0 * LinearStiffness * (pzSlaveDesiredTarget - pzSlave);
double fxDamping = -1.0 * LinearDamping * vxMaster;
double fyDamping = -1.0 * LinearDamping * vyMaster;
double fzDamping = -1.0 * LinearDamping * vzMaster;
fx = fxSpring + fxDamping;
fy = fySpring + fyDamping;
fz = fzSpring + fzDamping;
}
{
std::cout <<
"error: failed to set haptic force (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << "error: regulation thread not running on slave device" << std::endl;
return -1;
}
{
{
case ' ':
{
engageOverride = ! engageOverride;
break;
}
case 'q':
{
std::cout << std::endl << std::endl << "exiting at user's request" << std::endl;
running = false;
break;
}
}
}
}
{
std::cout <<
"error: failed to close the connection to the master device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to close the connection to the slave device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connections closed" << std::endl;
return 0;
}
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID=-1)
bool __SDK dhdHasGripper(char ID=-1)
int __SDK dhdEnableForce(uchar val, char ID=-1)
int __SDK dhdGetLinearVelocity(double *vx, double *vy, double *vz, char ID=-1)
void __SDK dhdSleep(double sec)
const char *__SDK dhdGetSDKVersionStr()
int __SDK dhdGetButton(int index, char ID=-1)
int __SDK dhdEmulateButton(uchar val, char ID=-1)
const char *__SDK dhdErrorGetLastStr()
const char *__SDK dhdGetSystemName(char ID=-1)
int __SDK dhdGetPosition(double *px, double *py, double *pz, char ID=-1)
int __SDK dhdGetAvailableCount()
int __SDK drdOpen()
Definition: drdc.cpp:127
int __SDK drdStart(char ID=-1)
Definition: drdc.cpp:919
int __SDK drdClose(char ID=-1)
Definition: drdc.cpp:283
int __SDK drdTrackPos(double px, double py, double pz, char ID=-1)
Definition: drdc.cpp:1626
int __SDK drdMoveTo(double p[DHD_MAX_DOF], bool block=true, char ID=-1)
Definition: drdc.cpp:1422
bool __SDK drdIsRunning(char ID=-1)
Definition: drdc.cpp:371