48 #include <visp/vpPoint.h>
51 #include <visp/vpMomentObject.h>
52 #include <visp/vpMomentCommon.h>
53 #include <visp/vpFeatureMomentCommon.h>
54 #include <visp/vpServo.h>
55 #include <visp/vpRobotCamera.h>
56 #include <visp/vpPlane.h>
57 #include <visp/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;
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,vec_p_d;
90 cMoToABC(cdMo,Ad,Bd,Cd);
92 for (
int i = 0 ; i < nbpoints ; i++){
103 cur.fromVector(vec_p);
119 mdb_dst.updateAll(dst);
121 fmdb_dst.updateAll(Ad,Bd,Cd);
129 task.
addFeature(fmdb_cur.getFeatureGravityNormalized(),fmdb_dst.getFeatureGravityNormalized());
130 task.
addFeature(fmdb_cur.getFeatureAn(),fmdb_dst.getFeatureAn());
133 task.
addFeature(fmdb_cur.getFeatureCInvariant(),fmdb_dst.getFeatureCInvariant(),
135 task.
addFeature(fmdb_cur.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
142 float sampling_time = 0.010f;
150 for (
int i = 0 ; i < nbpoints ; i++){
158 cur.fromVector(vec_p);
160 mdb_cur.updateAll(cur);
162 fmdb_cur.updateAll(A,B,C);
169 }
while(( task.
getError() ).sumSquare()>0.005);
170 std::cout <<
"final error=" << ( task.
getError() ).sumSquare() << std::endl;
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void setPosition(const vpHomogeneousMatrix &cMw)
static double getAlpha(vpMomentObject &objec)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
error that can be emited by ViSP classes.
Class for generic objects.
void track(const vpHomogeneousMatrix &cMo)
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.
static double measureTimeMs()
static int wait(double t0, double t)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
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...
vpColVector getError() const
static unsigned int selectC4()
vpColVector computeControlLaw()
compute the desired control law
void changeFrame(const vpHomogeneousMatrix &cMo)
Class that defines the simplest robot: a free flying camera.
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)
Set the type of the interaction matrix (current, mean, desired, user).
static double rad(double deg)
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
Class that provides a data structure for the column vectors as well as a set of operations on these v...
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class defines the container for a plane geometrical structure.
static unsigned int selectC6()
Class required to compute the visual servoing control law descbribed in and .
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...