Visual Servoing Platform  version 3.3.1 under development (2020-08-10)
servoFrankaPBVS.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  * 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/gui/vpDisplayGDI.h>
63 #include <visp3/gui/vpDisplayX.h>
64 #include <visp3/io/vpImageIo.h>
65 #include <visp3/sensor/vpRealSense2.h>
66 #include <visp3/robot/vpRobotFranka.h>
67 #include <visp3/detection/vpDetectorAprilTag.h>
68 #include <visp3/visual_features/vpFeatureThetaU.h>
69 #include <visp3/visual_features/vpFeatureTranslation.h>
70 #include <visp3/vs/vpServo.h>
71 #include <visp3/vs/vpServoDisplay.h>
72 #include <visp3/gui/vpPlot.h>
73 
74 #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
75  (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_FRANKA)
76 
77 void display_point_trajectory(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
78  std::vector<vpImagePoint> *traj_vip)
79 {
80  for (size_t i = 0; i < vip.size(); i++) {
81  if (traj_vip[i].size()) {
82  // Add the point only if distance with the previous > 1 pixel
83  if (vpImagePoint::distance(vip[i], traj_vip[i].back()) > 1.) {
84  traj_vip[i].push_back(vip[i]);
85  }
86  }
87  else {
88  traj_vip[i].push_back(vip[i]);
89  }
90  }
91  for (size_t i = 0; i < vip.size(); i++) {
92  for (size_t j = 1; j < traj_vip[i].size(); j++) {
93  vpDisplay::displayLine(I, traj_vip[i][j - 1], traj_vip[i][j], vpColor::green, 2);
94  }
95  }
96 }
97 
98 int main(int argc, char **argv)
99 {
100  double opt_tagSize = 0.120;
101  std::string opt_robot_ip = "192.168.1.1";
102  std::string opt_eMc_filename = "";
103  bool display_tag = true;
104  int opt_quad_decimate = 2;
105  bool opt_verbose = false;
106  bool opt_plot = false;
107  bool opt_adaptive_gain = false;
108  bool opt_task_sequencing = false;
109  double convergence_threshold_t = 0.0005, convergence_threshold_tu = vpMath::rad(0.5);
110 
111  for (int i = 1; i < argc; i++) {
112  if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
113  opt_tagSize = std::stod(argv[i + 1]);
114  }
115  else if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
116  opt_robot_ip = std::string(argv[i + 1]);
117  }
118  else if (std::string(argv[i]) == "--eMc" && i + 1 < argc) {
119  opt_eMc_filename = std::string(argv[i + 1]);
120  }
121  else if (std::string(argv[i]) == "--verbose") {
122  opt_verbose = true;
123  }
124  else if (std::string(argv[i]) == "--plot") {
125  opt_plot = true;
126  }
127  else if (std::string(argv[i]) == "--adaptive_gain") {
128  opt_adaptive_gain = true;
129  }
130  else if (std::string(argv[i]) == "--task_sequencing") {
131  opt_task_sequencing = true;
132  }
133  else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
134  opt_quad_decimate = std::stoi(argv[i + 1]);
135  }
136  else if (std::string(argv[i]) == "--no-convergence-threshold") {
137  convergence_threshold_t = 0.;
138  convergence_threshold_tu = 0.;
139  }
140  else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
141  std::cout << argv[0] << " [--ip <default " << opt_robot_ip << ">] [--tag_size <marker size in meter; default " << opt_tagSize << ">] [--eMc <eMc extrinsic file>] "
142  << "[--quad_decimate <decimation; default " << opt_quad_decimate << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
143  << "\n";
144  return EXIT_SUCCESS;
145  }
146  }
147 
148  vpRobotFranka robot;
149 
150  try {
151  robot.connect(opt_robot_ip);
152 
153  vpRealSense2 rs;
154  rs2::config config;
155  unsigned int width = 640, height = 480;
156  config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
157  config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
158  config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
159  rs.open(config);
160 
161  // Get camera extrinsics
162  vpPoseVector ePc;
163  // Set camera extrinsics default values
164  ePc[0] = 0.0337731; ePc[1] = -0.00535012; ePc[2] = -0.0523339;
165  ePc[3] = -0.247294; ePc[4] = -0.306729; ePc[5] = 1.53055;
166 
167  // If provided, read camera extrinsics from --eMc <file>
168  if (!opt_eMc_filename.empty()) {
169  ePc.loadYAML(opt_eMc_filename, ePc);
170  }
171  else {
172  std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << "\n";
173  }
174  vpHomogeneousMatrix eMc(ePc);
175  std::cout << "eMc:\n" << eMc << "\n";
176 
177  // Get camera intrinsics
179  std::cout << "cam:\n" << cam << "\n";
180 
181  vpImage<unsigned char> I(height, width);
182 
183 #if defined(VISP_HAVE_X11)
184  vpDisplayX dc(I, 10, 10, "Color image");
185 #elif defined(VISP_HAVE_GDI)
186  vpDisplayGDI dc(I, 10, 10, "Color image");
187 #endif
188 
191  //vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS;
192  vpDetectorAprilTag detector(tagFamily);
193  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
194  detector.setDisplayTag(display_tag);
195  detector.setAprilTagQuadDecimate(opt_quad_decimate);
196 
197  // Servo
198  vpHomogeneousMatrix cdMc, cMo, oMo;
199 
200  // Desired pose to reach
201  vpHomogeneousMatrix cdMo( vpTranslationVector(0, 0, opt_tagSize * 3), // 3 times tag with along camera z axis
202  vpRotationMatrix( {1, 0, 0, 0, -1, 0, 0, 0, -1} ) );
203 
204  cdMc = cdMo * cMo.inverse();
207  t.buildFrom(cdMc);
208  tu.buildFrom(cdMc);
209 
212 
213  vpServo task;
214  task.addFeature(t, td);
215  task.addFeature(tu, tud);
218 
219  if (opt_adaptive_gain) {
220  vpAdaptiveGain lambda(1.5, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
221  task.setLambda(lambda);
222  }
223  else {
224  task.setLambda(0.5);
225  }
226 
227  vpPlot *plotter = nullptr;
228  int iter_plot = 0;
229 
230  if (opt_plot) {
231  plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10, "Real time curves plotter");
232  plotter->setTitle(0, "Visual features error");
233  plotter->setTitle(1, "Camera velocities");
234  plotter->initGraph(0, 6);
235  plotter->initGraph(1, 6);
236  plotter->setLegend(0, 0, "error_feat_tx");
237  plotter->setLegend(0, 1, "error_feat_ty");
238  plotter->setLegend(0, 2, "error_feat_tz");
239  plotter->setLegend(0, 3, "error_feat_theta_ux");
240  plotter->setLegend(0, 4, "error_feat_theta_uy");
241  plotter->setLegend(0, 5, "error_feat_theta_uz");
242  plotter->setLegend(1, 0, "vc_x");
243  plotter->setLegend(1, 1, "vc_y");
244  plotter->setLegend(1, 2, "vc_z");
245  plotter->setLegend(1, 3, "wc_x");
246  plotter->setLegend(1, 4, "wc_y");
247  plotter->setLegend(1, 5, "wc_z");
248  }
249 
250  bool final_quit = false;
251  bool has_converged = false;
252  bool send_velocities = false;
253  bool servo_started = false;
254  std::vector<vpImagePoint> *traj_vip = nullptr; // To memorize point trajectory
255 
256  static double t_init_servo = vpTime::measureTimeMs();
257 
258  robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
260 
261  while (!has_converged && !final_quit) {
262  double t_start = vpTime::measureTimeMs();
263 
264  rs.acquire(I);
265 
267 
268  std::vector<vpHomogeneousMatrix> cMo_vec;
269  detector.detect(I, opt_tagSize, cam, cMo_vec);
270 
271  std::stringstream ss;
272  ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
273  vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
274 
275  vpColVector v_c(6);
276 
277  // Only one tag is detected
278  if (cMo_vec.size() == 1) {
279  cMo = cMo_vec[0];
280 
281  static bool first_time = true;
282  if (first_time) {
283  // Introduce security wrt tag positionning in order to avoid PI rotation
284  std::vector<vpHomogeneousMatrix> v_oMo(2), v_cdMc(2);
285  v_oMo[1].buildFrom(0, 0, 0, 0, 0, M_PI);
286  for (size_t i = 0; i < 2; i++) {
287  v_cdMc[i] = cdMo * v_oMo[i] * cMo.inverse();
288  }
289  if (std::fabs(v_cdMc[0].getThetaUVector().getTheta()) < std::fabs(v_cdMc[1].getThetaUVector().getTheta())) {
290  oMo = v_oMo[0];
291  }
292  else {
293  std::cout << "Desired frame modified to avoid PI rotation of the camera" << std::endl;
294  oMo = v_oMo[1]; // Introduce PI rotation
295  }
296  }
297 
298  // Update visual features
299  cdMc = cdMo * oMo * cMo.inverse();
300  t.buildFrom(cdMc);
301  tu.buildFrom(cdMc);
302 
303  if (opt_task_sequencing) {
304  if (! servo_started) {
305  if (send_velocities) {
306  servo_started = true;
307  }
308  t_init_servo = vpTime::measureTimeMs();
309  }
310  v_c = task.computeControlLaw((vpTime::measureTimeMs() - t_init_servo)/1000.);
311  }
312  else {
313  v_c = task.computeControlLaw();
314  }
315 
316  // Display desired and current pose features
317  vpDisplay::displayFrame(I, cdMo * oMo, cam, opt_tagSize / 1.5, vpColor::yellow, 2);
318  vpDisplay::displayFrame(I, cMo, cam, opt_tagSize / 2, vpColor::none, 3);
319  // Get tag corners
320  std::vector<vpImagePoint> vip = detector.getPolygon(0);
321  // Get the tag cog corresponding to the projection of the tag frame in the image
322  vip.push_back(detector.getCog(0));
323  // Display the trajectory of the points
324  if (first_time) {
325  traj_vip = new std::vector<vpImagePoint> [vip.size()];
326  }
327  display_point_trajectory(I, vip, traj_vip);
328 
329  if (opt_plot) {
330  plotter->plot(0, iter_plot, task.getError());
331  plotter->plot(1, iter_plot, v_c);
332  iter_plot++;
333  }
334 
335  if (opt_verbose) {
336  std::cout << "v_c: " << v_c.t() << std::endl;
337  }
338 
340  vpThetaUVector cd_tu_c = cdMc.getThetaUVector();
341  double error_tr = sqrt(cd_t_c.sumSquare());
342  double error_tu = vpMath::deg(sqrt(cd_tu_c.sumSquare()));
343 
344  ss.str("");
345  ss << "error_t: " << error_tr;
346  vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
347  ss.str("");
348  ss << "error_tu: " << error_tu;
349  vpDisplay::displayText(I, 40, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
350 
351  if (opt_verbose)
352  std::cout << "error translation: " << error_tr << " ; error rotation: " << error_tu << std::endl;
353 
354  if (error_tr < convergence_threshold_t && error_tu < convergence_threshold_tu) {
355  has_converged = true;
356  std::cout << "Servo task has converged" << std::endl;;
357  vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
358  }
359 
360  if (first_time) {
361  first_time = false;
362  }
363  } // end if (cMo_vec.size() == 1)
364  else {
365  v_c = 0;
366  }
367 
368  if (!send_velocities) {
369  v_c = 0;
370  }
371 
372  // Send to the robot
374 
375  ss.str("");
376  ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
377  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
378  vpDisplay::flush(I);
379 
381  if (vpDisplay::getClick(I, button, false)) {
382  switch (button) {
384  send_velocities = !send_velocities;
385  break;
386 
388  final_quit = true;
389  v_c = 0;
390  break;
391 
392  default:
393  break;
394  }
395  }
396  }
397  std::cout << "Stop the robot " << std::endl;
399 
400  if (opt_plot && plotter != nullptr) {
401  delete plotter;
402  plotter = nullptr;
403  }
404 
405  if (!final_quit) {
406  while (!final_quit) {
407  rs.acquire(I);
409 
410  vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
411  vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
412 
413  if (vpDisplay::getClick(I, false)) {
414  final_quit = true;
415  }
416 
417  vpDisplay::flush(I);
418  }
419  }
420 
421  if (traj_vip) {
422  delete [] traj_vip;
423  }
424  }
425  catch(const vpException &e) {
426  std::cout << "ViSP exception: " << e.what() << std::endl;
427  std::cout << "Stop the robot " << std::endl;
429  return EXIT_FAILURE;
430  }
431  catch(const franka::NetworkException &e) {
432  std::cout << "Franka network exception: " << e.what() << std::endl;
433  std::cout << "Check if you are connected to the Franka robot"
434  << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
435  return EXIT_FAILURE;
436  }
437  catch(const std::exception &e) {
438  std::cout << "Franka exception: " << e.what() << std::endl;
439  return EXIT_FAILURE;
440  }
441 
442  return 0;
443 }
444 #else
445 int main()
446 {
447 #if !defined(VISP_HAVE_REALSENSE2)
448  std::cout << "Install librealsense-2.x" << std::endl;
449 #endif
450 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
451  std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
452 #endif
453 #if !defined(VISP_HAVE_FRANKA)
454  std::cout << "Install libfranka." << std::endl;
455 #endif
456  return 0;
457 }
458 #endif
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Adaptive gain computation.
Class that defines the translation visual feature .
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
Definition: vpArray2D.h:653
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:486
double sumSquare() const
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
static const vpColor none
Definition: vpColor.h:229
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
void open(const rs2::config &cfg=rs2::config())
static const vpColor green
Definition: vpColor.h:220
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
vpThetaUVector getThetaUVector() const
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:217
Implementation of a rotation matrix and operations on such kind of matrices.
void set_eMc(const vpHomogeneousMatrix &eMc)
void setAprilTagQuadDecimate(float quadDecimate)
Initialize the velocity controller.
Definition: vpRobot.h:66
vpColVector computeControlLaw()
Definition: vpServo.cpp:925
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:402
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void acquire(vpImage< unsigned char > &grey)
vpImagePoint getCog(size_t i) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:563
const char * what() const
static double rad(double deg)
Definition: vpMath.h:110
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
vpTranslationVector getTranslationVector() const
static double deg(double rad)
Definition: vpMath.h:103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
void setDisplayTag(bool display, const vpColor &color=vpColor::none, unsigned int thickness=2)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
Class that defines a 3D visual feature from a axis/angle parametrization that represent the rotatio...
void connect(const std::string &franka_address, franka::RealtimeConfig realtime_config=franka::RealtimeConfig::kEnforce)
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:115
vpColVector getError() const
Definition: vpServo.h:278
static const vpColor yellow
Definition: vpColor.h:225
unsigned int getWidth() const
Definition: vpImage.h:246
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:214
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
std::vector< std::vector< vpImagePoint > > & getPolygon()
bool detect(const vpImage< unsigned char > &I)