Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
servoSimuPoint2DCamVelocity2.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 on a point.
33 *
34*****************************************************************************/
35
52#include <stdio.h>
53#include <stdlib.h>
54
55#include <visp3/core/vpHomogeneousMatrix.h>
56#include <visp3/core/vpMath.h>
57#include <visp3/io/vpParseArgv.h>
58#include <visp3/robot/vpSimulatorCamera.h>
59#include <visp3/visual_features/vpFeatureBuilder.h>
60#include <visp3/visual_features/vpFeaturePoint.h>
61#include <visp3/vs/vpServo.h>
62
63// List of allowed command line options
64#define GETOPTARGS "h"
65
66void usage(const char *name, const char *badparam);
67bool getOptions(int argc, const char **argv);
68
77void usage(const char *name, const char *badparam)
78{
79 fprintf(stdout, "\n\
80Simulation of a 2D visual servoing on a point:\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(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
97}
98
109bool getOptions(int argc, const char **argv)
110{
111 const char *optarg_;
112 int c;
113 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
114
115 switch (c) {
116 case 'h':
117 usage(argv[0], NULL);
118 return false;
119
120 default:
121 usage(argv[0], optarg_);
122 return false;
123 }
124 }
125
126 if ((c == 1) || (c == -1)) {
127 // standalone param or error
128 usage(argv[0], NULL);
129 std::cerr << "ERROR: " << std::endl;
130 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
131 return false;
132 }
133
134 return true;
135}
136
137int main(int argc, const char **argv)
138{
139#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
140 try {
141 // Read the command line options
142 if (getOptions(argc, argv) == false) {
143 return EXIT_FAILURE;
144 }
145
146 vpServo task;
147 vpSimulatorCamera robot;
148
149 std::cout << std::endl;
150 std::cout << "-------------------------------------------------------" << std::endl;
151 std::cout << " Test program for vpServo " << std::endl;
152 std::cout << " Eye-in-hand task control, articular velocity are computed" << std::endl;
153 std::cout << " Simulation " << std::endl;
154 std::cout << " task : servo a point " << std::endl;
155 std::cout << "-------------------------------------------------------" << std::endl;
156 std::cout << std::endl;
157
158 // sets the initial camera location
160 cMo[0][3] = 0.1;
161 cMo[1][3] = 0.2;
162 cMo[2][3] = 2;
163 // Compute the position of the object in the world frame
164 vpHomogeneousMatrix wMc, wMo;
165 robot.getPosition(wMc);
166 wMo = wMc * cMo;
167
168 // sets the point coordinates in the world frame
169 vpPoint point(0, 0, 0);
170
171 // computes the point coordinates in the camera frame and its 2D
172 // coordinates
173 point.track(cMo);
174
175 // sets the current position of the visual feature
177 vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
178
179 // sets the desired position of the visual feature
181 pd.buildFrom(0, 0, 1);
182
183 // define the task
184 // - we want an eye-in-hand control law
185 // - articular velocity are computed
188
189 // Set the position of the end-effector frame in the camera frame
191 vpVelocityTwistMatrix cVe(cMe);
192 task.set_cVe(cVe);
193
194 // Set the Jacobian (expressed in the end-effector frame)
195 vpMatrix eJe;
196 robot.get_eJe(eJe);
197 task.set_eJe(eJe);
198
199 // we want to see a point on a point
200 task.addFeature(p, pd);
201
202 // set the gain
203 task.setLambda(1);
204 // Display task information
205 task.print();
206
207 unsigned int iter = 0;
208 // loop
209 while (iter++ < 100) {
210 std::cout << "---------------------------------------------" << iter << std::endl;
211 vpColVector v;
212
213 // Set the Jacobian (expressed in the end-effector frame)
214 // since q is modified eJe is modified
215 robot.get_eJe(eJe);
216 task.set_eJe(eJe);
217
218 // get the robot position
219 robot.getPosition(wMc);
220 // Compute the position of the object frame in the camera frame
221 cMo = wMc.inverse() * wMo;
222
223 // new point position
224 point.track(cMo);
225 vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
226 pd.buildFrom(0, 0, 1); // Since vpServo::MEAN interaction matrix is
227 // used, we need to update the desired feature at
228 // each iteration
229
230 // compute the control law
231 v = task.computeControlLaw();
232
233 // send the camera velocity to the controller
235
236 std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
237 }
238
239 // Display task information
240 task.print();
241 return EXIT_SUCCESS;
242 } catch (const vpException &e) {
243 std::cout << "Catch a ViSP exception: " << e << std::endl;
244 return EXIT_FAILURE;
245 }
246#else
247 (void)argc;
248 (void)argv;
249 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
250 return EXIT_SUCCESS;
251#endif
252}
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)
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 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.