#include <cmath>
#include <iomanip>
#include <iostream>
inline void transpose(const double a_matrix[3][3],
double a_result[3][3])
{
a_result[0][0] = a_matrix[0][0];
a_result[1][0] = a_matrix[0][1];
a_result[2][0] = a_matrix[0][2];
a_result[0][1] = a_matrix[1][0];
a_result[1][1] = a_matrix[1][1];
a_result[2][1] = a_matrix[1][2];
a_result[0][2] = a_matrix[2][0];
a_result[1][2] = a_matrix[2][1];
a_result[2][2] = a_matrix[2][2];
}
inline void axisAngle(const double a_matrix[3][3],
double a_axis[3],
double& a_angle)
{
a_angle = acos((a_matrix[0][0] + a_matrix[1][1 ] + a_matrix[2][2] - 1.0) / 2.0);
if (a_angle < 1e-6)
{
a_axis[0] = 1.0;
a_axis[1] = 0.0;
a_axis[2] = 0.0;
return;
}
a_axis[0] = (a_matrix[2][1] - a_matrix[1][2]) / std::sqrt(std::pow(a_matrix[2][1] - a_matrix[1][2], 2.0) + std::pow(a_matrix[0][2] - a_matrix[2][0], 2.0) + std::pow(a_matrix[1][0] - a_matrix[0][1], 2.0));
a_axis[1] = (a_matrix[0][2] - a_matrix[2][0]) / std::sqrt(std::pow(a_matrix[2][1] - a_matrix[1][2], 2.0) + std::pow(a_matrix[0][2] - a_matrix[2][0], 2.0) + std::pow(a_matrix[1][0] - a_matrix[0][1], 2.0));
a_axis[2] = (a_matrix[1][0] - a_matrix[0][1]) / std::sqrt(std::pow(a_matrix[2][1] - a_matrix[1][2], 2.0) + std::pow(a_matrix[0][2] - a_matrix[2][0], 2.0) + std::pow(a_matrix[1][0] - a_matrix[0][1], 2.0));
double axisNorm = std::sqrt(std::pow(a_axis[0], 2.0) + std::pow(a_axis[1], 2.0) + std::pow(a_axis[2], 2.0));
if (axisNorm > 1e-6)
{
a_axis[0] /= axisNorm;
a_axis[1] /= axisNorm;
a_axis[2] /= axisNorm;
}
}
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 <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
{
std::cout <<
"error: failed to enable button emulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "press [space] to toggle the virtual spring" << std::endl;
std::cout << " 'q' to quit" << std::endl << std::endl;
{
std::cout <<
"error: failed to enable force rendering (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
bool centeringActive = false;
double px = 0;
double py = 0;
double pz = 0;
double rotation[3][3] = {};
double axis[3] = { 1.0, 0.0, 0.0 };
double angle = {};
double vx = 0;
double vy = 0;
double vz = 0;
double wx = 0;
double wy = 0;
double wz = 0;
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
double tx = 0.0;
double ty = 0.0;
double tz = 0.0;
double Jb[3][3] = {};
double JbT[3][3] = {};
double Jw[3][3] = {};
double JwT[3][3] = {};
bool running = true;
while (running)
{
{
std::cout << std::endl <<
"error: failed to read position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
axisAngle(rotation, axis, angle);
{
std::cout << std::endl <<
"error: failed to read linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << std::endl <<
"error: failed to read linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to read joint angles (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to compute base Jacobian matrix (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to compute wrist Jacobian matrix (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to compute gravity compensation joint torques (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
if (centeringActive)
{
constexpr double Kp = 100.0;
constexpr double Kv = 20.0;
constexpr double MaxForce = 5.0;
fx = -1.0 * Kp * px - Kv * vx;
fy = -1.0 * Kp * py - Kv * vy;
fz = -1.0 * Kp * 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;
}
transpose(Jb, JbT);
jointTorques[0] += JbT[0][0] * fx + JbT[0][1] * fy + JbT[0][2] * fz;
jointTorques[1] += JbT[1][0] * fx + JbT[1][1] * fy + JbT[1][2] * fz;
jointTorques[2] += JbT[2][0] * fx + JbT[2][1] * fy + JbT[2][2] * fz;
constexpr double Kr = 0.2;
constexpr double Kw = 0.05;
constexpr double MaxTorque = 0.05;
tx = -1.0 * Kr * angle * axis[0] - Kw * wx;
ty = -1.0 * Kr * angle * axis[1] - Kw * wy;
tz = -1.0 * Kr * angle * axis[2] - Kw * wz;
double torqueMagnitude = std::sqrt(tx * tx + ty * ty + tz * tz);
if (torqueMagnitude > MaxTorque)
{
double torqueRatio = MaxTorque / torqueMagnitude;
tx *= torqueRatio;
ty *= torqueRatio;
tz *= torqueRatio;
}
transpose(Jw, JwT);
jointTorques[3] += JwT[0][0] * tx + JwT[0][1] * ty + JwT[0][2] * tz;
jointTorques[4] += JwT[1][0] * tx + JwT[1][1] * ty + JwT[1][2] * tz;
jointTorques[5] += JwT[2][0] * tx + JwT[2][1] * ty + JwT[2][2] * tz;
}
{
std::cout <<
"error: failed to apply joint torques (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << std::endl <<
"error: failed to read force (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
if (time - lastDisplayUpdateTime > 0.1)
{
lastDisplayUpdateTime = time;
std::cout << ((centeringActive) ? "ACTIVE" : "------") << " | ";
std::cout << std::showpos << std::fixed << std::setprecision(3);
std::cout << "f (" << std::setw(6) << fx << " " << std::setw(6) << fy << " " << std::setw(6) << fz << ") N | ";
std::cout << "t (" << std::setw(6) << tx << " " << std::setw(6) << ty << " " << std::setw(6) << tz << ") Nm | ";
std::cout <<
"freq " << std::noshowpos << std::setprecision(2) << std::setw(4) <<
dhdGetComFreq() <<
" kHz \r";
std::cout.flush();
}
{
{
case ' ':
{
centeringActive = !centeringActive;
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;
}