Test for reading the Virtuose's and Glove joint positions.
#include <stdlib.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <visp3/robot/vpVirtuose.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
#if defined(VISP_HAVE_VIRTUOSE) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
int main()
{
int port = 5000;
std::vector<vpVirtuose> virtuose(4);
std::vector<vpHomogeneousMatrix> wMd(4);
for (size_t device=0; device < virtuose.size(); device ++) {
std::stringstream ss; ss << port + device;
std::string ip = "localhost#" + ss.str();
std::cout << "Connect to: " << ip << std::endl;
virtuose[device].setIpAddress(ip);
virtuose[device].init();
}
std::cout << "After init" << std::endl;
#if 0
for (size_t device=0; device < virtuose.size(); device ++) {
std::cout << "Number of joints: " << virtuose[device].getJointsNumber()
<< " Joint position: " << virtuose[device].getArticularPosition().t() << std::endl
<< " Cartesian position: " << virtuose[device].getAvatarPosition().t() << std::endl;
}
#endif
bool end = false;
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#endif
while(!end) {
for (size_t device=0; device < virtuose.size(); device ++) {
std::cout <<
"Device #" << device <<
" has position: " << wpd.
t() << std::endl;
wMd[device].buildFrom(wpd);
std::stringstream ss;
ss << device;
double X = cMd[0][3];
double Y = cMd[1][3];
double Z = cMd[2][3];
}
end = true;
}
}
for (size_t device=0; device < virtuose.size(); device ++) {
virtuose[device].close();
}
std::cout << "The end" << std::endl;
}
#else
int main() { std::cout << "You should install Virtuose API to use this binary..." << std::endl; }
#endif