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];
45 robot.setSamplingTime(0.04);
47 robot.getPosition(wMc);
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");
97 robot.getPosition(wMc);
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...";
141 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 static camera.
vpVelocityTwistMatrix get_cVe() const