42 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpMomentCommon.h>
49 #include <visp3/core/vpMomentObject.h>
50 #include <visp3/core/vpPlane.h>
51 #include <visp3/robot/vpSimulatorCamera.h>
52 #include <visp3/visual_features/vpFeatureMomentCommon.h>
53 #include <visp3/vs/vpServo.h>
65 if (fabs(pl.
getD()) < std::numeric_limits<double>::epsilon()) {
66 std::cout <<
"Invalid position:" << std::endl;
67 std::cout << cMo << std::endl;
68 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
79 double x[8] = { 1, 3, 4, -1, -3, -2, -1, 1 };
80 double y[8] = { 0, 1, 4, 4, -2, -2, 1, 0 };
81 double A, B, C, Ad, Bd, Cd;
84 std::vector<vpPoint> vec_p,
92 cMoToABC(cMo, A, B, C);
93 cMoToABC(cdMo, Ad, Bd, Cd);
95 for (
int i = 0; i < nbpoints; i++) {
100 vec_p_d.push_back(p);
106 cur.fromVector(vec_p);
111 dst.fromVector(vec_p_d);
124 mdb_dst.updateAll(dst);
126 fmdb_dst.updateAll(Ad, Bd, Cd);
134 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(), fmdb_dst.getFeatureGravityNormalized());
135 task.
addFeature(fmdb_cur.getFeatureAn(), fmdb_dst.getFeatureAn());
138 task.
addFeature(fmdb_cur.getFeatureCInvariant(), fmdb_dst.getFeatureCInvariant(),
140 task.
addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
147 float sampling_time = 0.010f;
148 robot.setSamplingTime(sampling_time);
150 robot.setPosition(wMc);
153 wMc = robot.getPosition();
157 for (
int i = 0; i < nbpoints; i++) {
162 cMoToABC(cMo, A, B, C);
164 cur.fromVector(vec_p);
166 mdb_cur.updateAll(cur);
168 fmdb_cur.updateAll(A, B, C);
175 }
while ((task.
getError()).sumSquare() > 0.005);
176 std::cout <<
"final error=" << (task.
getError()).sumSquare() << std::endl;
180 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 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()