Visual Servoing Platform version 3.5.0
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
69void usage(const char *name, const char *badparam);
70bool getOptions(int argc, const char **argv);
71
80void usage(const char *name, const char *badparam)
81{
82 fprintf(stdout, "\n\
83Simulation 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\
89SYNOPSIS\n\
90 %s [-h]\n", name);
91
92 fprintf(stdout, "\n\
93OPTIONS: 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
114bool 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
125 default:
126 usage(argv[0], optarg_);
127 return false;
128 }
129 }
130
131 if ((c == 1) || (c == -1)) {
132 // standalone param or error
133 usage(argv[0], NULL);
134 std::cerr << "ERROR: " << std::endl;
135 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
136 return false;
137 }
138
139 return true;
140}
141
142int main(int argc, const char **argv)
143{
144#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
145 try {
146 // Read the command line options
147 if (getOptions(argc, argv) == false) {
148 exit(-1);
149 }
150
151 vpServo task;
152 vpSimulatorCamera robot;
153
154 std::cout << std::endl;
155 std::cout << "-------------------------------------------------------" << std::endl;
156 std::cout << " Test program for vpServo " << std::endl;
157 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
158 std::cout << " Simulation " << std::endl;
159 std::cout << " task : servo 4 points " << std::endl;
160 std::cout << "-------------------------------------------------------" << std::endl;
161 std::cout << std::endl;
162
163 // sets the initial camera location with respect to the object
165 cMo[0][3] = 0.1;
166 cMo[1][3] = 0.2;
167 cMo[2][3] = 2;
168
169 // Compute the position of the object in the world frame
170 vpHomogeneousMatrix wMc, wMo;
171 robot.getPosition(wMc);
172 wMo = wMc * cMo;
173
174 // sets the point coordinates in the object frame
175 vpPoint point[4];
176 point[0].setWorldCoordinates(-1, -1, 0);
177 point[1].setWorldCoordinates(1, -1, 0);
178 point[2].setWorldCoordinates(1, 1, 0);
179 point[3].setWorldCoordinates(-1, 1, 0);
180
181 // computes the point coordinates in the camera frame and its 2D
182 // coordinates
183 for (unsigned int i = 0; i < 4; i++)
184 point[i].track(cMo);
185
186 // sets the desired position of the point
187 vpFeaturePoint p[4];
188 for (unsigned int i = 0; i < 4; i++)
189 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
190
191 // sets the desired position of the point
192 vpFeaturePoint pd[4];
193
194 pd[0].buildFrom(-0.1, -0.1, 1);
195 pd[1].buildFrom(0.1, -0.1, 1);
196 pd[2].buildFrom(0.1, 0.1, 1);
197 pd[3].buildFrom(-0.1, 0.1, 1);
198
199 // define the task
200 // - we want an eye-in-hand control law
201 // - articular velocity are computed
204
205 // Set the position of the end-effector frame in the camera frame as identity
207 vpVelocityTwistMatrix cVe(cMe);
208 task.set_cVe(cVe);
209
210 // Set the Jacobian (expressed in the end-effector frame)
211 vpMatrix eJe;
212 robot.get_eJe(eJe);
213 task.set_eJe(eJe);
214
215 // we want to see a point on a point
216 for (unsigned int i = 0; i < 4; i++)
217 task.addFeature(p[i], pd[i]);
218
219 // set the gain
220 task.setLambda(1);
221
222 // Display task information
223 task.print();
224
225 unsigned int iter = 0;
226 // loop
227 while (iter++ < 1500) {
228 std::cout << "---------------------------------------------" << iter << std::endl;
229 vpColVector v;
230
231 // Set the Jacobian (expressed in the end-effector frame)
232 // since q is modified eJe is modified
233 robot.get_eJe(eJe);
234 task.set_eJe(eJe);
235
236 // get the robot position
237 robot.getPosition(wMc);
238 // Compute the position of the object frame in the camera frame
239 cMo = wMc.inverse() * wMo;
240
241 // update new point position and corresponding features
242 for (unsigned int i = 0; i < 4; i++) {
243 point[i].track(cMo);
244 // retrieve x,y and Z of the vpPoint structure
245 vpFeatureBuilder::create(p[i], point[i]);
246 }
247 // since vpServo::MEAN interaction matrix is used, we need also to
248 // update the desired features at each iteration
249 pd[0].buildFrom(-0.1, -0.1, 1);
250 pd[1].buildFrom(0.1, -0.1, 1);
251 pd[2].buildFrom(0.1, 0.1, 1);
252 pd[3].buildFrom(-0.1, 0.1, 1);
253
254 // compute the control law ") ;
255 v = task.computeControlLaw();
256
257 // send the camera velocity to the controller ") ;
259
260 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
261 }
262
263 // Display task information
264 task.print();
265 return EXIT_SUCCESS;
266 } catch (const vpException &e) {
267 std::cout << "Catch a ViSP exception: " << e << std::endl;
268 return EXIT_FAILURE;
269 }
270#else
271 (void)argc;
272 (void)argv;
273 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
274 return EXIT_SUCCESS;
275#endif
276}
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)
void track(const vpHomogeneousMatrix &cMo)
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 setWorldCoordinates(double oX, double oY, double oZ)
Definition: vpPoint.cpp:113
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition: vpRobot.h:82
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ 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
@ MEAN
Definition: vpServo.h:190
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.