Minimalist example of moment-based visual servoing with polygon and a simple robot
#include <visp3/core/vpConfig.h>
#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>
#ifdef ENABLE_VISP_NAMESPACE
#endif
{
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);
dst.fromVector(vec_p_d);
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;
robot.setSamplingTime(sampling_time);
do {
wMc = robot.
getPosition();
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;
}
}
class that defines what is a visual feature
virtual vpColVector error(const vpBasicFeature &s_star, unsigned int select=FEATURE_ALL)
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ divideByZeroError
Division by zero.
Functionality computation for in-plane rotation moment feature : computes the interaction matrix asso...
static unsigned int selectC6()
static unsigned int selectC4()
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
This class defines the container for a plane geometrical structure.
void changeFrame(const vpHomogeneousMatrix &cMo)
void setABCD(double a, double b, double c, double d)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()