44 #include <visp3/core/vpPoint.h> 47 #include <visp3/core/vpMomentObject.h> 48 #include <visp3/core/vpMomentCommon.h> 49 #include <visp3/visual_features/vpFeatureMomentCommon.h> 50 #include <visp3/vs/vpServo.h> 51 #include <visp3/robot/vpSimulatorCamera.h> 52 #include <visp3/core/vpPlane.h> 53 #include <visp3/core/vpException.h> 66 if(fabs(pl.
getD())<std::numeric_limits<double>::epsilon()){
67 std::cout <<
"Invalid position:" << std::endl;
68 std::cout << cMo << std::endl;
69 std::cout <<
"Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
80 double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
81 double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
82 double A,B,C,Ad,Bd,Cd;
85 std::vector<vpPoint> vec_p,vec_p_d;
93 cMoToABC(cdMo,Ad,Bd,Cd);
95 for (
int i = 0 ; i < nbpoints ; i++){
100 vec_p_d.push_back(p);
105 cur.fromVector(vec_p);
120 mdb_dst.updateAll(dst);
122 fmdb_dst.updateAll(Ad,Bd,Cd);
130 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
131 task.
addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
134 task.
addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
136 task.
addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
143 float sampling_time = 0.010f;
153 for (
int i = 0 ; i < nbpoints ; i++){
160 cur.fromVector(vec_p);
162 mdb_cur.updateAll(cur);
164 fmdb_cur.updateAll(A,B,C);
171 }
while(( task.
getError() ).sumSquare()>0.005);
172 std::cout <<
"final error=" << ( task.
getError() ).sumSquare() << std::endl;
176 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)
Compute the error between two visual features from a subset of the possible features.
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)