Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-ibvs-4pts-wireframe-robot-viper.cpp
1
2#include <visp3/gui/vpDisplayGDI.h>
3#include <visp3/gui/vpDisplayOpenCV.h>
4#include <visp3/gui/vpDisplayX.h>
5#include <visp3/robot/vpSimulatorViper850.h>
6#include <visp3/visual_features/vpFeatureBuilder.h>
7#include <visp3/vs/vpServo.h>
8
9void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
10 const vpCameraParameters &cam);
11
12void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
13 const vpCameraParameters &cam)
14{
15 unsigned int thickness = 3;
16 static std::vector<vpImagePoint> traj[4];
17 vpImagePoint cog;
18 for (unsigned int i = 0; i < 4; i++) {
19 // Project the point at the given camera position
20 point[i].project(cMo);
21 vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
22 traj[i].push_back(cog);
23 }
24 for (unsigned int i = 0; i < 4; i++) {
25 for (unsigned int j = 1; j < traj[i].size(); j++) {
26 vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green, thickness);
27 }
28 }
29}
30
31int main()
32{
33#if defined(VISP_HAVE_PTHREAD)
34 try {
35 vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
36 vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
37
38 /*
39 Top view of the world frame, the camera frame and the object frame
40
41 world, also robot base frame : --> w_y
42 |
43 \|/
44 w_x
45
46 object :
47 o_y
48 /|\
49 |
50 o_x <--
51
52
53 camera :
54 c_y
55 /|\
56 |
57 c_x <--
58
59 */
60 vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15), vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI / 2.)));
61
62 std::vector<vpPoint> point;
63 point.push_back(vpPoint(-0.1, -0.1, 0));
64 point.push_back(vpPoint(0.1, -0.1, 0));
65 point.push_back(vpPoint(0.1, 0.1, 0));
66 point.push_back(vpPoint(-0.1, 0.1, 0));
67
68 vpServo task;
71 task.setLambda(0.5);
72
73 vpFeaturePoint p[4], pd[4];
74 for (unsigned int i = 0; i < 4; i++) {
75 point[i].track(cdMo);
76 vpFeatureBuilder::create(pd[i], point[i]);
77 point[i].track(cMo);
78 vpFeatureBuilder::create(p[i], point[i]);
79 task.addFeature(p[i], pd[i]);
80 }
81
82 vpSimulatorViper850 robot(true);
83 robot.setVerbose(true);
84
85 // Enlarge the default joint limits
86 vpColVector qmin = robot.getJointMin();
87 vpColVector qmax = robot.getJointMax();
88 qmin[0] = -vpMath::rad(180);
89 qmax[0] = vpMath::rad(180);
90 qmax[1] = vpMath::rad(0);
91 qmax[2] = vpMath::rad(270);
92 qmin[4] = -vpMath::rad(180);
93 qmax[4] = vpMath::rad(180);
94
95 robot.setJointLimit(qmin, qmax);
96
97 std::cout << "Robot joint limits: " << std::endl;
98 for (unsigned int i = 0; i < qmin.size(); i++)
99 std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)"
100 << std::endl;
101
105 robot.set_fMo(wMo);
106 bool ret = robot.initialiseCameraRelativeToObject(cMo);
107 if (ret == false)
108 return EXIT_FAILURE; // Not able to set the position
109 robot.setDesiredCameraPosition(cdMo);
110 // We modify the default external camera position
111 robot.setExternalCameraPosition(
113
114 vpImage<unsigned char> Iint(480, 640, 255);
115#if defined(VISP_HAVE_X11)
116 vpDisplayX displayInt(Iint, 700, 0, "Internal view");
117#elif defined(VISP_HAVE_GDI)
118 vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
119#elif defined(HAVE_OPENCV_HIGHGUI)
120 vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view");
121#else
122 std::cout << "No image viewer is available..." << std::endl;
123#endif
124
125 vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
126 // Modify the camera parameters to match those used in the other
127 // simulations
128 robot.setCameraParameters(cam);
129
130 bool start = true;
131 // for ( ; ; )
132 for (int iter = 0; iter < 275; iter++) {
133 cMo = robot.get_cMo();
134
135 for (unsigned int i = 0; i < 4; i++) {
136 point[i].track(cMo);
137 vpFeatureBuilder::create(p[i], point[i]);
138 }
139
140 vpDisplay::display(Iint);
141 robot.getInternalView(Iint);
142 if (!start) {
143 display_trajectory(Iint, point, cMo, cam);
144 vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
145 }
146 vpDisplay::flush(Iint);
147
150
151 // A click to exit
152 if (vpDisplay::getClick(Iint, false))
153 break;
154
155 if (start) {
156 start = false;
157 v = 0;
159 vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
160 vpDisplay::flush(Iint);
161 // vpDisplay::getClick(Iint);
162 }
163
164 vpTime::wait(1000 * robot.getSamplingTime());
165 }
166 } catch (const vpException &e) {
167 std::cout << "Catch an exception: " << e << std::endl;
168 }
169#endif
170}
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor blue
Definition vpColor.h:217
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=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.
Definition vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setVerbose(bool verbose)
Definition vpRobot.h:157
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Simulator of Irisa's Viper S850 robot named Viper850.
Class that consider the case of a translation vector.
@ TOOL_PTGREY_FLEA2_CAMERA
Definition vpViper850.h:127
VISP_EXPORT int wait(double t0, double t)