22 #include <visp3/core/vpHomogeneousMatrix.h> 23 #include <visp3/core/vpVelocityTwistMatrix.h> 24 #include <visp3/gui/vpPlot.h> 25 #include <visp3/robot/vpSimulatorPioneerPan.h> 26 #include <visp3/visual_features/vpFeatureBuilder.h> 27 #include <visp3/visual_features/vpFeatureDepth.h> 28 #include <visp3/visual_features/vpFeaturePoint.h> 29 #include <visp3/vs/vpServo.h> 42 cMo[1][3] = cdMo[1][3];
90 double Z = point.get_Z();
92 double Zd = cdMo[2][3];
100 #ifdef VISP_HAVE_DISPLAY 102 vpPlot graph(3, 800, 500, 400, 10,
"Curves...");
105 graph.initGraph(0, 3);
106 graph.initGraph(1, 2);
107 graph.initGraph(2, 1);
108 graph.setTitle(0,
"Velocities");
109 graph.setTitle(1,
"Error s-s*");
110 graph.setTitle(2,
"Depth");
111 graph.setLegend(0, 0,
"vx");
112 graph.setLegend(0, 1,
"wz");
113 graph.setLegend(0, 2,
"qdot_pan");
114 graph.setLegend(1, 0,
"x");
115 graph.setLegend(1, 1,
"log(Z/Z*)");
116 graph.setLegend(2, 0,
"Z");
146 #ifdef VISP_HAVE_DISPLAY 147 graph.plot(0, iter, v);
148 graph.plot(1, iter, task.
getError());
149 graph.plot(2, 0, iter, Z);
154 std::cout <<
"Reached a small error. We stop the loop... " << std::endl;
158 #ifdef VISP_HAVE_DISPLAY 159 const char *legend =
"Click to quit...";
169 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)
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 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...
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...
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 get_eJe(vpMatrix &eJe)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void buildFrom(const double x, const double y, const double Z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
vpHomogeneousMatrix inverse() const
Class that defines the Pioneer mobile robot simulator equipped with a camera able to move in pan...
void getPosition(vpHomogeneousMatrix &wMc) 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 setServo(const vpServoType &servo_type)