#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_DISPLAY))
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
int main()
{
try {
bool reset = false;
#ifdef VISP_HAVE_X11
#elif defined(VISP_HAVE_OPENCV)
#elif defined(VISP_HAVE_GTK)
#endif
Qmiddle = (jointMin + jointMax) / 2.;
double rho = 0.1;
double rho1 = 0.3;
char legend[10];
for (unsigned int i = 0; i < 6; i++) {
sprintf(legend, "q%u", i + 1);
}
for (unsigned int i = 6; i < 12; i++)
std::cout << "Click on a dot..." << std::endl;
std::cout << cVe << std::endl;
std::cout << std::endl;
int iter = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
iter++;
v = prim_task + sec_task;
{
for (unsigned int i = 0; i < 6; i++) {
data[i] = (q[i] - Qmiddle[i]);
data[i] /= (jointMax[i] - jointMin[i]);
data[i] *= 2;
}
data[6] = -1.0;
data[7] = 1.0;
unsigned int joint = 2;
double tQmin_l0 = jointMin[joint] + rho * (jointMax[joint] - jointMin[joint]);
double tQmax_l0 = jointMax[joint] - rho * (jointMax[joint] - jointMin[joint]);
double tQmin_l1 = tQmin_l0 - rho * rho1 * (jointMax[joint] - jointMin[joint]);
double tQmax_l1 = tQmax_l0 + rho * rho1 * (jointMax[joint] - jointMin[joint]);
data[8] = 2 * (tQmin_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
data[9] = 2 * (tQmax_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
data[10] = 2 * (tQmin_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
data[11] = 2 * (tQmax_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
plot.
plot(0, iter, data);
plot.
plot(1, iter, sec_task);
}
}
return EXIT_SUCCESS;
}
std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
return EXIT_FAILURE;
}
}
#else
int main()
{
std::cout << "You do not have an Viper 850 robot connected to your computer..." << std::endl;
return EXIT_SUCCESS;
}
#endif