Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
servoSimuPoint2DCamVelocity2.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  * Simulation of a 2D visual servoing on a point.
33  *
34 *****************************************************************************/
35 
52 #include <stdio.h>
53 #include <stdlib.h>
54 
55 #include <visp3/core/vpHomogeneousMatrix.h>
56 #include <visp3/core/vpMath.h>
57 #include <visp3/io/vpParseArgv.h>
58 #include <visp3/robot/vpSimulatorCamera.h>
59 #include <visp3/visual_features/vpFeatureBuilder.h>
60 #include <visp3/visual_features/vpFeaturePoint.h>
61 #include <visp3/vs/vpServo.h>
62 
63 // List of allowed command line options
64 #define GETOPTARGS "h"
65 
66 void usage(const char *name, const char *badparam);
67 bool getOptions(int argc, const char **argv);
68 
77 void usage(const char *name, const char *badparam)
78 {
79  fprintf(stdout, "\n\
80 Simulation of a 2D visual servoing on a point:\n\
81 - eye-in-hand control law,\n\
82 - articular velocity are computed,\n\
83 - without display.\n\
84 \n\
85 SYNOPSIS\n\
86  %s [-h]\n",
87  name);
88 
89  fprintf(stdout, "\n\
90 OPTIONS: Default\n\
91 \n\
92  -h\n\
93  Print the help.\n");
94 
95  if (badparam)
96  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
97 }
98 
109 bool getOptions(int argc, const char **argv)
110 {
111  const char *optarg_;
112  int c;
113  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
114 
115  switch (c) {
116  case 'h':
117  usage(argv[0], nullptr);
118  return false;
119 
120  default:
121  usage(argv[0], optarg_);
122  return false;
123  }
124  }
125 
126  if ((c == 1) || (c == -1)) {
127  // standalone param or error
128  usage(argv[0], nullptr);
129  std::cerr << "ERROR: " << std::endl;
130  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
131  return false;
132  }
133 
134  return true;
135 }
136 
137 int main(int argc, const char **argv)
138 {
139 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
140  try {
141  // Read the command line options
142  if (getOptions(argc, argv) == false) {
143  return EXIT_FAILURE;
144  }
145 
146  vpServo task;
147  vpSimulatorCamera robot;
148 
149  std::cout << std::endl;
150  std::cout << "-------------------------------------------------------" << std::endl;
151  std::cout << " Test program for vpServo " << std::endl;
152  std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
153  std::cout << " Simulation " << std::endl;
154  std::cout << " task : servo a point " << std::endl;
155  std::cout << "-------------------------------------------------------" << std::endl;
156  std::cout << std::endl;
157 
158  // sets the initial camera location
160  cMo[0][3] = 0.1;
161  cMo[1][3] = 0.2;
162  cMo[2][3] = 2;
163  // Compute the position of the object in the world frame
164  vpHomogeneousMatrix wMc, wMo;
165  robot.getPosition(wMc);
166  wMo = wMc * cMo;
167 
168  // sets the point coordinates in the world frame
169  vpPoint point(0, 0, 0);
170 
171  // computes the point coordinates in the camera frame and its 2D
172  // coordinates
173  point.track(cMo);
174 
175  // sets the current position of the visual feature
176  vpFeaturePoint p;
177  vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
178 
179  // sets the desired position of the visual feature
180  vpFeaturePoint pd;
181  pd.buildFrom(0, 0, 1);
182 
183  // define the task
184  // - we want an eye-in-hand control law
185  // - articular velocity are computed
188 
189  // Set the position of the end-effector frame in the camera frame
191  vpVelocityTwistMatrix cVe(cMe);
192  task.set_cVe(cVe);
193 
194  // Set the Jacobian (expressed in the end-effector frame)
195  vpMatrix eJe;
196  robot.get_eJe(eJe);
197  task.set_eJe(eJe);
198 
199  // we want to see a point on a point
200  task.addFeature(p, pd);
201 
202  // set the gain
203  task.setLambda(1);
204  // Display task information
205  task.print();
206 
207  unsigned int iter = 0;
208  // loop
209  while (iter++ < 100) {
210  std::cout << "---------------------------------------------" << iter << std::endl;
211  vpColVector v;
212 
213  // Set the Jacobian (expressed in the end-effector frame)
214  // since q is modified eJe is modified
215  robot.get_eJe(eJe);
216  task.set_eJe(eJe);
217 
218  // get the robot position
219  robot.getPosition(wMc);
220  // Compute the position of the object frame in the camera frame
221  cMo = wMc.inverse() * wMo;
222 
223  // new point position
224  point.track(cMo);
225  vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
226  pd.buildFrom(0, 0, 1); // Since vpServo::MEAN interaction matrix is
227  // used, we need to update the desired feature at
228  // each iteration
229 
230  // compute the control law
231  v = task.computeControlLaw();
232 
233  // send the camera velocity to the controller
235 
236  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
237  }
238 
239  // Display task information
240  task.print();
241  return EXIT_SUCCESS;
242  } catch (const vpException &e) {
243  std::cout << "Catch a ViSP exception: " << e << std::endl;
244  return EXIT_FAILURE;
245  }
246 #else
247  (void)argc;
248  (void)argv;
249  std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
250  return EXIT_SUCCESS;
251 #endif
252 }
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
void buildFrom(double x, double y, double Z)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
void get_eJe(vpMatrix &eJe) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:378
@ EYEINHAND_L_cVe_eJe
Definition: vpServo.h:162
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:329
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:1028
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:169
void setLambda(double c)
Definition: vpServo.h:976
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:1091
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:132
vpColVector getError() const
Definition: vpServo.h:504
vpColVector computeControlLaw()
Definition: vpServo.cpp:703
@ MEAN
Definition: vpServo.h:208
Class that defines the simplest robot: a free flying camera.