Visual Servoing Platform  version 3.5.1 under development (2022-07-06)
servoFlirPtuIBVS.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  *****************************************************************************/
61 #include <iostream>
62 
63 #include <visp3/core/vpCameraParameters.h>
64 #include <visp3/core/vpXmlParserCamera.h>
65 #include <visp3/detection/vpDetectorAprilTag.h>
66 #include <visp3/gui/vpDisplayGDI.h>
67 #include <visp3/gui/vpDisplayX.h>
68 #include <visp3/gui/vpPlot.h>
69 #include <visp3/io/vpImageIo.h>
70 #include <visp3/robot/vpRobotFlirPtu.h>
71 #include <visp3/sensor/vpFlyCaptureGrabber.h>
72 #include <visp3/visual_features/vpFeatureBuilder.h>
73 #include <visp3/visual_features/vpFeaturePoint.h>
74 #include <visp3/vs/vpServo.h>
75 #include <visp3/vs/vpServoDisplay.h>
76 
77 #if defined(VISP_HAVE_FLIR_PTU_SDK) && defined(VISP_HAVE_FLYCAPTURE) && \
78  (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
79 
80 void display_point_trajectory(const vpImage<unsigned char> &I, const vpImagePoint &ip,
81  std::vector<vpImagePoint> &traj_ip)
82 {
83  if (traj_ip.size()) {
84  // Add the point only if distance with the previous > 2 pixel
85  if (vpImagePoint::distance(ip, traj_ip.back()) > 2.) {
86  traj_ip.push_back(ip);
87  }
88  } else {
89  traj_ip.push_back(ip);
90  }
91  for (size_t j = 1; j < traj_ip.size(); j++) {
92  vpDisplay::displayLine(I, traj_ip[j - 1], traj_ip[j], vpColor::green, 2);
93  }
94 }
95 
96 int main(int argc, char **argv)
97 {
98  std::string opt_portname;
99  int opt_baudrate = 9600;
100  bool opt_network = false;
101  std::string opt_extrinsic;
102  std::string opt_intrinsic;
103  std::string opt_camera_name;
104  bool display_tag = true;
105  int opt_quad_decimate = 2;
106  double opt_tag_size = 0.120; // Used to compute the distance of the cog wrt the camera
107  bool opt_verbose = false;
108  bool opt_plot = false;
109  bool opt_adaptive_gain = false;
110  bool opt_task_sequencing = false;
111  double opt_constant_gain = 0.5;
112  bool opt_display_trajectory = true;
113  double convergence_threshold = 0.00005;
114 
115  if (argc == 1) {
116  std::cout << "To see how to use this example, run: " << argv[0] << " --help" << std::endl;
117  return EXIT_SUCCESS;
118  }
119 
120  for (int i = 1; i < argc; i++) {
121  if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) {
122  opt_portname = std::string(argv[i + 1]);
123  } else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) {
124  opt_baudrate = std::atoi(argv[i + 1]);
125  } else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) {
126  opt_network = true;
127  } else if (std::string(argv[i]) == "--extrinsic" && i + 1 < argc) {
128  opt_extrinsic = std::string(argv[i + 1]);
129  } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
130  opt_intrinsic = std::string(argv[i + 1]);
131  } else if (std::string(argv[i]) == "--camera-name" && i + 1 < argc) {
132  opt_camera_name = std::string(argv[i + 1]);
133  } else if (std::string(argv[i]) == "--verbose" || std::string(argv[i]) == "-v") {
134  opt_verbose = true;
135  } else if (std::string(argv[i]) == "--plot" || std::string(argv[i]) == "-p") {
136  opt_plot = true;
137  } else if (std::string(argv[i]) == "--display-image-trajectory" || std::string(argv[i]) == "-traj") {
138  opt_display_trajectory = true;
139  } else if (std::string(argv[i]) == "--adaptive-gain" || std::string(argv[i]) == "-a") {
140  opt_adaptive_gain = true;
141  } else if (std::string(argv[i]) == "--constant-gain" || std::string(argv[i]) == "-g") {
142  opt_constant_gain = std::stod(argv[i + 1]);
143  } else if (std::string(argv[i]) == "--task-sequencing") {
144  opt_task_sequencing = true;
145  } else if (std::string(argv[i]) == "--quad-decimate" && i + 1 < argc) {
146  opt_quad_decimate = std::stoi(argv[i + 1]);
147  }
148  if (std::string(argv[i]) == "--tag-size" && i + 1 < argc) {
149  opt_tag_size = std::stod(argv[i + 1]);
150  } else if (std::string(argv[i]) == "--no-convergence-threshold") {
151  convergence_threshold = 0.;
152  } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
153  std::cout << "SYNOPSIS" << std::endl
154  << " " << argv[0] << " [--portname <portname>] [--baudrate <rate>] [--network] "
155  << "[--extrinsic <extrinsic.yaml>] [--intrinsic <intrinsic.xml>] [--camera-name <name>] "
156  << "[--quad-decimate <decimation>] [--tag-size <size>] "
157  << "[--adaptive-gain] [--constant-gain] [--display-image-trajectory] [--plot] [--task-sequencing] "
158  << "[--no-convergence-threshold] [--verbose] [--help] [-h]" << std::endl
159  << std::endl;
160  std::cout << "DESCRIPTION" << std::endl
161  << " --portname, -p <portname>" << std::endl
162  << " Set serial or tcp port name." << std::endl
163  << std::endl
164  << " --baudrate, -b <rate>" << std::endl
165  << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl
166  << std::endl
167  << " --network, -n" << std::endl
168  << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl
169  << std::endl
170  << " --reset, -r" << std::endl
171  << " Reset PTU axis and exit. " << std::endl
172  << std::endl
173  << " --extrinsic <extrinsic.yaml>" << std::endl
174  << " YAML file containing extrinsic camera parameters as a vpHomogeneousMatrix." << std::endl
175  << " It corresponds to the homogeneous transformation eMc, between end-effector" << std::endl
176  << " and camera frame." << std::endl
177  << std::endl
178  << " --intrinsic <intrinsic.xml>" << std::endl
179  << " Intrinsic camera parameters obtained after camera calibration." << std::endl
180  << std::endl
181  << " --camera-name <name>" << std::endl
182  << " Name of the camera to consider in the xml file provided for intrinsic camera parameters."
183  << std::endl
184  << std::endl
185  << " --quad-decimate <decimation>" << std::endl
186  << " Decimation factor used to detect AprilTag. Default " << opt_quad_decimate << "." << std::endl
187  << std::endl
188  << " --tag-size <size>" << std::endl
189  << " Width in meter or the black part of the AprilTag used as target. Default " << opt_tag_size
190  << "." << std::endl
191  << std::endl
192  << " --adaptive-gain, -a" << std::endl
193  << " Enable adaptive gain instead of constant gain to speed up convergence. " << std::endl
194  << std::endl
195  << " --constant-gain, -g" << std::endl
196  << " Constant gain value. Default value: " << opt_constant_gain << std::endl
197  << std::endl
198  << " --display-image-trajectory, -traj" << std::endl
199  << " Display the trajectory of the target cog in the image. " << std::endl
200  << std::endl
201  << " --plot, -p" << std::endl
202  << " Enable curve plotter. " << std::endl
203  << std::endl
204  << " --task-sequencing" << std::endl
205  << " Enable task sequencing that allows to smoothly control the velocity of the robot. " << std::endl
206  << std::endl
207  << " --no-convergence-threshold" << std::endl
208  << " Disable ending servoing when it reaches the desired position." << std::endl
209  << std::endl
210  << " --verbose, -v" << std::endl
211  << " Additional printings. " << std::endl
212  << std::endl
213  << " --help, -h" << std::endl
214  << " Print this helper message. " << std::endl
215  << std::endl;
216  std::cout << "EXAMPLE" << std::endl
217  << " - How to get network IP" << std::endl
218 #ifdef _WIN32
219  << " $ " << argv[0] << " --portname COM1 --network" << std::endl
220  << " Try to connect FLIR PTU to port: COM1 with baudrate: 9600" << std::endl
221 #else
222  << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl
223  << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl
224 #endif
225  << " PTU HostName: PTU-5" << std::endl
226  << " PTU IP : 169.254.110.254" << std::endl
227  << " PTU Gateway : 0.0.0.0" << std::endl
228  << " - How to run this binary using network communication" << std::endl
229  << " $ " << argv[0] << " --portname tcp:169.254.110.254 --tag-size 0.1 --gain 0.1" << std::endl;
230 
231  return EXIT_SUCCESS;
232  }
233  }
234 
235  vpRobotFlirPtu robot;
236 
237  try {
238  std::cout << "Try to connect FLIR PTU to port: " << opt_portname << " with baudrate: " << opt_baudrate << std::endl;
239  robot.connect(opt_portname, opt_baudrate);
240 
241  if (opt_network) {
242  std::cout << "PTU HostName: " << robot.getNetworkHostName() << std::endl;
243  std::cout << "PTU IP : " << robot.getNetworkIP() << std::endl;
244  std::cout << "PTU Gateway : " << robot.getNetworkGateway() << std::endl;
245  return EXIT_SUCCESS;
246  }
247 
249 
251  g.open(I);
252 
253  // Get camera extrinsics
255  vpRotationMatrix eRc;
256  eRc << 0, 0, 1, -1, 0, 0, 0, -1, 0;
257  etc << -0.1, -0.123, 0.035;
258  vpHomogeneousMatrix eMc(etc, eRc);
259 
260  // If provided, read camera extrinsics from command line option
261  if (!opt_extrinsic.empty()) {
262  vpPoseVector ePc;
263  ePc.loadYAML(opt_extrinsic, ePc);
264  eMc.buildFrom(ePc);
265  } else {
266  std::cout << "***************************************************************" << std::endl;
267  std::cout << "Warning, use hard coded values for extrinsic camera parameters." << std::endl;
268  std::cout << "Create a yaml file that contains the extrinsic:" << std::endl
269  << std::endl
270  << "$ cat eMc.yaml" << std::endl
271  << "rows: 4" << std::endl
272  << "cols: 4" << std::endl
273  << "data:" << std::endl
274  << " - [0, 0, 1, -0.1]" << std::endl
275  << " - [-1, 0, 0, -0.123]" << std::endl
276  << " - [0, -1, 0, 0.035]" << std::endl
277  << " - [0, 0, 0, 1]" << std::endl
278  << std::endl
279  << "and load this file with [--extrinsic <extrinsic.yaml] command line option, like:" << std::endl
280  << std::endl
281  << "$ " << argv[0] << "-p " << opt_portname << " --extrinsic eMc.yaml" << std::endl
282  << std::endl;
283  std::cout << "***************************************************************" << std::endl;
284  }
285 
286  std::cout << "Considered extrinsic transformation eMc:\n" << eMc << std::endl;
287 
288  // Get camera intrinsics
289  vpCameraParameters cam(900, 900, I.getWidth() / 2., I.getHeight() / 2.);
290 
291  // If provided, read camera intrinsics from command line option
292  if (!opt_intrinsic.empty() && !opt_camera_name.empty()) {
293  vpXmlParserCamera parser;
294  parser.parse(cam, opt_intrinsic, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
295  } else {
296  std::cout << "***************************************************************" << std::endl;
297  std::cout << "Warning, use hard coded values for intrinsic camera parameters." << std::endl;
298  std::cout << "Calibrate your camera and load the parameters from command line options, like:" << std::endl
299  << std::endl
300  << "$ " << argv[0] << "-p " << opt_portname << " --intrinsic camera.xml --camera-name \"Camera\""
301  << std::endl
302  << std::endl;
303  std::cout << "***************************************************************" << std::endl;
304  }
305 
306  std::cout << "Considered intrinsic camera parameters:\n" << cam << "\n";
307 
308 #if defined(VISP_HAVE_X11)
309  vpDisplayX dc(I, 10, 10, "Color image");
310 #elif defined(VISP_HAVE_GDI)
311  vpDisplayGDI dc(I, 10, 10, "Color image");
312 #endif
313 
316  vpDetectorAprilTag detector(tagFamily);
317  detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
318  detector.setDisplayTag(display_tag);
319  detector.setAprilTagQuadDecimate(opt_quad_decimate);
320 
321  // Create visual features
322  vpFeaturePoint p, pd; // We use 1 point, the tag cog
323 
324  // Set desired position to the image center
325  pd.set_x(0);
326  pd.set_y(0);
327 
328  vpServo task;
329  // Add the visual feature point
330  task.addFeature(p, pd);
333 
334  if (opt_adaptive_gain) {
335  vpAdaptiveGain lambda(1.5, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
336  task.setLambda(lambda);
337  } else {
338  task.setLambda(opt_constant_gain);
339  }
340 
341  vpPlot *plotter = nullptr;
342  int iter_plot = 0;
343 
344  if (opt_plot) {
345  plotter = new vpPlot(2, static_cast<int>(250 * 2), 500, static_cast<int>(I.getWidth()) + 80, 10,
346  "Real time curves plotter");
347  plotter->setTitle(0, "Visual features error");
348  plotter->setTitle(1, "Joint velocities");
349  plotter->initGraph(0, 2);
350  plotter->initGraph(1, 2);
351  plotter->setLegend(0, 0, "error_feat_p_x");
352  plotter->setLegend(0, 1, "error_feat_p_y");
353  plotter->setLegend(1, 0, "qdot_pan");
354  plotter->setLegend(1, 1, "qdot_tilt");
355  }
356 
357  bool final_quit = false;
358  bool has_converged = false;
359  bool send_velocities = false;
360  bool servo_started = false;
361  std::vector<vpImagePoint> traj_cog;
362  vpMatrix eJe;
363 
364  static double t_init_servo = vpTime::measureTimeMs();
365 
366  robot.set_eMc(eMc); // Set location of the camera wrt end-effector frame
367 
368  vpVelocityTwistMatrix cVe = robot.get_cVe();
369  std::cout << cVe << std::endl;
370  task.set_cVe(cVe);
371 
373 
374  while (!has_converged && !final_quit) {
375  double t_start = vpTime::measureTimeMs();
376 
377  g.acquire(I);
378 
380 
381  std::vector<vpHomogeneousMatrix> cMo_vec;
382  detector.detect(I, opt_tag_size, cam, cMo_vec);
383 
384  std::stringstream ss;
385  ss << "Left click to " << (send_velocities ? "stop the robot" : "servo the robot") << ", right click to quit.";
386  vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
387 
388  vpColVector qdot(2);
389 
390  // Only one tag has to be detected
391  if (detector.getNbObjects() == 1) {
392 
393  vpImagePoint cog = detector.getCog(0);
394  double Z = cMo_vec[0][2][3];
395 
396  // Update current feature from measured cog position
397  double x = 0, y = 0;
398  vpPixelMeterConversion::convertPoint(cam, cog, x, y);
399  if (opt_verbose) {
400  std::cout << "Z: " << Z << std::endl;
401  }
402  p.set_xyZ(x, y, Z);
403  pd.set_Z(Z);
404 
405  // Get robot Jacobian
406  robot.get_eJe(eJe);
407  task.set_eJe(eJe);
408 
409  if (opt_task_sequencing) {
410  if (!servo_started) {
411  if (send_velocities) {
412  servo_started = true;
413  }
414  t_init_servo = vpTime::measureTimeMs();
415  }
416  qdot = task.computeControlLaw((vpTime::measureTimeMs() - t_init_servo) / 1000.);
417  } else {
418  qdot = task.computeControlLaw();
419  }
420 
421  // Display the current and desired feature points in the image display
423 
424  // Display the trajectory of the points used as features
425  if (opt_display_trajectory) {
426  display_point_trajectory(I, cog, traj_cog);
427  }
428 
429  if (opt_plot) {
430  plotter->plot(0, iter_plot, task.getError());
431  plotter->plot(1, iter_plot, qdot);
432  iter_plot++;
433  }
434 
435  if (opt_verbose) {
436  std::cout << "qdot: " << qdot.t() << std::endl;
437  }
438 
439  double error = task.getError().sumSquare();
440  ss.str("");
441  ss << "error: " << error;
442  vpDisplay::displayText(I, 20, static_cast<int>(I.getWidth()) - 150, ss.str(), vpColor::red);
443 
444  if (opt_verbose)
445  std::cout << "error: " << error << std::endl;
446 
447  if (error < convergence_threshold) {
448  has_converged = true;
449  std::cout << "Servo task has converged"
450  << "\n";
451  vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
452  }
453  } // end if (cMo_vec.size() == 1)
454  else {
455  qdot = 0;
456  }
457 
458  if (!send_velocities) {
459  qdot = 0;
460  }
461 
462  // Send to the robot
463  robot.setVelocity(vpRobot::JOINT_STATE, qdot);
464  ss.str("");
465  ss << "Loop time: " << vpTime::measureTimeMs() - t_start << " ms";
466  vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
467 
468  vpDisplay::flush(I);
469 
471  if (vpDisplay::getClick(I, button, false)) {
472  switch (button) {
474  send_velocities = !send_velocities;
475  break;
476 
478  final_quit = true;
479  qdot = 0;
480  break;
481 
482  default:
483  break;
484  }
485  }
486  }
487  std::cout << "Stop the robot " << std::endl;
489 
490  if (opt_plot && plotter != nullptr) {
491  delete plotter;
492  plotter = nullptr;
493  }
494 
495  if (!final_quit) {
496  while (!final_quit) {
497  g.acquire(I);
499 
500  vpDisplay::displayText(I, 20, 20, "Click to quit the program.", vpColor::red);
501  vpDisplay::displayText(I, 40, 20, "Visual servo converged.", vpColor::red);
502 
503  if (vpDisplay::getClick(I, false)) {
504  final_quit = true;
505  }
506 
507  vpDisplay::flush(I);
508  }
509  }
510  } catch (const vpRobotException &e) {
511  std::cout << "Catch Flir Ptu signal exception: " << e.getMessage() << std::endl;
513  } catch (const vpException &e) {
514  std::cout << "ViSP exception: " << e.what() << std::endl;
515  std::cout << "Stop the robot " << std::endl;
517  }
518 
519  return EXIT_SUCCESS;
520 }
521 #else
522 int main()
523 {
524 #if !defined(VISP_HAVE_FLYCAPTURE)
525  std::cout << "Install FLIR Flycapture" << std::endl;
526 #endif
527 #if !defined(VISP_HAVE_FLIR_PTU_SDK)
528  std::cout << "Install FLIR PTU SDK." << std::endl;
529 #endif
530  return 0;
531 }
532 #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 * getMessage() const
Definition: vpException.cpp:87
const char * what() const
Definition: vpException.cpp:99
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void set_xyZ(double x, double y, double Z)
void set_y(double y)
void set_x(double x)
void set_Z(double Z)
void open(vpImage< unsigned char > &I)
void acquire(vpImage< unsigned char > &I)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ JOINT_STATE
Definition: vpRobot.h:81
@ 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_L_cVe_eJe
Definition: vpServo.h:159
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:448
void setLambda(double c)
Definition: vpServo.h:404
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:506
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.
vpVelocityTwistMatrix get_cVe() const
Definition: vpUnicycle.h:82
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0)
VISP_EXPORT double measureTimeMs()