Joint limits avoidance by stopping the motion on axis near the joint limits.
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits>
#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;
double Tloop = 1. / 60.f;
switch (fps) {
Tloop = 1.f / 15.f;
break;
Tloop = 1.f / 30.f;
break;
Tloop = 1.f / 60.f;
break;
Tloop = 1.f / 120.f;
break;
default:
break;
}
std::cout << "Tloop: " << Tloop << std::endl;
#ifdef VISP_HAVE_X11
#elif defined(VISP_HAVE_OPENCV)
#elif defined(VISP_HAVE_GTK)
#endif
double rho = 0.25;
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]);
}
plot.
initRange(0, 0, 200, 1, -1.2, 1.2, 0.1);
plot.
initRange(1, 0, 200, 1, -0.01, 0.01, 0.05);
char legend[10];
for (unsigned int i = 0; i < 6; i++) {
sprintf(legend, "q%u", i + 1);
sprintf(legend, "q%u", i + 1);
}
for (unsigned int i = 6; i < 10; i++)
std::cout << "Click on a dot..." << std::endl;
std::cout << cVe << std::endl;
std::cout << std::endl;
double lambda = 0.8;
int iter = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for (;;) {
iter++;
double Tv = (double)(t_0 - t_1) / 1000.0;
std::cout << "Tv: " << Tv << std::endl;
t_1 = t_0;
dc1394video_frame_t *frame = g.
dequeue(I);
qpre = q;
qpre += -lambda * prim_task * (4 * Tloop);
pb = 0;
unsigned int npb = 0;
for (unsigned int i = 0; i < 6; i++) {
if (q[i] < tQmin[i])
if (fabs(Qmin[i] - q[i]) > fabs(Qmin[i] - qpre[i])) {
pb[i] = 1;
npb++;
std::cout << "Joint " << i << " near limit " << std::endl;
}
if (q[i] > tQmax[i]) {
if (fabs(Qmax[i] - q[i]) > fabs(Qmax[i] - qpre[i])) {
pb[i] = 1;
npb++;
std::cout << "Joint " << i << " near limit " << std::endl;
}
}
}
unsigned int dimKernelL = kernelJ1.
getCols();
if (npb != 0) {
unsigned int k = 0;
for (unsigned int j = 0; j < 6; j++)
if (std::fabs(pb[j] - 1) <= std::numeric_limits<double>::epsilon()) {
for (unsigned int i = 0; i < dimKernelL; i++)
E[k][i] = kernelJ1[j][i];
S[k] = -prim_task[j];
k++;
}
Ep = E.
t() * (E * E.
t()).pseudoInverse();
a0 = Ep * S;
e2 = (kernelJ1 * a0);
} else {
e2 = 0;
}
v = -lambda * (prim_task + e2);
{
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);
}
}
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