Visual Servoing Platform  version 3.6.1 under development (2025-02-19)
servoFrankaPBVS.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 
59 #include <iostream>
60 
61 #include <visp3/core/vpCameraParameters.h>
62 #include <visp3/core/vpConfig.h>
63 #include <visp3/detection/vpDetectorAprilTag.h>
64 #include <visp3/gui/vpDisplayFactory.h>
65 #include <visp3/gui/vpPlot.h>
66 #include <visp3/io/vpImageIo.h>
67 #include <visp3/robot/vpRobotFranka.h>
68 #include <visp3/sensor/vpRealSense2.h>
69 #include <visp3/visual_features/vpFeatureThetaU.h>
70 #include <visp3/visual_features/vpFeatureTranslation.h>
71 #include <visp3/vs/vpServo.h>
72 #include <visp3/vs/vpServoDisplay.h>
73 
74 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_DISPLAY) && defined(VISP_HAVE_FRANKA)
75 
76 #ifdef ENABLE_VISP_NAMESPACE
77 using namespace VISP_NAMESPACE_NAME;
78 #endif
79 
80 void display_point_trajectory(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
81  std::vector<vpImagePoint> *traj_vip)
82 {
83  for (size_t i = 0; i < vip.size(); i++) {
84  if (traj_vip[i].size()) {
85  // Add the point only if distance with the previous > 1 pixel
86  if (vpImagePoint::distance(vip[i], traj_vip[i].back()) > 1.) {
87  traj_vip[i].push_back(vip[i]);
88  }
89  }
90  else {
91  traj_vip[i].push_back(vip[i]);
92  }
93  }
94  for (size_t i = 0; i < vip.size(); i++) {
95  for (size_t j = 1; j < traj_vip[i].size(); j++) {
96  vpDisplay::displayLine(I, traj_vip[i][j - 1], traj_vip[i][j], vpColor::green, 2);
97  }
98  }
99 }
100 
101 int main(int argc, char **argv)
102 {
103  double opt_tagSize = 0.120;
104  std::string opt_robot_ip = "192.168.1.1";
105  std::string opt_eMc_filename = "";
106  bool display_tag = true;
107  int opt_quad_decimate = 2;
108  bool opt_verbose = false;
109  bool opt_plot = false;
110  bool opt_adaptive_gain = false;
111  bool opt_task_sequencing = false;
112  double convergence_threshold_t = 0.0005; // Value in [m]
113  double convergence_threshold_tu = 0.5; // Value in [deg]
114 
115  for (int i = 1; i < argc; i++) {
116  if ((std::string(argv[i]) == "--tag-size") && (i + 1 < argc)) {
117  opt_tagSize = std::stod(argv[i + 1]);
118  ++i;
119  }
120  else if ((std::string(argv[i]) == "--ip") && (i + 1 < argc)) {
121  opt_robot_ip = std::string(argv[i + 1]);
122  ++i;
123  }
124  else if ((std::string(argv[i]) == "--eMc") && (i + 1 < argc)) {
125  opt_eMc_filename = std::string(argv[i + 1]);
126  ++i;
127  }
128  else if (std::string(argv[i]) == "--verbose") {
129  opt_verbose = true;
130  }
131  else if (std::string(argv[i]) == "--plot") {
132  opt_plot = true;
133  }
134  else if (std::string(argv[i]) == "--adpative-gain") {
135  opt_adaptive_gain = true;
136  }
137  else if (std::string(argv[i]) == "--task-sequencing") {
138  opt_task_sequencing = true;
139  }
140  else if ((std::string(argv[i]) == "--quad-decimate") && (i + 1 < argc)) {
141  opt_quad_decimate = std::stoi(argv[i + 1]);
142  ++i;
143  }
144  else if (std::string(argv[i]) == "--no-convergence-threshold") {
145  convergence_threshold_t = 0.;
146  convergence_threshold_tu = 0.;
147  }
148  else if ((std::string(argv[i]) == "--help") || (std::string(argv[i]) == "-h")) {
149  std::cout << "SYNOPSYS" << std::endl
150  << " " << argv[0]
151  << " [--ip <controller ip>]"
152  << " [--tag-size <size>]"
153  << " [--eMc <extrinsic transformation file>]"
154  << " [--quad-decimate <decimation factor>]"
155  << " [--adaptive-gain]"
156  << " [--plot]"
157  << " [--task-sequencing]"
158  << " [--no-convergence-threshold]"
159  << " [--verbose]"
160  << " [--help] [-h]\n"
161  << std::endl;
162  std::cout << "DESCRIPTION" << std::endl
163  << " Use a position-based visual-servoing scheme to position the camera in front of an Apriltag." << std::endl
164  << std::endl
165  << " --ip <controller ip>" << std::endl
166  << " Franka controller ip address" << std::endl
167  << " Default: " << opt_robot_ip << std::endl
168  << std::endl
169  << " --tag-size <size>" << std::endl
170  << " Apriltag size in [m]." << std::endl
171  << " Default: " << opt_tagSize << " [m]" << std::endl
172  << std::endl
173  << " --eMc <extrinsic transformation file>" << std::endl
174  << " File containing the homogeneous transformation matrix between" << std::endl
175  << " robot end-effector and camera frame." << std::endl
176  << std::endl
177  << " --quad-decimate <decimation factor>" << std::endl
178  << " Decimation factor used during Apriltag detection." << std::endl
179  << " Default: " << opt_quad_decimate << std::endl
180  << std::endl
181  << " --adaptive-gain" << std::endl
182  << " Flag to enable adaptive gain to speed up visual servo near convergence." << std::endl
183  << std::endl
184  << " --plot" << std::endl
185  << " Flag to enable curve plotter." << std::endl
186  << std::endl
187  << " --task-sequencing" << std::endl
188  << " Flag to enable task sequencing scheme." << std::endl
189  << std::endl
190  << " --no-convergence-threshold" << std::endl
191  << " Flag to disable convergence threshold used to stop the visual servo." << std::endl
192  << std::endl
193  << " --verbose" << std::endl
194  << " Flag to enable extra verbosity." << std::endl
195  << std::endl
196  << " --help, -h" << std::endl
197  << " Print this helper message." << std::endl
198  << std::endl;
199  return EXIT_SUCCESS;
200  }
201  }
202 
203  vpRobotFranka robot;
204 
205  try {
206  robot.connect(opt_robot_ip);
207 
208  vpRealSense2 rs;
209  rs2::config config;
210  unsigned int width = 640, height = 480;
211  config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
212  config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
213  config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
214  rs.open(config);
215 
216  // Get camera extrinsics
217  vpPoseVector ePc;
218  // Set camera extrinsics default values
219  ePc[0] = 0.0337731;
220  ePc[1] = -0.00535012;
221  ePc[2] = -0.0523339;
222  ePc[3] = -0.247294;
223  ePc[4] = -0.306729;
224  ePc[5] = 1.53055;
225 
226  // If provided, read camera extrinsics from --eMc <file>
227  if (!opt_eMc_filename.empty()) {
228  ePc.loadYAML(opt_eMc_filename, ePc);
229  }
230  else {
231  std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << std::endl;
232  }
233  vpHomogeneousMatrix eMc(ePc);
234  std::cout << "eMc:\n" << eMc << std::endl;
235 
236  // Get camera intrinsics
238  std::cout << "cam:\n" << cam << std::endl;
239 
240  vpImage<unsigned char> I(height, width);
241 
242 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
243  display = vpDisplayFactory::createDisplay(I, 10, 10, "Color image");
244 #else
245  display = vpDisplayFactory::allocateDisplay(I, 10, 10, "Color image");
246 #endif
247 
250  // vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS;
251  vpDetectorAprilTag detector(tagFamily);
252  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
253  detector.setDisplayTag(display_tag);
254  detector.setAprilTagQuadDecimate(opt_quad_decimate);
255 
256  // Servo
257  vpHomogeneousMatrix cdMc, cMo, oMo;
258 
259  // Desired pose to reach
260  vpHomogeneousMatrix cdMo(vpTranslationVector(0, 0, opt_tagSize * 3), // 3 times tag with along camera z axis
261  vpRotationMatrix({ 1, 0, 0, 0, -1, 0, 0, 0, -1 }));
262 
263  cdMc = cdMo * cMo.inverse();
266  t.buildFrom(cdMc);
267  tu.buildFrom(cdMc);
268 
271 
272  vpServo task;
273  task.addFeature(t, td);
274  task.addFeature(tu, tud);
277 
278  if (opt_adaptive_gain) {
279  vpAdaptiveGain lambda(1.5, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
280  task.setLambda(lambda);
281  }
282  else {
283  task.setLambda(0.5);
284  }
285 
286  vpPlot *plotter = nullptr;
287  int iter_plot = 0;
288 
289  if (opt_plot) {
290  plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10,
291  "Real time curves plotter");
292  plotter->setTitle(0, "Visual features error");
293  plotter->setTitle(1, "Camera velocities");
294  plotter->initGraph(0, 6);
295  plotter->initGraph(1, 6);
296  plotter->setLegend(0, 0, "error_feat_tx");
297  plotter->setLegend(0, 1, "error_feat_ty");
298  plotter->setLegend(0, 2, "error_feat_tz");
299  plotter->setLegend(0, 3, "error_feat_theta_ux");
300  plotter->setLegend(0, 4, "error_feat_theta_uy");
301  plotter->setLegend(0, 5, "error_feat_theta_uz");
302  plotter->setLegend(1, 0, "vc_x");
303  plotter->setLegend(1, 1, "vc_y");
304  plotter->setLegend(1, 2, "vc_z");
305  plotter->setLegend(1, 3, "wc_x");
306  plotter->setLegend(1, 4, "wc_y");
307  plotter->setLegend(1, 5, "wc_z");
308  }
309 
310  bool final_quit = false;
311  bool has_converged = false;
312  bool send_velocities = false;
313  bool servo_started = false;
314  std::vector<vpImagePoint> *traj_vip = nullptr; // To memorize point trajectory
315 
316  static double t_init_servo = vpTime::measureTimeMs();
317 
318  robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
320 
321  while (!has_converged && !final_quit) {
322  double t_start = vpTime::measureTimeMs();
323 
324  rs.acquire(I);
325 
327 
328  std::vector<vpHomogeneousMatrix> cMo_vec;
329  detector.detect(I, opt_tagSize, cam, cMo_vec);
330 
331  std::stringstream ss;
332  ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
333  vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
334 
335  vpColVector v_c(6);
336 
337  // Only one tag is detected
338  if (cMo_vec.size() == 1) {
339  cMo = cMo_vec[0];
340 
341  static bool first_time = true;
342  if (first_time) {
343  // Introduce security wrt tag positioning in order to avoid PI rotation
344  std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
345  v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
346  for (size_t i = 0; i < 2; i++) {
347  v_cdMc[i] = cdMo * v_oMo[i] * cMo.inverse();
348  }
349  if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
350  oMo = v_oMo[0];
351  }
352  else {
353  std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
354  oMo = v_oMo[1]; // Introduce PI rotation
355  }
356  }
357 
358  // Update visual features
359  cdMc = cdMo * oMo * cMo.inverse();
360  t.buildFrom(cdMc);
361  tu.buildFrom(cdMc);
362 
363  if (opt_task_sequencing) {
364  if (!servo_started) {
365  if (send_velocities) {
366  servo_started = true;
367  }
368  t_init_servo = vpTime::measureTimeMs();
369  }
370  v_c = task.computeControlLaw((vpTime::measureTimeMs() - t_init_servo) / 1000.);
371  }
372  else {
373  v_c = task.computeControlLaw();
374  }
375 
376  // Display desired and current pose features
377  vpDisplay::displayFrame(I, cdMo * oMo, cam, opt_tagSize / 1.5, vpColor::yellow, 2);
378  vpDisplay::displayFrame(I, cMo, cam, opt_tagSize / 2, vpColor::none, 3);
379  // Get tag corners
380  std::vector<vpImagePoint> vip = detector.getPolygon(0);
381  // Get the tag cog corresponding to the projection of the tag frame in the image
382  vip.push_back(detector.getCog(0));
383  // Display the trajectory of the points
384  if (first_time) {
385  traj_vip = new std::vector<vpImagePoint>[vip.size()];
386  }
387  display_point_trajectory(I, vip, traj_vip);
388 
389  if (opt_plot) {
390  plotter->plot(0, iter_plot, task.getError());
391  plotter->plot(1, iter_plot, v_c);
392  iter_plot++;
393  }
394 
395  if (opt_verbose) {
396  std::cout << "v_c: " << v_c.t() << std::endl;
397  }
398 
400  vpThetaUVector cd_tu_c = cdMc.getThetaUVector();
401  double error_tr = sqrt(cd_t_c.sumSquare());
402  double error_tu = vpMath::deg(sqrt(cd_tu_c.sumSquare()));
403 
404  ss.str("");
405  ss << "error_t: " << error_tr;
406  vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
407  ss.str("");
408  ss << "error_tu: " << error_tu;
409  vpDisplay::displayText(I, 40, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
410 
411  if (opt_verbose)
412  std::cout << "error translation: " << error_tr << " ; error rotation: " << error_tu << std::endl;
413 
414  if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
415  has_converged = true;
416  std::cout << "Servo task has converged" << std::endl;
417  ;
418  vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
419  }
420 
421  if (first_time) {
422  first_time = false;
423  }
424  } // end if (cMo_vec.size() == 1)
425  else {
426  v_c = 0;
427  }
428 
429  if (!send_velocities) {
430  v_c = 0;
431  }
432 
433  // Send to the robot
435 
436  ss.str("");
437  ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
438  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
439  vpDisplay::flush(I);
440 
442  if (vpDisplay::getClick(I, button, false)) {
443  switch (button) {
445  send_velocities = !send_velocities;
446  break;
447 
449  final_quit = true;
450  v_c = 0;
451  break;
452 
453  default:
454  break;
455  }
456  }
457  }
458  std::cout << "Stop the robot " << std::endl;
460 
461  if (opt_plot && plotter != nullptr) {
462  delete plotter;
463  plotter = nullptr;
464  }
465 
466  if (!final_quit) {
467  while (!final_quit) {
468  rs.acquire(I);
470 
471  vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
472  vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
473 
474  if (vpDisplay::getClick(I, false)) {
475  final_quit = true;
476  }
477 
478  vpDisplay::flush(I);
479  }
480  }
481 
482  if (traj_vip) {
483  delete[] traj_vip;
484  }
485  }
486  catch (const vpException &e) {
487  std::cout << "ViSP exception: " << e.what() << std::endl;
488  std::cout << "Stop the robot " << std::endl;
490 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
491  if (display != nullptr) {
492  delete display;
493  }
494 #endif
495  return EXIT_FAILURE;
496  }
497  catch (const franka::NetworkException &e) {
498  std::cout << "Franka network exception: " << e.what() << std::endl;
499  std::cout << "Check if you are connected to the Franka robot"
500  << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. "
501  << std::endl;
502 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
503  if (display != nullptr) {
504  delete display;
505  }
506 #endif
507  return EXIT_FAILURE;
508  }
509  catch (const std::exception &e) {
510  std::cout << "Franka exception: " << e.what() << std::endl;
511 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
512  if (display != nullptr) {
513  delete display;
514  }
515 #endif
516  return EXIT_FAILURE;
517  }
518 
519 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
520  if (display != nullptr) {
521  delete display;
522  }
523 #endif
524  return EXIT_SUCCESS;
525 }
526 #else
527 int main()
528 {
529 #if !defined(VISP_HAVE_REALSENSE2)
530  std::cout << "Install librealsense-2.x" << std::endl;
531 #endif
532 #if !defined(VISP_HAVE_FRANKA)
533  std::cout << "Install libfranka." << std::endl;
534 #endif
535  return EXIT_SUCCESS;
536 }
537 #endif
Adaptive gain computation.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=nullptr)
Definition: vpArray2D.h:868
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor red
Definition: vpColor.h:198
static const vpColor none
Definition: vpColor.h:210
static const vpColor yellow
Definition: vpColor.h:206
static const vpColor green
Definition: vpColor.h:201
@ TAG_36h11
AprilTag 36h11 pattern (recommended)
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:60
const char * what() const
Definition: vpException.cpp:71
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
Class that defines the translation visual feature .
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
Definition: vpImage.h:242
static double deg(double rad)
Definition: vpMath.h:119
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:112
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:203
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:552
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:270
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:510
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
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:380
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void setLambda(double c)
Definition: vpServo.h:991
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector getError() const
Definition: vpServo.h:515
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
@ CURRENT
Definition: vpServo.h:202
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()