#include <cmath>
#include <iomanip>
#include <iostream>
void projectPointOnSegment(double a_point[3],
double a_A[3],
double a_B[3],
double a_projection[3])
{
double Ap[] =
{
a_point[0] - a_A[0],
a_point[1] - a_A[1],
a_point[2] - a_A[2]
};
double AB[3] =
{
a_B[0] - a_A[0],
a_B[1] - a_A[1],
a_B[2] - a_A[2]
};
double norm = std::sqrt(AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2]);
if (norm <= 1e-6)
{
a_projection[0] = a_point[0];
a_projection[1] = a_point[1];
a_projection[2] = a_point[2];
return;
}
double direction[3] =
{
AB[0] / norm,
AB[1] / norm,
AB[2] / norm,
};
double projectionRatio = Ap[0] * direction[0] + Ap[1] * direction[1] + Ap[2] * direction[2];
if (projectionRatio < 0.0)
{
a_projection[0] = a_A[0];
a_projection[1] = a_A[1];
a_projection[2] = a_A[2];
}
else if (projectionRatio > norm)
{
a_projection[0] = a_B[0];
a_projection[1] = a_B[1];
a_projection[2] = a_B[2];
}
else
{
a_projection[0] = a_A[0] + projectionRatio * direction[0];
a_projection[1] = a_A[1] + projectionRatio * direction[1];
a_projection[2] = a_A[2] + projectionRatio * direction[2];
}
}
void projectForceOnDirection(double a_force[3],
double a_A[3],
double a_B[3],
double a_projectedForce[3])
{
double AB[3] =
{
a_B[0] - a_A[0],
a_B[1] - a_A[1],
a_B[2] - a_A[2]
};
double norm = std::sqrt(AB[0] * AB[0] + AB[1] * AB[1] + AB[2] * AB[2]);
if (norm <= 1e-6)
{
a_projectedForce[0] = 0.0;
a_projectedForce[1] = 0.0;
a_projectedForce[2] = 0.0;
return;
}
double direction[3] =
{
AB[0] / norm,
AB[1] / norm,
AB[2] / norm,
};
double projectionRatio = a_force[0] * direction[0] + a_force[1] * direction[1] + a_force[2] * direction[2];
a_projectedForce[0] = projectionRatio * direction[0];
a_projectedForce[1] = projectionRatio * direction[1];
a_projectedForce[2] = projectionRatio * direction[2];
}
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 << "press BUTTON or 'p' to define the segment extremities" << std::endl;
std::cout << " 'c' to clear the current segment" << std::endl;
std::cout << " 'q' to quit" << std::endl << std::endl;
std::cout << "select first segment point \r";
std::cout.flush();
{
std::cout <<
"error: failed to enable force rendering (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
{
std::cout <<
"error: failed to enable button emulation (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
double numPoints = 0;
double A[3] = {};
double B[3] = {};
double position[3] = {};
double velocity[3] = {};
double projectedPosition[3] = {};
double force[3] = {};
double projectedForce[3] = {};
bool previousUserButton = false;
bool running = true;
while(running)
{
if (
dhdGetPosition(&(position[0]), &(position[1]), &(position[2])) < 0)
{
std::cout <<
"error: failed to retrieve device position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout <<
"error: failed to retrieve linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
if (numPoints >= 2)
{
constexpr double Kp = 2000.0;
constexpr double Kv = 20.0;
projectPointOnSegment(position, A, B, projectedPosition);
force[0] = Kp * (projectedPosition[0] - position[0]) - Kv * velocity[0];
force[1] = Kp * (projectedPosition[1] - position[1]) - Kv * velocity[1];
force[2] = Kp * (projectedPosition[2] - position[2]) - Kv * velocity[2];
projectForceOnDirection(force, position, projectedPosition, projectedForce);
}
else
{
projectedForce[0] = 0.0;
projectedForce[1] = 0.0;
projectedForce[2] = 0.0;
}
{
std::cout <<
"error: failed to apply forces (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
bool addPoint = (!userButton && previousUserButton);
previousUserButton = userButton;
{
{
case 'p':
{
addPoint = true;
break;
}
case 'c':
{
numPoints = 0;
std::cout << "select first segment point \r";
std::cout.flush();
break;
}
case 'q':
{
std::cout << std::endl << std::endl << "exiting at user's request" << std::endl;
running = false;
break;
}
}
}
if (addPoint)
{
if (numPoints == 0)
{
numPoints = 1;
std::cout << "select second segment point \r";
std::cout.flush();
}
else if (numPoints == 1)
{
numPoints = 2;
std::cout << "segment constraint active \r";
std::cout.flush();
}
}
}
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return -1;
}
std::cout << "connection closed" << std::endl;
return 0;
}
char __SDK dhdKbGet()
Definition: dhdc.cpp:4743
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 dhdClose(char ID=-1)
Definition: dhdc.cpp:443
void __SDK dhdSleep(double sec)
Definition: dhdc.cpp:4754
bool __SDK dhdHasActiveGripper(char ID=-1)
Definition: dhdc.cpp:726
int __SDK dhdGetButton(int index, char ID=-1)
Definition: dhdc.cpp:1195
@ DHD_NO_ERROR
Definition: dhdc.h:52
int __SDK dhdEmulateButton(uchar val, char ID=-1)
Definition: dhdc.cpp:2941
bool __SDK dhdKbHit()
Definition: dhdc.cpp:4731
const char *__SDK dhdGetSDKVersionStr()
Definition: dhdc.cpp:925
int __SDK dhdOpen()
Definition: dhdc.cpp:265
#define DHD_ON
Definition: dhdc.h:128
int __SDK dhdGetPosition(double *px, double *py, double *pz, char ID=-1)
Definition: dhdc.cpp:1614
const char *__SDK dhdGetSystemName(char ID=-1)
Definition: dhdc.cpp:814
const char *__SDK dhdErrorGetLastStr()
Definition: dhdError.cpp:197