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