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