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);
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;
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;
void setPosition(const vpHomogeneousMatrix &wMc)
VISP_EXPORT int wait(double t0, double t)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
void setABCD(const double a, const double b, const double c, const double d)
Class for generic objects.
vpHomogeneousMatrix inverse() const
virtual vpColVector error(const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL)
vpHomogeneousMatrix getPosition() const
VISP_EXPORT double measureTimeMs()
Class that defines what is a point.
class that defines what is a visual feature
virtual void setSamplingTime(const double &delta_t)
Functionality computation for in-plane rotation moment feature : computes the interaction matrix asso...
static unsigned int selectC4()
vpColVector computeControlLaw()
void changeFrame(const vpHomogeneousMatrix &cMo)
static std::vector< double > getMu3(vpMomentObject &object)
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
Implementation of column vector and the associated operations.
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector getError() const
This class defines the container for a plane geometrical structure.
static unsigned int selectC6()
void setServo(const vpServoType &servo_type)