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;
void setSamplingTime(const double &delta_t)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
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
Class for generic objects.
void track(const vpHomogeneousMatrix &cMo)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
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
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 getPosition(vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType, const vpColVector &)
Set a displacement (frame has to be specified) in position control.
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)
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.
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...