autocenter.cpp
////////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2001-2023 Force Dimension, Switzerland.
// All Rights Reserved.
//
// Force Dimension SDK 3.17.1
//
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
///
/// This example illustrates how to use the robotic library to move the base,
/// wrist or gripper axes of a Force Dimension haptic device back to a given
/// position (the center of the workspace in this case).
///
/// It starts by putting the haptic device in a gravity compensation loop, and
/// engages the robotic regulation selectively on the relevant axes group (base,
/// wrist or gripper) when a keyboard input from the user requests it.
///
////////////////////////////////////////////////////////////////////////////////
// C++ library headers
#include <iomanip>
#include <iostream>
// Force Dimension SDK library header
#include "drdc.h"
////////////////////////////////////////////////////////////////////////////////
///
/// This function centers selected axes groups on the device.
///
/// \param a_moveBase
/// If \b true, center the device base position.
/// If \b false, leave the base free to be moved by the user.
///
/// \param a_moveWrist
/// If \b true, center the device wrist rotation.
/// If \b false, leave the wrist free to be moved by the user.
///
/// \param a_moveGripper
/// If \b true, close the device gripper.
/// If \b false, leave the gripper free to be moved by the user.
///
/// \return
/// 0 on success, -1 on failure
///
////////////////////////////////////////////////////////////////////////////////
int moveToCenter(double a_moveBase,
double a_moveWrist,
double a_moveGripper)
{
// Enable/disable base regulation.
if (drdRegulatePos(a_moveBase) < 0)
{
std::cout << "error: failed to set base regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Enable/disable wrist regulation.
if (dhdHasActiveWrist() && drdRegulateRot(a_moveWrist) < 0)
{
std::cout << "error: failed to set wrist regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Enable/disable gripper regulation.
if (dhdHasActiveGripper() && drdRegulateGrip(a_moveGripper) < 0)
{
std::cout << "error: failed to set gripper regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Start robotic regulation.
if (drdStart() < 0)
{
std::cout << "error: failed to start robotic regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Move to the center of the workspace.
double positionCenter[DHD_MAX_DOF] = {};
if (drdMoveTo(positionCenter, true) < 0)
{
std::cout << "error: failed to move the device (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Stop the regulation thread but leaves the forces enabled on the device.
if (drdStop(true) < 0)
{
std::cout << "error: failed to stop robotic regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
return 0;
}
////////////////////////////////////////////////////////////////////////////////
int main(int argc,
char *argv[])
{
// Optimize console output performance.
std::cout.rdbuf()->pubsetbuf(nullptr, 512);
std::cout << std::nounitbuf;
std::ios_base::sync_with_stdio(false);
// Display version information.
std::cout << "Automatic Centering Example " << dhdGetSDKVersionStr() << std::endl;
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
// Open the first available haptic device.
if (drdOpen() < 0)
{
std::cout << "error: failed to open device (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Make sure the device is compatible with the robotics library (DRD).
{
std::cout << "unsupported device type " << dhdGetSystemName() << std::endl;
std::cout << "exiting..." << std::endl;
dhdSleep(2.0);
return -1;
}
// Display the device type.
std::cout << dhdGetSystemName() << " device detected" << std::endl << std::endl;
// Perform automatic initialization of the device if required.
if (!drdIsInitialized() && drdAutoInit() < 0)
{
std::cout << "error: failed to initialize device (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
else if (drdStart() < 0)
{
std::cout << "error: failed to start robotic regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Move to the center of the workspace.
double positionCenter[DHD_MAX_DOF] = {};
if (drdMoveTo(positionCenter) < 0)
{
std::cout << "error: failed to move the device (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Stop the regulation thread but leaves the forces enabled on the device.
if (drdStop(true) < 0)
{
std::cout << "error: failed to stop robotic regulation (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Display user instructions.
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;
// Allocate and initialize haptic loop variables.
double px = 0.0;
double py = 0.0;
double pz = 0.0;
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
double lastDisplayUpdateTime = dhdGetTime();
// Run the haptic loop.
bool running = true;
while (running)
{
// Apply zero force on the haptic device.
if (dhdSetForceAndTorqueAndGripperForce(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) < DHD_NO_ERROR)
{
std::cout << "error: failed to render force (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
break;
}
// Retrieve the haptic device position.
if (dhdGetPosition(&px, &py, &pz) < DHD_NO_ERROR)
{
std::cout << std::endl << "error: failed to read position (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
break;
}
// Retrieve the haptic device force.
if (dhdGetForce(&fx, &fy, &fz) < DHD_NO_ERROR)
{
std::cout << std::endl << "error: failed to read forces (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
break;
}
// Periodically display the haptic device status and flush stdout.
double time = dhdGetTime();
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();
}
// Process user input.
if (dhdKbHit())
{
switch (dhdKbGet())
{
// Center base, wrist and gripper.
case 'c':
{
if (moveToCenter(true, true, true) < 0)
{
running = false;
}
break;
}
// Center the base.
case 'p':
{
if (moveToCenter(true, false, false) < 0)
{
running = false;
}
break;
}
// Center the wrist.
case 'r':
{
if (moveToCenter(false, true, false) < 0)
{
running = false;
}
break;
}
// Center the gripper.
case 'g':
{
if (moveToCenter(false, false, true) < 0)
{
running = false;
}
break;
}
// Quit the application.
case 'q':
{
std::cout << std::endl << std::endl << "exiting at user's request" << std::endl;
running = false;
break;
}
}
}
}
// Close the connection to the haptic device.
if (drdClose() < 0)
{
std::cout << "error: failed to close the connection (" << dhdErrorGetLastStr() << ")" << std::endl;
dhdSleep(2.0);
return -1;
}
// Report success.
std::cout << "connection closed" << std::endl;
return 0;
}
drdRegulateGrip
int __SDK drdRegulateGrip(bool on, char ID=-1)
Definition: drdc.cpp:1033
dhdGetPosition
int __SDK dhdGetPosition(double *px, double *py, double *pz, char ID=-1)
drdClose
int __SDK drdClose(char ID=-1)
Definition: drdc.cpp:283
dhdErrorGetLastStr
const char *__SDK dhdErrorGetLastStr()
dhdGetComFreq
double __SDK dhdGetComFreq(char ID=-1)
drdIsInitialized
bool __SDK drdIsInitialized(char ID=-1)
Definition: drdc.cpp:461
dhdSetForceAndTorqueAndGripperForce
int __SDK dhdSetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double fg, char ID=-1)
drdStop
int __SDK drdStop(bool frc=false, char ID=-1)
Definition: drdc.cpp:2069
drdStart
int __SDK drdStart(char ID=-1)
Definition: drdc.cpp:919
dhdKbHit
bool __SDK dhdKbHit()
dhdGetSystemName
const char *__SDK dhdGetSystemName(char ID=-1)
dhdGetForce
int __SDK dhdGetForce(double *fx, double *fy, double *fz, char ID=-1)
dhdKbGet
char __SDK dhdKbGet()
dhdSleep
void __SDK dhdSleep(double sec)
dhdHasActiveGripper
bool __SDK dhdHasActiveGripper(char ID=-1)
drdMoveTo
int __SDK drdMoveTo(double p[DHD_MAX_DOF], bool block=true, char ID=-1)
Definition: drdc.cpp:1422
drdRegulatePos
int __SDK drdRegulatePos(bool on, char ID=-1)
Definition: drdc.cpp:945
drdIsSupported
bool __SDK drdIsSupported(char ID=-1)
Definition: drdc.cpp:352
drdc.h
DRD header file.
drdAutoInit
int __SDK drdAutoInit(char ID=-1)
Definition: drdc.cpp:489
dhdGetSDKVersionStr
const char *__SDK dhdGetSDKVersionStr()
drdRegulateRot
int __SDK drdRegulateRot(bool on, char ID=-1)
Definition: drdc.cpp:989
dhdGetTime
double __SDK dhdGetTime()
drdOpen
int __SDK drdOpen()
Definition: drdc.cpp:127
dhdHasActiveWrist
bool __SDK dhdHasActiveWrist(char ID=-1)