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