 |
Visual Servoing Platform
version 3.2.0
|
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;
void buildFrom(const double x, const double y, const double Z, const double LogZoverZstar)
void get_cVe(vpVelocityTwistMatrix &cVe) const
void buildFrom(const double x, const double y, const double Z)
void set_eJe(const vpMatrix &eJe_)
void insert(const vpRotationMatrix &R)
double getSamplingTime() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Implementation of column vector and the associated operations.
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
vpHomogeneousMatrix getPosition() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static unsigned int selectX()
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector getError() const
Class that defines a 3D point visual feature which is composed by one parameters that is that defin...
void get_eJe(vpMatrix &eJe)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
virtual void setSamplingTime(const double &delta_t)
vpHomogeneousMatrix inverse() const
Class that defines what is a point.
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
error that can be emited by ViSP classes.
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Class that defines the Pioneer mobile robot simulator equipped with a static camera.