Visual Servoing Platform  version 3.2.1 under development (2020-01-21)
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_t = 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_t;
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_t << " ; error rotation: " << error_tu << std::endl;
353 
354  if (error_t < 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  task.kill();
406 
407  if (!final_quit) {
408  while (!final_quit) {
409  rs.acquire(I);
411 
412  vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
413  vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
414 
415  if (vpDisplay::getClick(I, false)) {
416  final_quit = true;
417  }
418 
419  vpDisplay::flush(I);
420  }
421  }
422 
423  if (traj_vip) {
424  delete [] traj_vip;
425  }
426  }
427  catch(const vpException &e) {
428  std::cout << "ViSP exception: " << e.what() << std::endl;
429  std::cout << "Stop the robot " << std::endl;
431  return EXIT_FAILURE;
432  }
433  catch(const franka::NetworkException &e) {
434  std::cout << "Franka network exception: " << e.what() << std::endl;
435  std::cout << "Check if you are connected to the Franka robot"
436  << " or if you specified the right IP using --ip command line option set by default to 192.168.1.1. " << std::endl;
437  return EXIT_FAILURE;
438  }
439  catch(const std::exception &e) {
440  std::cout << "Franka exception: " << e.what() << std::endl;
441  return EXIT_FAILURE;
442  }
443 
444  return 0;
445 }
446 #else
447 int main()
448 {
449 #if !defined(VISP_HAVE_REALSENSE2)
450  std::cout << "Install librealsense-2.x" << std::endl;
451 #endif
452 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
453  std::cout << "Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11)." << std::endl;
454 #endif
455 #if !defined(VISP_HAVE_FRANKA)
456  std::cout << "Install libfranka." << std::endl;
457 #endif
458  return 0;
459 }
460 #endif
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void setAprilTagQuadDecimate(const float quadDecimate)
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:654
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Implementation of an homogeneous matrix and operations on such kind of matrices.
AprilTag 36h11 pattern (recommended)
double sumSquare() const
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:151
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
static const vpColor none
Definition: vpColor.h:192
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpHomogeneousMatrix inverse() const
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
Definition: vpPlot.cpp:286
void open(const rs2::config &cfg=rs2::config())
static const vpColor green
Definition: vpColor.h:183
vpThetaUVector getThetaUVector() const
static void flush(const vpImage< unsigned char > &I)
void setTitle(const unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
static const vpColor red
Definition: vpColor.h:180
Implementation of a rotation matrix and operations on such kind of matrices.
void set_eMc(const vpHomogeneousMatrix &eMc)
void kill()
Definition: vpServo.cpp:192
Initialize the velocity controller.
Definition: vpRobot.h:66
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:406
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:574
const char * what() const
static double rad(double deg)
Definition: vpMath.h:108
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
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:101
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))
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
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:282
static const vpColor yellow
Definition: vpColor.h:188
unsigned int getWidth() const
Definition: vpImage.h:244
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
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.
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition: vpImagePoint.h:285
std::vector< std::vector< vpImagePoint > > & getPolygon()
void setDisplayTag(const bool display, const vpColor &color=vpColor::none, const unsigned int thickness=2)
bool detect(const vpImage< unsigned char > &I)