#include <cmath>
#include <iomanip>
#include <iostream>
#include "drdc.h"
template<typename T>
constexpr const T& clamp(const T& a_val, const T& a_minVal, const T& a_maxVal)
{
return (a_val < a_minVal) ? a_minVal : (a_maxVal < a_val) ? a_maxVal : a_val;
}
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;
if (drdOpen() < 0)
{
std::cout <<
"error: failed to open device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
if (!drdIsSupported())
{
std::cout << "exiting..." << std::endl;
drdClose();
return -1;
}
std::cout <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
if (!drdIsInitialized() && drdAutoInit() < 0)
{
std::cout <<
"error: failed to initialize device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
else if (drdStart() < 0)
{
std::cout <<
"error: failed to start robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
if (drdMoveTo(positionCenter) < 0)
{
std::cout <<
"error: failed to move the device (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
if (drdStop(true) < 0)
{
std::cout <<
"error: failed to stop robotic regulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to enable button emulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "press 'q' to quit" << std::endl << std::endl;
bool previousUserButton = false;
double px = 0;
double py = 0;
double pz = 0;
double ra = 0;
double rb = 0;
double rg = 0;
double pg = 0;
double vx = 0.0;
double vy = 0.0;
double vz = 0.0;
double wa = 0.0;
double wb = 0.0;
double wg = 0.0;
double vg = 0.0;
double pxHold = 0;
double pyHold = 0;
double pzHold = 0;
double raHold = 0;
double rbHold = 0;
double rgHold = 0;
double pgHold = 0;
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
double qa = 0.0;
double qb = 0.0;
double qg = 0.0;
double fg = 0.0;
while (true)
{
{
std::cout <<
"error: failed to retrieve device position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to retrieve device gripper opening (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to retrieve linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to retrieve gripper velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to retrieve angular velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
wa = jointAnglesVelocity[3];
wb = jointAnglesVelocity[4];
wg = jointAnglesVelocity[5];
bool userButtonPressed = (userButton && !previousUserButton);
bool userButtonReleased = (!userButton && previousUserButton);
previousUserButton = userButton;
if (userButtonReleased)
{
pxHold = px;
pyHold = py;
pzHold = pz;
raHold = ra;
rbHold = rb;
rgHold = rg;
pgHold = pg;
}
if (userButton)
{
fx = 0.0;
fy = 0.0;
fz = 0.0;
qa = 0.0;
qb = 0.0;
qg = 0.0;
fg = 0.0;
}
else
{
constexpr double Kp = 100.0;
constexpr double Kv = 20.0;
constexpr double MaxForce = 2.0;
constexpr double Kr = 0.01;
constexpr double Kw = 0.04;
constexpr double MaxTorque = 0.02;
constexpr double Kgp = 100.0;
constexpr double Kgv = 5.0;
constexpr double MaxGripperForce = 1.0;
fx = Kp * (pxHold - px) - Kv * vx;
fy = Kp * (pyHold - py) - Kv * vy;
fz = Kp * (pzHold - pz) - Kv * vz;
double forceMagnitude = std::sqrt(fx * fx + fy * fy + fz * fz);
if (forceMagnitude > MaxForce)
{
double forceRatio = MaxForce / forceMagnitude;
fx *= forceRatio;
fy *= forceRatio;
fz *= forceRatio;
}
qa = clamp(Kr * (raHold - ra) - Kw * wa, -MaxTorque, MaxTorque);
qb = clamp(Kr * (rbHold - rb) - Kw * wb, -MaxTorque, MaxTorque);
qg = clamp(Kr * (rgHold - rg) - Kw * wg, -MaxTorque, MaxTorque);
fg = clamp(Kgp * (pgHold - pg) - Kgv * vg, -MaxGripperForce, MaxGripperForce);
}
{
std::cout <<
"error: failed to apply forces (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
if (time - lastDisplayUpdateTime > 0.1)
{
lastDisplayUpdateTime = time;
if (userButton)
{
std::cout << "current status: in use \r";
}
else
{
std::cout << "current status: holding \r";
}
std::cout.flush();
}
{
std::cout << std::endl << std::endl << "exiting at user's request" << std::endl;
break;
}
}
if (drdClose() < 0)
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connection closed" << std::endl;
return 0;
}