#include <iomanip>
#include <iostream>
int moveToCenter(double a_moveBase,
double a_moveWrist,
double a_moveGripper)
{
{
std::cout <<
"error: failed to set base regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to set wrist regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to set gripper regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to start robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to move the device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to stop robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
return 0;
}
int main(int argc,
char *argv[])
{
std::cout.rdbuf()->pubsetbuf(nullptr, 512);
std::cout << std::nounitbuf;
std::ios_base::sync_with_stdio(false);
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
{
std::cout <<
"error: failed to open device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout << "exiting..." << std::endl;
return -1;
}
std::cout <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
{
std::cout <<
"error: failed to initialize device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to start robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to move the device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to stop robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "press 'q' to quit" << std::endl;
std::cout << " 'c' to re-center the end-effector (all axes)" << std::endl;
std::cout << " 'p' to re-center the base positions only" << std::endl;
std::cout << " 'r' to re-center the wrist rotation only" << std::endl;
std::cout << " 'g' to close the gripper only" << std::endl << std::endl;
double px = 0.0;
double py = 0.0;
double pz = 0.0;
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
bool running = true;
while (running)
{
{
std::cout <<
"error: failed to render force (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << std::endl <<
"error: failed to read position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << std::endl <<
"error: failed to read forces (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
if (time - lastDisplayUpdateTime > 0.1)
{
lastDisplayUpdateTime = time;
std::cout << std::showpos << std::fixed << std::setprecision(3);
std::cout << "p (" << std::setw(6) << px << " " << std::setw(6) << py << " " << std::setw(6) << pz << ") m | ";
std::cout << "f (" << std::setw(6) << fx << " " << std::setw(6) << fy << " " << std::setw(6) << fz << ") N | ";
std::cout <<
"freq " << std::noshowpos << std::setprecision(2) << std::setw(4) <<
dhdGetComFreq() <<
" kHz \r";
std::cout.flush();
}
{
{
case 'c':
{
if (moveToCenter(true, true, true) < 0)
{
running = false;
}
break;
}
case 'p':
{
if (moveToCenter(true, false, false) < 0)
{
running = false;
}
break;
}
case 'r':
{
if (moveToCenter(false, true, false) < 0)
{
running = false;
}
break;
}
case 'g':
{
if (moveToCenter(false, false, true) < 0)
{
running = false;
}
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 (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connection closed" << std::endl;
return 0;
}
bool __SDK dhdHasActiveWrist(char ID)
const char *__SDK dhdGetSystemName(char ID)
bool __SDK dhdHasActiveGripper(char ID)
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID)
int __SDK dhdGetForce(double *fx, double *fy, double *fz, char ID)
void __SDK dhdSleep(double sec)
int __SDK dhdGetPosition(double *px, double *py, double *pz, char ID)
const char *__SDK dhdGetSDKVersionStr()
double __SDK dhdGetComFreq(char ID)
double __SDK dhdGetTime()
const char *__SDK dhdErrorGetLastStr()
int __SDK drdOpen()
Definition: drdc.cpp:220
bool __SDK drdIsInitialized(char ID)
Definition: drdc.cpp:556
int __SDK drdAutoInit(char ID)
Definition: drdc.cpp:590
int __SDK drdMoveTo(double p[DHD_MAX_DOF], bool block, char ID)
Definition: drdc.cpp:1672
int __SDK drdClose(char ID)
Definition: drdc.cpp:367
bool __SDK drdIsSupported(char ID)
Definition: drdc.cpp:423
int __SDK drdRegulatePos(bool on, char ID)
Definition: drdc.cpp:1119
int __SDK drdRegulateRot(bool on, char ID)
Definition: drdc.cpp:1173
int __SDK drdStart(char ID)
Definition: drdc.cpp:1087
int __SDK drdStop(bool frc, char ID)
Definition: drdc.cpp:2420
int __SDK drdRegulateGrip(bool on, char ID)
Definition: drdc.cpp:1227