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