Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuFourPoints2DCamVelocity.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 using 4 points as visual feature.
33 *
34*****************************************************************************/
35
50#include <stdio.h>
51#include <stdlib.h>
52
53#include <visp3/core/vpConfig.h>
54#include <visp3/core/vpHomogeneousMatrix.h>
55#include <visp3/core/vpMath.h>
56#include <visp3/io/vpParseArgv.h>
57#include <visp3/robot/vpSimulatorCamera.h>
58#include <visp3/visual_features/vpFeatureBuilder.h>
59#include <visp3/visual_features/vpFeaturePoint.h>
60#include <visp3/vs/vpServo.h>
61
62// List of allowed command line options
63#define GETOPTARGS "h"
64
65void usage(const char *name, const char *badparam);
66bool getOptions(int argc, const char **argv);
67
76void usage(const char *name, const char *badparam)
77{
78 fprintf(stdout, "\n\
79Simulation of a 2D visual servoing:\n\
80- servo on 4 points,\n\
81- eye-in-hand control law,\n\
82- articular velocity are computed,\n\
83- without display.\n\
84 \n\
85SYNOPSIS\n\
86 %s [-h]\n",
87 name);
88
89 fprintf(stdout, "\n\
90OPTIONS: Default\n\
91 \n\
92 -h\n\
93 Print the help.\n");
94
95 if (badparam) {
96 fprintf(stderr, "ERROR: \n");
97 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
98 }
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 return EXIT_FAILURE;
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 4 points " << std::endl;
157 std::cout << "-------------------------------------------------------" << std::endl;
158 std::cout << std::endl;
159
160 // sets the initial camera location with respect to the object
162 cMo[0][3] = 0.1;
163 cMo[1][3] = 0.2;
164 cMo[2][3] = 2;
165
166 // Compute the position of the object in the world frame
167 vpHomogeneousMatrix wMc, wMo;
168 robot.getPosition(wMc);
169 wMo = wMc * cMo;
170
171 // sets the point coordinates in the object frame
172 vpPoint point[4];
173 point[0].setWorldCoordinates(-1, -1, 0);
174 point[1].setWorldCoordinates(1, -1, 0);
175 point[2].setWorldCoordinates(1, 1, 0);
176 point[3].setWorldCoordinates(-1, 1, 0);
177
178 // computes the point coordinates in the camera frame and its 2D
179 // coordinates
180 for (unsigned int i = 0; i < 4; i++)
181 point[i].track(cMo);
182
183 // sets the desired position of the point
184 vpFeaturePoint p[4];
185 for (unsigned int i = 0; i < 4; i++)
186 vpFeatureBuilder::create(p[i], point[i]); // retrieve x,y and Z of the vpPoint structure
187
188 // sets the desired position of the point
189 vpFeaturePoint pd[4];
190
191 pd[0].buildFrom(-0.1, -0.1, 1);
192 pd[1].buildFrom(0.1, -0.1, 1);
193 pd[2].buildFrom(0.1, 0.1, 1);
194 pd[3].buildFrom(-0.1, 0.1, 1);
195
196 // define the task
197 // - we want an eye-in-hand control law
198 // - articular velocity are computed
201
202 // Set the position of the end-effector frame in the camera frame as identity
204 vpVelocityTwistMatrix cVe(cMe);
205 task.set_cVe(cVe);
206
207 // Set the Jacobian (expressed in the end-effector frame)
208 vpMatrix eJe;
209 robot.get_eJe(eJe);
210 task.set_eJe(eJe);
211
212 // we want to see a point on a point
213 for (unsigned int i = 0; i < 4; i++)
214 task.addFeature(p[i], pd[i]);
215
216 // set the gain
217 task.setLambda(1);
218
219 // Display task information
220 task.print();
221
222 unsigned int iter = 0;
223 // loop
224 while (iter++ < 1500) {
225 std::cout << "---------------------------------------------" << iter << std::endl;
226 vpColVector v;
227
228 // Set the Jacobian (expressed in the end-effector frame)
229 // since q is modified eJe is modified
230 robot.get_eJe(eJe);
231 task.set_eJe(eJe);
232
233 // get the robot position
234 robot.getPosition(wMc);
235 // Compute the position of the object frame in the camera frame
236 cMo = wMc.inverse() * wMo;
237
238 // update new point position and corresponding features
239 for (unsigned int i = 0; i < 4; i++) {
240 point[i].track(cMo);
241 // retrieve x,y and Z of the vpPoint structure
242 vpFeatureBuilder::create(p[i], point[i]);
243 }
244 // since vpServo::MEAN interaction matrix is used, we need also to
245 // update the desired features at each iteration
246 pd[0].buildFrom(-0.1, -0.1, 1);
247 pd[1].buildFrom(0.1, -0.1, 1);
248 pd[2].buildFrom(0.1, 0.1, 1);
249 pd[3].buildFrom(-0.1, 0.1, 1);
250
251 // compute the control law ") ;
252 v = task.computeControlLaw();
253
254 // send the camera velocity to the controller ") ;
256
257 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
258 }
259
260 // Display task information
261 task.print();
262 return EXIT_SUCCESS;
263 } catch (const vpException &e) {
264 std::cout << "Catch a ViSP exception: " << e << std::endl;
265 return EXIT_FAILURE;
266 }
267#else
268 (void)argc;
269 (void)argv;
270 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
271 return EXIT_SUCCESS;
272#endif
273}
Implementation of column vector and the associated operations.
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)
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:152
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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 setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ CAMERA_FRAME
Definition vpRobot.h:80
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_L_cVe_eJe
Definition vpServo.h:155
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:299
void setLambda(double c)
Definition vpServo.h:403
void set_eJe(const vpMatrix &eJe_)
Definition vpServo.h:506
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpColVector getError() const
Definition vpServo.h:276
vpColVector computeControlLaw()
Definition vpServo.cpp:930
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that defines the simplest robot: a free flying camera.