Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoAfma6AprilTagPBVS.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Data acquisition with RealSense RGB-D sensor and Franka robot.
33 *
34*****************************************************************************/
35
51#include <iostream>
52
53#include <visp3/core/vpCameraParameters.h>
54#include <visp3/detection/vpDetectorAprilTag.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/gui/vpPlot.h>
58#include <visp3/io/vpImageIo.h>
59#include <visp3/robot/vpRobotAfma6.h>
60#include <visp3/sensor/vpRealSense2.h>
61#include <visp3/visual_features/vpFeatureThetaU.h>
62#include <visp3/visual_features/vpFeatureTranslation.h>
63#include <visp3/vs/vpServo.h>
64#include <visp3/vs/vpServoDisplay.h>
65
66#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
67 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_AFMA6)
68
69void display_point_trajectory(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
70 std::vector<vpImagePoint> *traj_vip)
71{
72 for (size_t i = 0; i < vip.size(); i++) {
73 if (traj_vip[i].size()) {
74 // Add the point only if distance with the previous > 1 pixel
75 if (vpImagePoint::distance(vip[i], traj_vip[i].back()) > 1.) {
76 traj_vip[i].push_back(vip[i]);
77 }
78 } else {
79 traj_vip[i].push_back(vip[i]);
80 }
81 }
82 for (size_t i = 0; i < vip.size(); i++) {
83 for (size_t j = 1; j < traj_vip[i].size(); j++) {
84 vpDisplay::displayLine(I, traj_vip[i][j - 1], traj_vip[i][j], vpColor::green, 2);
85 }
86 }
87}
88
89int main(int argc, char **argv)
90{
91 double opt_tagSize = 0.120;
92 bool display_tag = true;
93 int opt_quad_decimate = 2;
94 bool opt_verbose = false;
95 bool opt_plot = false;
96 bool opt_adaptive_gain = false;
97 bool opt_task_sequencing = false;
98 double convergence_threshold_t = 0.0005; // Value in [m]
99 double convergence_threshold_tu = 0.5; // Value in [deg]
100
101 for (int i = 1; i < argc; i++) {
102 if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
103 opt_tagSize = std::stod(argv[i + 1]);
104 } else if (std::string(argv[i]) == "--verbose") {
105 opt_verbose = true;
106 } else if (std::string(argv[i]) == "--plot") {
107 opt_plot = true;
108 } else if (std::string(argv[i]) == "--adaptive_gain") {
109 opt_adaptive_gain = true;
110 } else if (std::string(argv[i]) == "--task_sequencing") {
111 opt_task_sequencing = true;
112 } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
113 opt_quad_decimate = std::stoi(argv[i + 1]);
114 } else if (std::string(argv[i]) == "--no-convergence-threshold") {
115 convergence_threshold_t = 0.;
116 convergence_threshold_tu = 0.;
117 } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
118 std::cout
119 << argv[0] << " [--tag_size <marker size in meter; default " << opt_tagSize << ">] "
120 << "[--quad_decimate <decimation; default " << opt_quad_decimate
121 << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
122 << "\n";
123 return EXIT_SUCCESS;
124 }
125 }
126
127 vpRobotAfma6 robot;
128
129 try {
130 std::cout << "WARNING: This example will move the robot! "
131 << "Please make sure to have the user stop button at hand!" << std::endl
132 << "Press Enter to continue..." << std::endl;
133 std::cin.ignore();
134
135 /*
136 * Move to a safe position
137 */
138 vpColVector q(6, 0);
139 q[0] = 0.;
140 q[1] = 0.;
141 q[2] = 0.;
142 q[3] = vpMath::rad(0.);
143 q[4] = vpMath::rad(0.);
144 q[5] = vpMath::rad(0.);
145 std::cout << "Move to joint position: " << q.t() << std::endl;
147 robot.setPosition(vpRobot::JOINT_STATE, q);
148
149 vpRealSense2 rs;
150 rs2::config config;
151 unsigned int width = 640, height = 480;
152 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
153 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
154 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
155 rs.open(config);
156
157 // Get camera intrinsics
160 std::cout << "cam:\n" << cam << "\n";
161
162 vpImage<unsigned char> I(height, width);
163
164#if defined(VISP_HAVE_X11)
165 vpDisplayX dc(I, 10, 10, "Color image");
166#elif defined(VISP_HAVE_GDI)
167 vpDisplayGDI dc(I, 10, 10, "Color image");
168#endif
169
172 // vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS;
173 vpDetectorAprilTag detector(tagFamily);
174 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
175 detector.setDisplayTag(display_tag);
176 detector.setAprilTagQuadDecimate(opt_quad_decimate);
177
178 // Servo
179 vpHomogeneousMatrix cdMc, cMo, oMo;
180
181 // Desired pose to reach
182 vpHomogeneousMatrix cdMo(vpTranslationVector(0, 0, opt_tagSize * 3), // 3 times tag with along camera z axis
183 vpRotationMatrix({1, 0, 0, 0, -1, 0, 0, 0, -1}));
184
185 cdMc = cdMo * cMo.inverse();
188 t.buildFrom(cdMc);
189 tu.buildFrom(cdMc);
190
193
194 vpServo task;
195 task.addFeature(t, td);
196 task.addFeature(tu, tud);
199
200 if (opt_adaptive_gain) {
201 vpAdaptiveGain lambda(1.5, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
202 task.setLambda(lambda);
203 } else {
204 task.setLambda(0.5);
205 }
206
207 vpPlot *plotter = nullptr;
208 int iter_plot = 0;
209
210 if (opt_plot) {
211 plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10,
212 "Real time curves plotter");
213 plotter->setTitle(0, "Visual features error");
214 plotter->setTitle(1, "Camera velocities");
215 plotter->initGraph(0, 6);
216 plotter->initGraph(1, 6);
217 plotter->setLegend(0, 0, "error_feat_tx");
218 plotter->setLegend(0, 1, "error_feat_ty");
219 plotter->setLegend(0, 2, "error_feat_tz");
220 plotter->setLegend(0, 3, "error_feat_theta_ux");
221 plotter->setLegend(0, 4, "error_feat_theta_uy");
222 plotter->setLegend(0, 5, "error_feat_theta_uz");
223 plotter->setLegend(1, 0, "vc_x");
224 plotter->setLegend(1, 1, "vc_y");
225 plotter->setLegend(1, 2, "vc_z");
226 plotter->setLegend(1, 3, "wc_x");
227 plotter->setLegend(1, 4, "wc_y");
228 plotter->setLegend(1, 5, "wc_z");
229 }
230
231 bool final_quit = false;
232 bool has_converged = false;
233 bool send_velocities = false;
234 bool servo_started = false;
235 std::vector<vpImagePoint> *traj_vip = nullptr; // To memorize point trajectory
236
237 static double t_init_servo = vpTime::measureTimeMs();
238
240
241 while (!has_converged && !final_quit) {
242 double t_start = vpTime::measureTimeMs();
243
244 rs.acquire(I);
245
247
248 std::vector<vpHomogeneousMatrix> cMo_vec;
249 detector.detect(I, opt_tagSize, cam, cMo_vec);
250
251 std::stringstream ss;
252 ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
253 vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
254
255 vpColVector v_c(6);
256
257 // Only one tag is detected
258 if (cMo_vec.size() == 1) {
259 cMo = cMo_vec[0];
260
261 static bool first_time = true;
262 if (first_time) {
263 // Introduce security wrt tag positioning in order to avoid PI rotation
264 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
265 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
266 for (size_t i = 0; i < 2; i++) {
267 v_cdMc[i] = cdMo * v_oMo[i] * cMo.inverse();
268 }
269 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
270 oMo = v_oMo[0];
271 } else {
272 std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
273 oMo = v_oMo[1]; // Introduce PI rotation
274 }
275 }
276
277 // Update visual features
278 cdMc = cdMo * oMo * cMo.inverse();
279 t.buildFrom(cdMc);
280 tu.buildFrom(cdMc);
281
282 if (opt_task_sequencing) {
283 if (!servo_started) {
284 if (send_velocities) {
285 servo_started = true;
286 }
287 t_init_servo = vpTime::measureTimeMs();
288 }
289 v_c = task.computeControlLaw((vpTime::measureTimeMs() - t_init_servo) / 1000.);
290 } else {
291 v_c = task.computeControlLaw();
292 }
293
294 // Display desired and current pose features
295 vpDisplay::displayFrame(I, cdMo * oMo, cam, opt_tagSize / 1.5, vpColor::yellow, 2);
296 vpDisplay::displayFrame(I, cMo, cam, opt_tagSize / 2, vpColor::none, 3);
297 // Get tag corners
298 std::vector<vpImagePoint> vip = detector.getPolygon(0);
299 // Get the tag cog corresponding to the projection of the tag frame in the image
300 vip.push_back(detector.getCog(0));
301 // Display the trajectory of the points
302 if (first_time) {
303 traj_vip = new std::vector<vpImagePoint>[vip.size()];
304 }
305 display_point_trajectory(I, vip, traj_vip);
306
307 if (opt_plot) {
308 plotter->plot(0, iter_plot, task.getError());
309 plotter->plot(1, iter_plot, v_c);
310 iter_plot++;
311 }
312
313 if (opt_verbose) {
314 std::cout << "v_c: " << v_c.t() << std::endl;
315 }
316
317 vpTranslationVector cd_t_c = cdMc.getTranslationVector();
318 vpThetaUVector cd_tu_c = cdMc.getThetaUVector();
319 double error_tr = sqrt(cd_t_c.sumSquare());
320 double error_tu = vpMath::deg(sqrt(cd_tu_c.sumSquare()));
321
322 ss.str("");
323 ss << "error_t: " << error_tr;
324 vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
325 ss.str("");
326 ss << "error_tu: " << error_tu;
327 vpDisplay::displayText(I, 40, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
328
329 if (opt_verbose)
330 std::cout << "error translation: " << error_tr << " ; error rotation: " << error_tu << std::endl;
331
332 if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
333 has_converged = true;
334 std::cout << "Servo task has converged" << std::endl;
335 ;
336 vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
337 }
338
339 if (first_time) {
340 first_time = false;
341 }
342 } // end if (cMo_vec.size() == 1)
343 else {
344 v_c = 0;
345 }
346
347 if (!send_velocities) {
348 v_c = 0;
349 }
350
351 // Send to the robot
353
354 ss.str("");
355 ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
356 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
358
360 if (vpDisplay::getClick(I, button, false)) {
361 switch (button) {
363 send_velocities = !send_velocities;
364 break;
365
367 final_quit = true;
368 v_c = 0;
369 break;
370
371 default:
372 break;
373 }
374 }
375 }
376 std::cout << "Stop the robot " << std::endl;
378
379 if (opt_plot && plotter != nullptr) {
380 delete plotter;
381 plotter = nullptr;
382 }
383
384 if (!final_quit) {
385 while (!final_quit) {
386 rs.acquire(I);
388
389 vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
390 vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
391
392 if (vpDisplay::getClick(I, false)) {
393 final_quit = true;
394 }
395
397 }
398 }
399
400 if (traj_vip) {
401 delete[] traj_vip;
402 }
403 } catch (const vpException &e) {
404 std::cout << "ViSP exception: " << e.what() << std::endl;
405 std::cout << "Stop the robot " << std::endl;
407 return EXIT_FAILURE;
408 } catch (const std::exception &e) {
409 std::cout << "ur_rtde exception: " << e.what() << std::endl;
410 return EXIT_FAILURE;
411 }
412
413 return EXIT_SUCCESS;
414}
415#else
416int main()
417{
418#if !defined(VISP_HAVE_REALSENSE2)
419 std::cout << "Install librealsense-2.x" << std::endl;
420#endif
421#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
422 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
423#endif
424#if !defined(VISP_HAVE_AFMA6)
425 std::cout << "ViSP is not build with Afma-6 robot support..." << std::endl;
426#endif
427 return EXIT_SUCCESS;
428}
429#endif
Adaptive gain computation.
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor yellow
Definition vpColor.h:219
static const vpColor green
Definition vpColor.h:214
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
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 displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
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
const char * what() const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotati...
Class that defines the translation visual feature .
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition vpPlot.h:113
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition vpPlot.cpp:202
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition vpPlot.cpp:545
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition vpPlot.cpp:269
void setTitle(unsigned int graphNum, const std::string &title)
Definition vpPlot.cpp:503
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ JOINT_STATE
Definition vpRobot.h:78
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
Implementation of a rotation matrix and operations on such kind of matrices.
double sumSquare() const
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 getError() const
Definition vpServo.h:276
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
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()