21 #include <visp3/core/vpHomogeneousMatrix.h> 22 #include <visp3/core/vpVelocityTwistMatrix.h> 23 #include <visp3/gui/vpPlot.h> 24 #include <visp3/robot/vpSimulatorPioneer.h> 25 #include <visp3/visual_features/vpFeatureBuilder.h> 26 #include <visp3/visual_features/vpFeatureDepth.h> 27 #include <visp3/visual_features/vpFeaturePoint.h> 28 #include <visp3/vs/vpServo.h> 39 cMo[1][3] = cdMo[1][3];
71 double Z = point.get_Z();
72 double Zd = cdMo[2][3];
77 #ifdef VISP_HAVE_DISPLAY 79 vpPlot graph(3, 800, 500, 400, 10,
"Curves...");
82 graph.initGraph(0, 2);
83 graph.initGraph(1, 2);
84 graph.initGraph(2, 1);
85 graph.setTitle(0,
"Velocities");
86 graph.setTitle(1,
"Error s-s*");
87 graph.setTitle(2,
"Depth");
88 graph.setLegend(0, 0,
"vx");
89 graph.setLegend(0, 1,
"wz");
90 graph.setLegend(1, 0,
"x");
91 graph.setLegend(1, 1,
"log(Z/Z*)");
92 graph.setLegend(2, 0,
"Z");
115 #ifdef VISP_HAVE_DISPLAY 116 graph.plot(0, iter, v);
117 graph.plot(1, iter, task.
getError());
118 graph.plot(2, 0, iter, Z);
124 std::cout <<
"Reached a small error. We stop the loop... " << std::endl;
128 #ifdef VISP_HAVE_DISPLAY 129 graph.saveData(0,
"./v2.dat");
130 graph.saveData(1,
"./error2.dat");
132 const char *legend =
"Click to quit...";
143 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of a matrix and operations on matrices.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
double getSamplingTime() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void set_eJe(const vpMatrix &eJe_)
vpVelocityTwistMatrix get_cVe() const
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
Class that defines the Pioneer mobile robot simulator equipped with a static camera.
error that can be emited by ViSP classes.
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void getPosition(vpHomogeneousMatrix &wMc) const
static void flush(const vpImage< unsigned char > &I)
Class that defines what is a point.
Implementation of a rotation matrix and operations on such kind of matrices.
virtual void setSamplingTime(const double &delta_t)
void insert(const vpRotationMatrix &R)
vpColVector getError() const
vpColVector computeControlLaw()
static unsigned int selectX()
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void buildFrom(const double x, const double y, const double Z)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void setServo(const vpServoType &servo_type)