1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
|
/*!
\example tutorial-simu-pioneer-pan.cpp
Example that shows how to simulate a visual servoing on a Pioneer mobile
robot equipped with a camera able to move along the pan axis. The current
visual features that are used are s = (x, log(Z/Z*)). The desired one are s*
= (x*, 0), with:
- x the abscise of the point measured at each iteration
- x* the desired abscise position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
The degrees of freedom that are controlled are (vx, wz), where wz is the
rotational velocity and vx the translational velocity of the mobile platform
at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpSimulatorPioneerPan.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
int main()
{
#if defined(ENABLE_VISP_NAMESPACE)
using namespace VISP_NAMESPACE_NAME;
#endif
try {
// Set the position the camera has to reach
vpHomogeneousMatrix cdMo;
cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
cdMo[2][3] = 0.5;
// Set the initial camera position
vpHomogeneousMatrix cMo;
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cdRo);
vpSimulatorPioneerPan robot;
robot.setSamplingTime(0.04);
vpHomogeneousMatrix wMc, wMo;
// Get robot position world frame
robot.getPosition(wMc);
// Compute the position of the object in the world frame
wMo = wMc * cMo;
// Define the target
vpPoint point(0, 0, 0); // Coordinates in the object frame
point.track(cMo);
vpServo task;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
task.setLambda(0.2);
vpVelocityTwistMatrix cVe;
cVe = robot.get_cVe();
task.set_cVe(cVe);
vpMatrix eJe;
robot.get_eJe(eJe);
task.set_eJe(eJe);
// Current and desired visual feature associated later to the x coordinate
// of the point
vpFeaturePoint s_x, s_xd;
// Create the current x visual feature
vpFeatureBuilder::create(s_x, point);
// Create the desired x* visual feature
s_xd.buildFrom(0, 0, cdMo[2][3]);
// Add the feature
task.addFeature(s_x, s_xd, vpFeaturePoint::selectX());
// Create the current and desired log(Z/Z*) visual feature
vpFeatureDepth s_Z, s_Zd;
// Initial depth of the target in front of the camera
double Z = point.get_Z();
// Desired depth Z* of the target.
double Zd = cdMo[2][3];
s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
s_Zd.buildFrom(0, 0, Zd,
0); // log(Z/Z*) = 0 that's why the last parameter is 0
// Add the feature
task.addFeature(s_Z, s_Zd);
#ifdef VISP_HAVE_DISPLAY
// Create a window (800 by 500) at position (400, 10) with 3 graphics
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
graph.initGraph(0, 3);
graph.initGraph(1, 2);
graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
graph.setLegend(0, 0, "vx");
graph.setLegend(0, 1, "wz");
graph.setLegend(0, 2, "qdot_pan");
graph.setLegend(1, 0, "x");
graph.setLegend(1, 1, "log(Z/Z*)");
graph.setLegend(2, 0, "Z");
#endif
int iter = 0;
for (;;) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
// Update the current x feature
vpFeatureBuilder::create(s_x, point);
// Update log(Z/Z*) feature. Since the depth Z change, we need to update
// the intection matrix
Z = point.get_Z();
s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
task.set_cVe(cVe);
robot.get_eJe(eJe);
task.set_eJe(eJe);
// Compute the control law. Velocities are computed in the mobile robot
// reference frame
vpColVector v = task.computeControlLaw();
// Send the velocity to the robot
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
graph.plot(0, iter, v); // plot velocities applied to the robot
graph.plot(1, iter, task.getError()); // plot error vector
graph.plot(2, 0, iter, Z); // plot the depth
#endif
iter++;
if (task.getError().sumSquare() < 0.0001) {
std::cout << "Reached a small error. We stop the loop... " << std::endl;
break;
}
}
#ifdef VISP_HAVE_DISPLAY
const char *legend = "Click to quit...";
vpDisplay::displayText(graph.I, static_cast<int>(graph.I.getHeight()) - 60, static_cast<int>(graph.I.getWidth()) - 150, legend, vpColor::red);
vpDisplay::flush(graph.I);
vpDisplay::getClick(graph.I);
#endif
// Kill the servo task
task.print();
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
|
