Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
1
2#include <visp3/gui/vpDisplayGDI.h>
3#include <visp3/gui/vpDisplayOpenCV.h>
4#include <visp3/gui/vpDisplayX.h>
5#include <visp3/robot/vpSimulatorAfma6.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 :
42 w_y
43 /|\
44 |
45 w_x <--
46
47 object :
48 o_y
49 /|\
50 |
51 o_x <--
52
53
54 camera :
55 c_y
56 /|\
57 |
58 c_x <--
59
60 */
61 vpHomogeneousMatrix wMo(0, 0, 1., 0, 0, 0);
62
63 std::vector<vpPoint> point;
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 point.push_back(vpPoint(-0.1, 0.1, 0));
68
69 vpServo task;
72 task.setLambda(0.5);
73
74 vpFeaturePoint p[4], pd[4];
75 for (unsigned int i = 0; i < 4; i++) {
76 point[i].track(cdMo);
77 vpFeatureBuilder::create(pd[i], point[i]);
78 point[i].track(cMo);
79 vpFeatureBuilder::create(p[i], point[i]);
80 task.addFeature(p[i], pd[i]);
81 }
82
83 vpSimulatorAfma6 robot(true);
84 robot.setVerbose(true);
85
86 // Get the default joint limits
87 vpColVector qmin = robot.getJointMin();
88 vpColVector qmax = robot.getJointMax();
89
90 std::cout << "Robot joint limits: " << std::endl;
91 for (unsigned int i = 0; i < 3; i++)
92 std::cout << "Joint " << i << ": min " << qmin[i] << " max " << qmax[i] << " (m)" << std::endl;
93 for (unsigned int i = 3; i < qmin.size(); i++)
94 std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)"
95 << std::endl;
96
100 robot.set_fMo(wMo);
101 bool ret = robot.initialiseCameraRelativeToObject(cMo);
102 if (ret == false)
103 return EXIT_FAILURE; // Not able to set the position
104 robot.setDesiredCameraPosition(cdMo);
105
106 vpImage<unsigned char> Iint(480, 640, 255);
107#if defined(VISP_HAVE_X11)
108 vpDisplayX displayInt(Iint, 700, 0, "Internal view");
109#elif defined(VISP_HAVE_GDI)
110 vpDisplayGDI displayInt(Iint, 700, 0, "Internal view");
111#elif defined(HAVE_OPENCV_HIGHGUI)
112 vpDisplayOpenCV displayInt(Iint, 700, 0, "Internal view");
113#else
114 std::cout << "No image viewer is available..." << std::endl;
115#endif
116
117 vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
118 robot.setCameraParameters(cam);
119
120 bool start = true;
121 for (;;) {
122 cMo = robot.get_cMo();
123
124 for (unsigned int i = 0; i < 4; i++) {
125 point[i].track(cMo);
126 vpFeatureBuilder::create(p[i], point[i]);
127 }
128
129 vpDisplay::display(Iint);
130 robot.getInternalView(Iint);
131 if (!start) {
132 display_trajectory(Iint, point, cMo, cam);
133 vpDisplay::displayText(Iint, 40, 120, "Click to stop the servo...", vpColor::red);
134 }
135 vpDisplay::flush(Iint);
136
139
140 // A click to exit
141 if (vpDisplay::getClick(Iint, false))
142 break;
143
144 if (start) {
145 start = false;
146 v = 0;
148 vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
149 vpDisplay::flush(Iint);
151 }
152
153 vpTime::wait(1000 * robot.getSamplingTime());
154 }
155 } catch (const vpException &e) {
156 std::cout << "Catch an exception: " << e << std::endl;
157 }
158#endif
159}
@ TOOL_CCMOP
Definition vpAfma6.h:124
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
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 gantry robot named Afma6.
VISP_EXPORT int wait(double t0, double t)