Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRobotViper850Pose.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 * Test for Afma 6 dof robot.
33 *
34*****************************************************************************/
35
46#include <iostream>
47#include <visp3/blob/vpDot.h>
48#include <visp3/core/vpCameraParameters.h>
49#include <visp3/core/vpConfig.h>
50#include <visp3/core/vpDebug.h>
51#include <visp3/core/vpImage.h>
52#include <visp3/core/vpPixelMeterConversion.h>
53#include <visp3/core/vpPoint.h>
54#include <visp3/gui/vpDisplayGTK.h>
55#include <visp3/gui/vpDisplayOpenCV.h>
56#include <visp3/gui/vpDisplayX.h>
57#include <visp3/robot/vpRobotViper850.h>
58#include <visp3/sensor/vp1394TwoGrabber.h>
59#include <visp3/vision/vpPose.h>
60#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394)
61
62int main()
63{
64 try {
65 // Create an image B&W container
67
68 // Create a firewire grabber based on libdc1394-2.x
69 bool reset = false;
70 vp1394TwoGrabber g(reset);
71
72 // Grab an image from the firewire camera
73 g.acquire(I);
74
75// Create an image viewer for the image
76#ifdef VISP_HAVE_X11
77 vpDisplayX display(I, 100, 100, "Current image");
78#elif defined(HAVE_OPENCV_HIGHGUI)
79 vpDisplayOpenCV display(I, 100, 100, "Current image");
80#elif defined(VISP_HAVE_GTK)
81 vpDisplayGTK display(I, 100, 100, "Current image");
82#endif
83
84 // Display the image
87
88 // Define a squared target
89 // The target is made of 4 planar points (square dim = 0.077m)
90 double sdim = 0.077; // square width and height
91 vpPoint target[4];
92 // Set the point world coordinates (x,y,z) in the object frame
93 // o ----> x
94 // |
95 // |
96 // \/
97 // y
98 target[0].setWorldCoordinates(-sdim / 2., -sdim / 2., 0);
99 target[1].setWorldCoordinates(sdim / 2., -sdim / 2., 0);
100 target[2].setWorldCoordinates(sdim / 2., sdim / 2., 0);
101 target[3].setWorldCoordinates(-sdim / 2., sdim / 2., 0);
102
103 // Image processing to extract the 2D coordinates in sub-pixels of the 4
104 // points from the image acquired by the camera
105 // Creation of 4 trackers
106 vpDot dot[4];
107 vpImagePoint cog;
108 for (int i = 0; i < 4; i++) {
109 dot[i].setGraphics(true); // to display the tracking results
110 std::cout << "Click on dot " << i << std::endl;
111 dot[i].initTracking(I);
112 // The tracker computes the sub-pixels coordinates in the image
113 // i ----> u
114 // |
115 // |
116 // \/
117 // v
118 std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
119 // Flush the tracking results in the viewer
121 }
122
123 // Create an intrinsic camera parameters structure
125
126 // Create a robot access
127 vpRobotViper850 robot;
128
129 // Load the end-effector to camera frame transformation obtained
130 // using a camera intrinsic model with distortion
132
133 // Get the intrinsic camera parameters associated to the image
134 robot.getCameraParameters(cam, I);
135
136 // Using the camera parameters, compute the perspective projection
137 // (transform the dot sub-pixel coordinates into coordinates in the camera
138 // frame in meter)
139 for (int i = 0; i < 4; i++) {
140 double x = 0, y = 0; // coordinates of the dots in the camera frame
141 // c ----> x
142 // |
143 // |
144 // \/
145 // y
146 // pixel to meter conversion
147 cog = dot[i].getCog();
149 target[i].set_x(x);
150 target[i].set_y(y);
151 }
152
153 // From now, in target[i], we have the 3D coordinates of a point in the
154 // object frame, and their correspondances in the camera frame. We can now
155 // compute the pose cMo between the camera and the object.
156 vpPose pose;
157 // Add the 4 points to compute the pose
158 for (int i = 0; i < 4; i++) {
159 pose.addPoint(target[i]);
160 }
161 // Create an homogeneous matrix for the camera to object transformation
162 // computed just bellow
165 vpRxyzVector r;
166 // Compute the pose: initialisation is done by Dementhon or Lagrange method, and the
167 // final pose is computed by the more accurate Virtual Visual Servoing method.
169
170 std::cout << "Pose cMo: " << std::endl << cMo;
171 cMo.extract(R);
172 r.buildFrom(R);
173 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
174 << std::endl
175 << std::endl;
176
177 // Get the robot position in the reference frame
179 vpColVector p; // position x,y,z,rx,ry,rz
180 robot.getPosition(vpRobotViper850::REFERENCE_FRAME, p);
181 std::cout << "Robot pose in reference frame: " << p << std::endl;
183 t[0] = p[0];
184 t[1] = p[1];
185 t[2] = p[2];
186 r[0] = p[3];
187 r[1] = p[4];
188 r[2] = p[5];
189 R.buildFrom(r);
190 rMc.buildFrom(t, R);
191 std::cout << "Pose rMc: " << std::endl << rMc;
192 rMc.extract(R);
193 r.buildFrom(R);
194 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
195 << std::endl
196 << std::endl;
197
198 robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, p);
199 std::cout << "Robot pose in articular: " << p << std::endl;
200
201 robot.get_fMc(p, rMc);
202 std::cout << "Pose rMc from MGD: " << std::endl << rMc;
203 rMc.extract(R);
204 r.buildFrom(R);
205 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
206 << std::endl
207 << std::endl;
208
210 rMo = rMc * cMo;
211 std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
212 rMo.extract(R);
213 r.buildFrom(R);
214 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
215 << std::endl
216 << std::endl;
217 return EXIT_SUCCESS;
218 } catch (const vpException &e) {
219 std::cout << "Catch an exception: " << e << std::endl;
220 return EXIT_FAILURE;
221 }
222}
223#else
224int main()
225{
226 std::cout << "Sorry, test not valid. You should have an Viper850 robot..." << std::endl;
227 return EXIT_SUCCESS;
228}
229
230#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
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
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
This tracker is meant to track a dot (connected pixels with same gray level) on a vpImage.
Definition vpDot.h:112
void initTracking(const vpImage< unsigned char > &I)
Definition vpDot.cpp:617
void setGraphics(bool activate)
Definition vpDot.h:357
vpImagePoint getCog() const
Definition vpDot.h:243
error that can be emitted by ViSP classes.
Definition vpException.h:59
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
static double deg(double rad)
Definition vpMath.h:106
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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 set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
void setWorldCoordinates(double oX, double oY, double oZ)
Definition vpPoint.cpp:110
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
void addPoint(const vpPoint &P)
Definition vpPose.cpp:140
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as Euler angle minimal representation.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition vpViper850.h:134