45 #include <visp3/core/vpPoint.h>
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpMomentCommon.h>
52 #include <visp3/core/vpMomentObject.h>
53 #include <visp3/core/vpPlane.h>
54 #include <visp3/robot/vpSimulatorCamera.h>
55 #include <visp3/visual_features/vpFeatureMomentCommon.h>
56 #include <visp3/vs/vpServo.h>
68 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
69 std::cout <<
"Invalid position:" << std::endl;
70 std::cout << cMo << std::endl;
71 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
82 double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
83 double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
84 double A, B, C, Ad, Bd, Cd;
87 std::vector<vpPoint> vec_p,
95 cMoToABC(cMo, A, B, C);
96 cMoToABC(cdMo, Ad, Bd, Cd);
98 for (
int i = 0; i < nbpoints; i++) {
103 vec_p_d.push_back(p);
109 cur.fromVector(vec_p);
114 dst.fromVector(vec_p_d);
127 mdb_dst.updateAll(dst);
129 fmdb_dst.updateAll(Ad, Bd, Cd);
137 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized());
138 task.
addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn());
141 task.
addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(),
143 task.
addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
150 float sampling_time = 0.010f;
151 robot.setSamplingTime(sampling_time);
153 robot.setPosition(wMc);
156 wMc = robot.getPosition();
160 for (
int i = 0; i < nbpoints; i++) {
165 cMoToABC(cMo, A, B, C);
167 cur.fromVector(vec_p);
169 mdb_cur.updateAll(cur);
171 fmdb_cur.updateAll(A, B, C);
178 }
while ((task.
getError()).sumSquare() > 0.005);
179 std::cout <<
"final error=" << (task.
getError()).sumSquare() << std::endl;
182 std::cout <<
"Catch an exception: " << e << std::endl;
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 emited 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)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines the simplest robot: a free flying camera.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()