Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
manServo4PointsDisplay.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 visual servoing with display.
33 *
34*****************************************************************************/
35
46#include <visp3/core/vpConfig.h>
47#include <visp3/core/vpDebug.h>
48
49#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
50
51#include <visp3/core/vpCameraParameters.h>
52#include <visp3/core/vpImage.h>
53#include <visp3/core/vpImageConvert.h>
54#include <visp3/core/vpTime.h>
55#include <visp3/gui/vpDisplayGDI.h>
56#include <visp3/gui/vpDisplayGTK.h>
57#include <visp3/gui/vpDisplayOpenCV.h>
58#include <visp3/gui/vpDisplayX.h>
59
60#include <visp3/core/vpHomogeneousMatrix.h>
61#include <visp3/core/vpIoTools.h>
62#include <visp3/core/vpMath.h>
63#include <visp3/robot/vpSimulatorCamera.h>
64#include <visp3/vision/vpPose.h>
65#include <visp3/visual_features/vpFeatureBuilder.h>
66#include <visp3/visual_features/vpFeaturePoint.h>
67#include <visp3/vs/vpServo.h>
68#include <visp3/vs/vpServoDisplay.h>
69
70int main()
71{
72 try {
74 // sets the initial camera location
75 vpHomogeneousMatrix cMo(0.3, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(40));
76 vpHomogeneousMatrix wMo; // Set to identity
77 vpHomogeneousMatrix wMc; // Camera position in the world frame
78
80 // initialize the robot
82 robot.setSamplingTime(0.04); // 40ms
83 wMc = wMo * cMo.inverse();
84 robot.setPosition(wMc);
85
86 // initialize the camera parameters
87 vpCameraParameters cam(800, 800, 240, 180);
88
89 // Image definition
90 unsigned int height = 360;
91 unsigned int width = 480;
92 vpImage<unsigned char> I(height, width);
93
94 // Display initialization
95#if defined(VISP_HAVE_X11)
96 vpDisplayX disp;
97#elif defined(VISP_HAVE_GTK)
98 vpDisplayGTK disp;
99#elif defined(VISP_HAVE_GDI)
100 vpDisplayGDI disp;
101#elif defined(HAVE_OPENCV_HIGHGUI)
102 vpDisplayOpenCV disp;
103#endif
104
105#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
106 disp.init(I, 100, 100, "Simulation display");
107#endif
108
110 // Desired visual features initialization
111
112 // sets the points coordinates in the object frame (in meter)
113 vpPoint point[4];
114 point[0].setWorldCoordinates(-0.1, -0.1, 0);
115 point[1].setWorldCoordinates(0.1, -0.1, 0);
116 point[2].setWorldCoordinates(0.1, 0.1, 0);
117 point[3].setWorldCoordinates(-0.1, 0.1, 0);
118
119 // sets the desired camera location
120 vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
121
122 // computes the 3D point coordinates in the camera frame and its 2D
123 // coordinates
124 for (int i = 0; i < 4; i++)
125 point[i].project(cMo_d);
126
127 // creates the associated features
128 vpFeaturePoint pd[4];
129 for (int i = 0; i < 4; i++)
130 vpFeatureBuilder::create(pd[i], point[i]);
131
133 // Current visual features initialization
134
135 // computes the 3D point coordinates in the camera frame and its 2D
136 // coordinates
137 for (int i = 0; i < 4; i++)
138 point[i].project(cMo);
139
140 // creates the associated features
141 vpFeaturePoint p[4];
142 for (int i = 0; i < 4; i++)
143 vpFeatureBuilder::create(p[i], point[i]);
144
146 // Task defintion
147 vpServo task;
148 // we want an eye-in-hand control law ;
151
152 // Set the position of the end-effector frame in the camera frame as identity
154 vpVelocityTwistMatrix cVe(cMe);
155 task.set_cVe(cVe);
156 // Set the Jacobian (expressed in the end-effector frame)
157 vpMatrix eJe;
158 robot.get_eJe(eJe);
159 task.set_eJe(eJe);
160
161 // we want to see a point on a point
162 for (int i = 0; i < 4; i++)
163 task.addFeature(p[i], pd[i]);
164 // Set the gain
165 task.setLambda(1.0);
166 // Print the current information about the task
167 task.print();
168
170 // The control loop
171 int k = 0;
172 while (k++ < 200) {
173 double t = vpTime::measureTimeMs();
174
175 // Display the image background
177
178 // Update the current features
179 for (int i = 0; i < 4; i++) {
180 point[i].project(cMo);
181 vpFeatureBuilder::create(p[i], point[i]);
182 }
183
184 // Display the task features (current and desired)
185 vpServoDisplay::display(task, cam, I);
187
188 // Update the robot Jacobian
189 robot.get_eJe(eJe);
190 task.set_eJe(eJe);
191
192 // Compute the control law
194
195 // Send the computed velocity to the robot and compute the new robot
196 // position
198 wMc = robot.getPosition();
199 cMo = wMc.inverse() * wMo;
200
201 // Print the current information about the task
202 task.print();
203
204 // Wait 40 ms
205 vpTime::wait(t, 40);
206 }
207 return EXIT_SUCCESS;
208 }
209 catch (const vpException &e) {
210 std::cout << "Catch an exception: " << e << std::endl;
211 return EXIT_FAILURE;
212 }
213}
214
215#else
216int main()
217{
218 std::cout
219 << "You do not have X11, GTK, or OpenCV, or GDI (Graphical Device Interface) functionalities to display images..."
220 << std::endl;
221 std::cout << "Tip if you are on a unix-like system:" << std::endl;
222 std::cout << "- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
223 std::cout << "Tip if you are on a windows-like system:" << std::endl;
224 std::cout << "- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
225 return EXIT_SUCCESS;
226}
227#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
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...
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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)
@ ARTICULAR_FRAME
Definition vpRobot.h:76
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
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
@ PSEUDO_INVERSE
Definition vpServo.h:199
vpColVector computeControlLaw()
Definition vpServo.cpp:930
@ DESIRED
Definition vpServo.h:183
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.
VISP_EXPORT double measureTimeMs()
VISP_EXPORT int wait(double t0, double t)