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