Visual Servoing Platform version 3.5.0
servoFrankaIBVS.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * tests the control law
33 * eye-in-hand control
34 * velocity computed in the camera frame
35 *
36 * Authors:
37 * Fabien Spindler
38 *
39 *****************************************************************************/
65#include <iostream>
66
67#include <visp3/core/vpCameraParameters.h>
68#include <visp3/gui/vpDisplayGDI.h>
69#include <visp3/gui/vpDisplayX.h>
70#include <visp3/io/vpImageIo.h>
71#include <visp3/sensor/vpRealSense2.h>
72#include <visp3/robot/vpRobotFranka.h>
73#include <visp3/detection/vpDetectorAprilTag.h>
74#include <visp3/visual_features/vpFeatureBuilder.h>
75#include <visp3/visual_features/vpFeaturePoint.h>
76#include <visp3/vs/vpServo.h>
77#include <visp3/vs/vpServoDisplay.h>
78#include <visp3/gui/vpPlot.h>
79
80#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
81(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
82
83void display_point_trajectory(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
84 std::vector<vpImagePoint> *traj_vip)
85{
86 for (size_t i = 0; i < vip.size(); i++) {
87 if (traj_vip[i].size()) {
88 // Add the point only if distance with the previous > 1 pixel
89 if (vpImagePoint::distance(vip[i], traj_vip[i].back()) > 1.) {
90 traj_vip[i].push_back(vip[i]);
91 }
92 }
93 else {
94 traj_vip[i].push_back(vip[i]);
95 }
96 }
97 for (size_t i = 0; i < vip.size(); i++) {
98 for (size_t j = 1; j < traj_vip[i].size(); j++) {
99 vpDisplay::displayLine(I, traj_vip[i][j - 1], traj_vip[i][j], vpColor::green, 2);
100 }
101 }
102}
103
104int main(int argc, char **argv)
105{
106 double opt_tagSize = 0.120;
107 std::string opt_robot_ip = "192.168.1.1";
108 std::string opt_eMc_filename = "";
109 bool display_tag = true;
110 int opt_quad_decimate = 2;
111 bool opt_verbose = false;
112 bool opt_plot = false;
113 bool opt_adaptive_gain = false;
114 bool opt_task_sequencing = false;
115 double convergence_threshold = 0.00005;
116
117 for (int i = 1; i < argc; i++) {
118 if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
119 opt_tagSize = std::stod(argv[i + 1]);
120 }
121 else if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
122 opt_robot_ip = std::string(argv[i + 1]);
123 }
124 else if (std::string(argv[i]) == "--eMc" && i + 1 < argc) {
125 opt_eMc_filename = std::string(argv[i + 1]);
126 }
127 else if (std::string(argv[i]) == "--verbose") {
128 opt_verbose = true;
129 }
130 else if (std::string(argv[i]) == "--plot") {
131 opt_plot = true;
132 }
133 else if (std::string(argv[i]) == "--adaptive_gain") {
134 opt_adaptive_gain = true;
135 }
136 else if (std::string(argv[i]) == "--task_sequencing") {
137 opt_task_sequencing = true;
138 }
139 else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
140 opt_quad_decimate = std::stoi(argv[i + 1]);
141 }
142 else if (std::string(argv[i]) == "--no-convergence-threshold") {
143 convergence_threshold = 0.;
144 }
145 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
146 std::cout << argv[0] << " [--ip <default " << opt_robot_ip << ">] [--tag_size <marker size in meter; default " << opt_tagSize << ">] [--eMc <eMc extrinsic file>] "
147 << "[--quad_decimate <decimation; default " << opt_quad_decimate << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
148 << "\n";
149 return EXIT_SUCCESS;
150 }
151 }
152
153 vpRobotFranka robot;
154
155 try {
156 robot.connect(opt_robot_ip);
157
158 vpRealSense2 rs;
159 rs2::config config;
160 unsigned int width = 640, height = 480;
161 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
162 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
163 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
164 rs.open(config);
165
166 // Get camera extrinsics
167 vpPoseVector ePc;
168 // Set camera extrinsics default values
169 ePc[0] = 0.0337731; ePc[1] = -0.00535012; ePc[2] = -0.0523339;
170 ePc[3] = -0.247294; ePc[4] = -0.306729; ePc[5] = 1.53055;
171
172 // If provided, read camera extrinsics from --eMc <file>
173 if (!opt_eMc_filename.empty()) {
174 ePc.loadYAML(opt_eMc_filename, ePc);
175 }
176 else {
177 std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << "\n";
178 }
179 vpHomogeneousMatrix eMc(ePc);
180 std::cout << "eMc:\n" << eMc << "\n";
181
182 // Get camera intrinsics
184 std::cout << "cam:\n" << cam << "\n";
185
186 vpImage<unsigned char> I(height, width);
187
188#if defined(VISP_HAVE_X11)
189 vpDisplayX dc(I, 10, 10, "Color image");
190#elif defined(VISP_HAVE_GDI)
191 vpDisplayGDI dc(I, 10, 10, "Color image");
192#endif
193
196 //vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS;
197 vpDetectorAprilTag detector(tagFamily);
198 detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
199 detector.setDisplayTag(display_tag);
200 detector.setAprilTagQuadDecimate(opt_quad_decimate);
201
202 // Servo
203 vpHomogeneousMatrix cdMc, cMo, oMo;
204
205 // Desired pose used to compute the desired features
206 vpHomogeneousMatrix cdMo( vpTranslationVector(0, 0, opt_tagSize * 3), // 3 times tag with along camera z axis
207 vpRotationMatrix( {1, 0, 0, 0, -1, 0, 0, 0, -1} ) );
208
209 // Create visual features
210 std::vector<vpFeaturePoint> p(4), pd(4); // We use 4 points
211
212 // Define 4 3D points corresponding to the CAD model of the Apriltag
213 std::vector<vpPoint> point(4);
214 point[0].setWorldCoordinates(-opt_tagSize/2., -opt_tagSize/2., 0);
215 point[1].setWorldCoordinates( opt_tagSize/2., -opt_tagSize/2., 0);
216 point[2].setWorldCoordinates( opt_tagSize/2., opt_tagSize/2., 0);
217 point[3].setWorldCoordinates(-opt_tagSize/2., opt_tagSize/2., 0);
218
219 vpServo task;
220 // Add the 4 visual feature points
221 for (size_t i = 0; i < p.size(); i++) {
222 task.addFeature(p[i], pd[i]);
223 }
226
227 if (opt_adaptive_gain) {
228 vpAdaptiveGain lambda(1.5, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
229 task.setLambda(lambda);
230 }
231 else {
232 task.setLambda(0.5);
233 }
234
235 vpPlot *plotter = nullptr;
236 int iter_plot = 0;
237
238 if (opt_plot) {
239 plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10, "Real time curves plotter");
240 plotter->setTitle(0, "Visual features error");
241 plotter->setTitle(1, "Camera velocities");
242 plotter->initGraph(0, 8);
243 plotter->initGraph(1, 6);
244 plotter->setLegend(0, 0, "error_feat_p1_x");
245 plotter->setLegend(0, 1, "error_feat_p1_y");
246 plotter->setLegend(0, 2, "error_feat_p2_x");
247 plotter->setLegend(0, 3, "error_feat_p2_y");
248 plotter->setLegend(0, 4, "error_feat_p3_x");
249 plotter->setLegend(0, 5, "error_feat_p3_y");
250 plotter->setLegend(0, 6, "error_feat_p4_x");
251 plotter->setLegend(0, 7, "error_feat_p4_y");
252 plotter->setLegend(1, 0, "vc_x");
253 plotter->setLegend(1, 1, "vc_y");
254 plotter->setLegend(1, 2, "vc_z");
255 plotter->setLegend(1, 3, "wc_x");
256 plotter->setLegend(1, 4, "wc_y");
257 plotter->setLegend(1, 5, "wc_z");
258 }
259
260 bool final_quit = false;
261 bool has_converged = false;
262 bool send_velocities = false;
263 bool servo_started = false;
264 std::vector<vpImagePoint> *traj_corners = nullptr; // To memorize point trajectory
265
266 static double t_init_servo = vpTime::measureTimeMs();
267
268 robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
270
271 while (!has_converged && !final_quit) {
272 double t_start = vpTime::measureTimeMs();
273
274 rs.acquire(I);
275
277
278 std::vector<vpHomogeneousMatrix> cMo_vec;
279 detector.detect(I, opt_tagSize, cam, cMo_vec);
280
281 {
282 std::stringstream ss;
283 ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
284 vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
285 }
286
287 vpColVector v_c(6);
288
289 // Only one tag is detected
290 if (cMo_vec.size() == 1) {
291 cMo = cMo_vec[0];
292
293 static bool first_time = true;
294 if (first_time) {
295 // Introduce security wrt tag positionning in order to avoid PI rotation
296 std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
297 v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
298 for (size_t i = 0; i < 2; i++) {
299 v_cdMc[i] = cdMo * v_oMo[i] * cMo.inverse();
300 }
301 if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
302 oMo = v_oMo[0];
303 }
304 else {
305 std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
306 oMo = v_oMo[1]; // Introduce PI rotation
307 }
308
309 // Compute the desired position of the features from the desired pose
310 for (size_t i = 0; i < point.size(); i++) {
311 vpColVector cP, p_;
312 point[i].changeFrame(cdMo * oMo, cP);
313 point[i].projection(cP, p_);
314
315 pd[i].set_x(p_[0]);
316 pd[i].set_y(p_[1]);
317 pd[i].set_Z(cP[2]);
318 }
319 }
320
321 // Get tag corners
322 std::vector<vpImagePoint> corners = detector.getPolygon(0);
323
324 // Update visual features
325 for (size_t i = 0; i < corners.size(); i++) {
326 // Update the point feature from the tag corners location
327 vpFeatureBuilder::create(p[i], cam, corners[i]);
328 // Set the feature Z coordinate from the pose
329 vpColVector cP;
330 point[i].changeFrame(cMo, cP);
331
332 p[i].set_Z(cP[2]);
333 }
334
335 if (opt_task_sequencing) {
336 if (! servo_started) {
337 if (send_velocities) {
338 servo_started = true;
339 }
340 t_init_servo = vpTime::measureTimeMs();
341 }
342 v_c = task.computeControlLaw((vpTime::measureTimeMs() - t_init_servo)/1000.);
343 }
344 else {
345 v_c = task.computeControlLaw();
346 }
347
348 // Display the current and desired feature points in the image display
349 vpServoDisplay::display(task, cam, I);
350 for (size_t i = 0; i < corners.size(); i++) {
351 std::stringstream ss;
352 ss << i;
353 // Display current point indexes
354 vpDisplay::displayText(I, corners[i]+vpImagePoint(15, 15), ss.str(), vpColor::red);
355 // Display desired point indexes
356 vpImagePoint ip;
357 vpMeterPixelConversion::convertPoint(cam, pd[i].get_x(), pd[i].get_y(), ip);
358 vpDisplay::displayText(I, ip+vpImagePoint(15, 15), ss.str(), vpColor::red);
359 }
360 if (first_time) {
361 traj_corners = new std::vector<vpImagePoint> [corners.size()];
362 }
363 // Display the trajectory of the points used as features
364 display_point_trajectory(I, corners, traj_corners);
365
366 if (opt_plot) {
367 plotter->plot(0, iter_plot, task.getError());
368 plotter->plot(1, iter_plot, v_c);
369 iter_plot++;
370 }
371
372 if (opt_verbose) {
373 std::cout << "v_c: " << v_c.t() << std::endl;
374 }
375
376 double error = task.getError().sumSquare();
377 std::stringstream ss;
378 ss << "error: " << error;
379 vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
380
381 if (opt_verbose)
382 std::cout << "error: " << error << std::endl;
383
384 if (error < convergence_threshold) {
385 has_converged = true;
386 std::cout << "Servo task has converged" << "\n";
387 vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
388 }
389 if (first_time) {
390 first_time = false;
391 }
392 } // end if (cMo_vec.size() == 1)
393 else {
394 v_c = 0;
395 }
396
397 if (!send_velocities) {
398 v_c = 0;
399 }
400
401 // Send to the robot
403
404 {
405 std::stringstream ss;
406 ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
407 vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
408 }
410
412 if (vpDisplay::getClick(I, button, false)) {
413 switch (button) {
415 send_velocities = !send_velocities;
416 break;
417
419 final_quit = true;
420 v_c = 0;
421 break;
422
423 default:
424 break;
425 }
426 }
427 }
428 std::cout << "Stop the robot " << std::endl;
430
431 if (opt_plot && plotter != nullptr) {
432 delete plotter;
433 plotter = nullptr;
434 }
435
436 if (!final_quit) {
437 while (!final_quit) {
438 rs.acquire(I);
440
441 vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
442 vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
443
444 if (vpDisplay::getClick(I, false)) {
445 final_quit = true;
446 }
447
449 }
450 }
451 if (traj_corners) {
452 delete [] traj_corners;
453 }
454 }
455 catch(const vpException &e) {
456 std::cout << "ViSP exception: " << e.what() << std::endl;
457 std::cout << "Stop the robot " << std::endl;
459 return EXIT_FAILURE;
460 }
461 catch(const franka::NetworkException &e) {
462 std::cout << "Franka network exception: " << e.what() << std::endl;
463 std::cout << "Check if you are connected to the Franka robot"
464 << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
465 return EXIT_FAILURE;
466 }
467 catch(const std::exception &e) {
468 std::cout << "Franka exception: " << e.what() << std::endl;
469 return EXIT_FAILURE;
470 }
471
472 return 0;
473}
474#else
475int main()
476{
477#if !defined(VISP_HAVE_REALSENSE2)
478 std::cout << "Install librealsense-2.x" << std::endl;
479#endif
480#if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
481 std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
482#endif
483#if !defined(VISP_HAVE_FRANKA)
484 std::cout << "Install libfranka." << std::endl;
485#endif
486 return 0;
487}
488#endif
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:652
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
static const vpColor red
Definition: vpColor.h:217
static const vpColor green
Definition: vpColor.h:220
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
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 emited by ViSP classes.
Definition: vpException.h:72
const char * what() const
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
Definition: vpImage.h:246
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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())
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Implementation of a rotation matrix and operations on such kind of matrices.
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void setLambda(double c)
Definition: vpServo.h:404
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpColVector getError() const
Definition: vpServo.h:278
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
@ CURRENT
Definition: vpServo.h:182
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that consider the case of a translation vector.
VISP_EXPORT double measureTimeMs()