Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
servoSimuFourPoints2DCamVelocity.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  * Simulation of a 2D visual servoing using 4 points as visual feature.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
54 #include <stdio.h>
55 #include <stdlib.h>
56 
57 #include <visp3/core/vpConfig.h>
58 #include <visp3/core/vpHomogeneousMatrix.h>
59 #include <visp3/core/vpMath.h>
60 #include <visp3/io/vpParseArgv.h>
61 #include <visp3/robot/vpSimulatorCamera.h>
62 #include <visp3/visual_features/vpFeatureBuilder.h>
63 #include <visp3/visual_features/vpFeaturePoint.h>
64 #include <visp3/vs/vpServo.h>
65 
66 // List of allowed command line options
67 #define GETOPTARGS "h"
68 
69 void usage(const char *name, const char *badparam);
70 bool getOptions(int argc, const char **argv);
71 
80 void usage(const char *name, const char *badparam)
81 {
82  fprintf(stdout, "\n\
83 Simulation of a 2D visual servoing:\n\
84 - servo on 4 points,\n\
85 - eye-in-hand control law,\n\
86 - articular velocity are computed,\n\
87 - without display.\n\
88  \n\
89 SYNOPSIS\n\
90  %s [-h]\n", name);
91 
92  fprintf(stdout, "\n\
93 OPTIONS: Default\n\
94  \n\
95  -h\n\
96  Print the help.\n");
97 
98  if (badparam) {
99  fprintf(stderr, "ERROR: \n");
100  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
101  }
102 }
103 
114 bool getOptions(int argc, const char **argv)
115 {
116  const char *optarg_;
117  int c;
118  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
119 
120  switch (c) {
121  case 'h':
122  usage(argv[0], NULL);
123  return false;
124  break;
125 
126  default:
127  usage(argv[0], optarg_);
128  return false;
129  break;
130  }
131  }
132 
133  if ((c == 1) || (c == -1)) {
134  // standalone param or error
135  usage(argv[0], NULL);
136  std::cerr << "ERROR: " << std::endl;
137  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
138  return false;
139  }
140 
141  return true;
142 }
143 
144 int main(int argc, const char **argv)
145 {
146  try {
147  // Read the command line options
148  if (getOptions(argc, argv) == false) {
149  exit(-1);
150  }
151 
152  int i;
153  vpServo task;
154  vpSimulatorCamera robot;
155 
156  std::cout << std::endl;
157  std::cout << "-------------------------------------------------------" << std::endl;
158  std::cout << " Test program for vpServo " << std::endl;
159  std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
160  std::cout << " Simulation " << std::endl;
161  std::cout << " task : servo 4 points " << std::endl;
162  std::cout << "-------------------------------------------------------" << std::endl;
163  std::cout << std::endl;
164 
165  // sets the initial camera location with respect to the object
167  cMo[0][3] = 0.1;
168  cMo[1][3] = 0.2;
169  cMo[2][3] = 2;
170 
171  // Compute the position of the object in the world frame
172  vpHomogeneousMatrix wMc, wMo;
173  robot.getPosition(wMc);
174  wMo = wMc * cMo;
175 
176  // sets the point coordinates in the object frame
177  vpPoint point[4];
178  point[0].setWorldCoordinates(-1, -1, 0);
179  point[1].setWorldCoordinates(1, -1, 0);
180  point[2].setWorldCoordinates(1, 1, 0);
181  point[3].setWorldCoordinates(-1, 1, 0);
182 
183  // computes the point coordinates in the camera frame and its 2D
184  // coordinates
185  for (i = 0; i < 4; i++)
186  point[i].track(cMo);
187 
188  // sets the desired position of the point
189  vpFeaturePoint p[4];
190  for (i = 0; i < 4; i++)
191  vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
192 
193  // sets the desired position of the point
194  vpFeaturePoint pd[4];
195 
196  pd[0].buildFrom(-0.1, -0.1, 1);
197  pd[1].buildFrom(0.1, -0.1, 1);
198  pd[2].buildFrom(0.1, 0.1, 1);
199  pd[3].buildFrom(-0.1, 0.1, 1);
200 
201  // define the task
202  // - we want an eye-in-hand control law
203  // - articular velocity are computed
206 
207  // Set the position of the end-effector frame in the camera frame as identity
209  vpVelocityTwistMatrix cVe(cMe);
210  task.set_cVe(cVe);
211 
212  // Set the Jacobian (expressed in the end-effector frame)
213  vpMatrix eJe;
214  robot.get_eJe(eJe);
215  task.set_eJe(eJe);
216 
217  // we want to see a point on a point
218  for (i = 0; i < 4; i++)
219  task.addFeature(p[i], pd[i]);
220 
221  // set the gain
222  task.setLambda(1);
223 
224  // Display task information
225  task.print();
226 
227  unsigned int iter = 0;
228  // loop
229  while (iter++ < 1500) {
230  std::cout << "---------------------------------------------" << iter << std::endl;
231  vpColVector v;
232 
233  // Set the Jacobian (expressed in the end-effector frame)
234  // since q is modified eJe is modified
235  robot.get_eJe(eJe);
236  task.set_eJe(eJe);
237 
238  // get the robot position
239  robot.getPosition(wMc);
240  // Compute the position of the object frame in the camera frame
241  cMo = wMc.inverse() * wMo;
242 
243  // update new point position and corresponding features
244  for (i = 0; i < 4; i++) {
245  point[i].track(cMo);
246  // retrieve x,y and Z of the vpPoint structure
247  vpFeatureBuilder::create(p[i], point[i]);
248  }
249  // since vpServo::MEAN interaction matrix is used, we need also to
250  // update the desired features at each iteration
251  pd[0].buildFrom(-0.1, -0.1, 1);
252  pd[1].buildFrom(0.1, -0.1, 1);
253  pd[2].buildFrom(0.1, 0.1, 1);
254  pd[3].buildFrom(-0.1, 0.1, 1);
255 
256  // compute the control law ") ;
257  v = task.computeControlLaw();
258 
259  // send the camera velocity to the controller ") ;
261 
262  std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
263  }
264 
265  // Display task information
266  task.print();
267  task.kill();
268  return EXIT_SUCCESS;
269  } catch (const vpException &e) {
270  std::cout << "Catch a ViSP exception: " << e << std::endl;
271  return EXIT_FAILURE;
272  }
273 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void buildFrom(double x, double y, double Z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
void set_eJe(const vpMatrix &eJe_)
Definition: vpServo.h:508
error that can be emited by ViSP classes.
Definition: vpException.h:71
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
vpHomogeneousMatrix getPosition() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
Class that defines what is a point.
Definition: vpPoint.h:58
void kill()
Definition: vpServo.cpp:192
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
void setLambda(double c)
Definition: vpServo.h:406
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void set_cVe(const vpVelocityTwistMatrix &cVe_)
Definition: vpServo.h:450
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
vpColVector getError() const
Definition: vpServo.h:282
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223