Visual Servoing Platform version 3.5.0
servoSimuPoint2DCamVelocity1.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
48#include <stdio.h>
49#include <stdlib.h>
50
51#include <visp3/core/vpHomogeneousMatrix.h>
52#include <visp3/core/vpMath.h>
53#include <visp3/io/vpParseArgv.h>
54#include <visp3/robot/vpSimulatorCamera.h>
55#include <visp3/visual_features/vpFeatureBuilder.h>
56#include <visp3/visual_features/vpFeaturePoint.h>
57#include <visp3/vs/vpServo.h>
58
59// List of allowed command line options
60#define GETOPTARGS "h"
61
62void usage(const char *name, const char *badparam);
63bool getOptions(int argc, const char **argv);
64
73void usage(const char *name, const char *badparam)
74{
75 fprintf(stdout, "\n\
76Simulation of a 2D visual servoing on a point:\n\
77- eye-in-hand control law,\n\
78- velocity computed in the camera frame,\n\
79- without display.\n\
80 \n\
81SYNOPSIS\n\
82 %s [-h]\n", name);
83
84 fprintf(stdout, "\n\
85OPTIONS: Default\n\
86 \n\
87 -h\n\
88 Print the help.\n");
89
90 if (badparam)
91 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
92}
93
104bool getOptions(int argc, const char **argv)
105{
106 const char *optarg_;
107 int c;
108 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
109
110 switch (c) {
111 case 'h':
112 usage(argv[0], NULL);
113 return false;
114
115 default:
116 usage(argv[0], optarg_);
117 return false;
118 }
119 }
120
121 if ((c == 1) || (c == -1)) {
122 // standalone param or error
123 usage(argv[0], NULL);
124 std::cerr << "ERROR: " << std::endl;
125 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
126 return false;
127 }
128
129 return true;
130}
131
132int main(int argc, const char **argv)
133{
134#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
135 try {
136 // Read the command line options
137 if (getOptions(argc, argv) == false) {
138 exit(-1);
139 }
140
141 vpServo task;
142 vpSimulatorCamera robot;
143
144 // sets the initial camera location
146 cMo[0][3] = 0.1;
147 cMo[1][3] = 0.2;
148 cMo[2][3] = 2;
149
150 // Compute the position of the object in the world frame
151 vpHomogeneousMatrix wMc, wMo;
152 robot.getPosition(wMc);
153 wMo = wMc * cMo;
154
155 // sets the point coordinates in the world frame
156 vpPoint point(0, 0, 0);
157
158 // computes the point coordinates in the camera frame and its 2D
159 // coordinates
160 point.track(cMo);
161
162 // sets the current position of the visual feature
164 vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
165
166 // sets the desired position of the visual feature
168 pd.buildFrom(0, 0, 1); // buildFrom(x,y,Z) ;
169
170 // define the task
171 // - we want an eye-in-hand control law
172 // - robot is controlled in the camera frame
174
175 // we want to see a point on a point
176 std::cout << std::endl;
177 task.addFeature(p, pd);
178
179 // set the gain
180 task.setLambda(1);
181
182 // Display task information
183 task.print();
184
185 unsigned int iter = 0;
186 // loop
187 while (iter++ < 100) {
188 std::cout << "---------------------------------------------" << iter << std::endl;
189 vpColVector v;
190
191 // get the robot position
192 robot.getPosition(wMc);
193 // Compute the position of the object frame in the camera frame
194 cMo = wMc.inverse() * wMo;
195
196 // new point position
197 point.track(cMo);
198 // retrieve x,y and Z of the vpPoint structure
199 vpFeatureBuilder::create(p, point);
200
201 // compute the control law
202 v = task.computeControlLaw();
203
204 // send the camera velocity to the controller
206
207 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
208 }
209
210 // Display task information
211 task.print();
212 return EXIT_SUCCESS;
213 } catch (const vpException &e) {
214 std::cout << "Catch a ViSP exception: " << e << std::endl;
215 return EXIT_FAILURE;
216 }
217#else
218 (void)argc;
219 (void)argv;
220 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
221 return EXIT_SUCCESS;
222#endif
223}
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
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)
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
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 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.