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