Minimalist example of moment-based visual servoing with polygon and a simple robot
#include <visp3/core/vpPoint.h>
#include <iostream>
#include <limits>
#include <vector>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/vs/vpServo.h>
{
if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
std::cout << "Invalid position:" << std::endl;
std::cout << cMo << std::endl;
std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
}
}
int main()
{
try {
double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
double A, B, C, Ad, Bd, Cd;
int nbpoints = 8;
std::vector<vpPoint> vec_p,
vec_p_d;
cMoToABC(cMo, A, B, C);
cMoToABC(cdMo, Ad, Bd, Cd);
for (int i = 0; i < nbpoints; i++) {
p.track(cMo);
vec_p.push_back(p);
p.track(cdMo);
vec_p_d.push_back(p);
}
cur.fromVector(vec_p);
mdb_dst.updateAll(dst);
fmdb_dst.updateAll(Ad, Bd, Cd);
task.
addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized());
task.
addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn());
task.
addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(),
task.
addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
float sampling_time = 0.010f;
do {
vec_p.clear();
for (int i = 0; i < nbpoints; i++) {
p.track(cMo);
vec_p.push_back(p);
}
cMoToABC(cMo, A, B, C);
cur.fromVector(vec_p);
mdb_cur.updateAll(cur);
fmdb_cur.updateAll(A, B, C);
}
while ((task.
getError()).sumSquare() > 0.005);
std::cout <<
"final error=" << (task.
getError()).sumSquare() << std::endl;
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}