Joint limits avoidance using a gradient projection approach.
#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) && 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
double rho = 0.15;
for (unsigned int i = 0; i < 6; i++) {
Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
}
Qmiddle = (Qmin + Qmax) / 2.;
double rho1 = 0.1;
for (unsigned int i = 0; i < 6; i++) {
tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
}
char legend[10];
for (unsigned int i = 0; i < 6; i++) {
sprintf(legend, "q%u", i + 1);
}
for (unsigned int i = 6; i < 10; i++)
double beta = 1;
std::cout << " Give the parameters beta (1) : ";
std::cin >> beta;
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++;
double h_s = 0;
{
de2dt = 0;
e2 = 0;
for (unsigned int i = 0; i < 6; i++) {
double S = 0;
if (q[i] > tQmax[i])
S = q[i] - tQmax[i];
if (q[i] < tQmin[i])
S = q[i] - tQmin[i];
double D = (Qmax[i] - Qmin[i]);
e2[i] = S / D;
}
h_s = beta * h_s / 2.0;
e2 *= beta;
std::cout << "Cost function h_s: " << h_s << std::endl;
}
v = prim_task + sec_task;
{
for (unsigned int i = 0; i < 6; i++) {
data[i] = (q[i] - Qmiddle[i]);
data[i] /= (Qmax[i] - Qmin[i]);
data[i] *= 2;
}
unsigned int joint = 2;
data[6] = 2 * (tQmin[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
data[7] = 2 * (tQmax[joint] - Qmiddle[joint]) / (Qmax[joint] - Qmin[joint]);
data[8] = -1;
data[9] = 1;
plot.
plot(0, iter, data);
plot.
plot(1, 0, iter, h_s);
}
}
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