Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRobotAfma6Pose.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/vpDebug.h>
50#include <visp3/core/vpImage.h>
51#include <visp3/core/vpPixelMeterConversion.h>
52#include <visp3/core/vpPoint.h>
53#include <visp3/gui/vpDisplayGTK.h>
54#include <visp3/gui/vpDisplayOpenCV.h>
55#include <visp3/gui/vpDisplayX.h>
56#include <visp3/robot/vpRobotAfma6.h>
57#include <visp3/sensor/vp1394TwoGrabber.h>
58#include <visp3/vision/vpPose.h>
59#if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394)
60
61int main()
62{
63 try {
64 // Create an image B&W container
66
67 // Create a firewire grabber based on libdc1394-2.x
69
70 // Grab an image from the firewire camera
71 g.acquire(I);
72
73// Create an image viewer for the image
74#ifdef VISP_HAVE_X11
75 vpDisplayX display(I, 100, 100, "Current image");
76#elif defined(HAVE_OPENCV_HIGHGUI)
77 vpDisplayOpenCV display(I, 100, 100, "Current image");
78#elif defined(VISP_HAVE_GTK)
79 vpDisplayGTK display(I, 100, 100, "Current image");
80#endif
81
82 // Display the image
85
86 // Define a squared target
87 // The target is made of 4 planar points (square dim = 0.077m)
88 double sdim = 0.077; // square width and height
89 vpPoint target[4];
90 // Set the point world coordinates (x,y,z) in the object frame
91 // o ----> x
92 // |
93 // |
94 // \/
95 // y
96 target[0].setWorldCoordinates(-sdim / 2., -sdim / 2., 0);
97 target[1].setWorldCoordinates(sdim / 2., -sdim / 2., 0);
98 target[2].setWorldCoordinates(sdim / 2., sdim / 2., 0);
99 target[3].setWorldCoordinates(-sdim / 2., sdim / 2., 0);
100
101 // Image processing to extract the 2D coordinates in sub-pixels of the 4
102 // points from the image acquired by the camera
103 // Creation of 4 trackers
104 vpDot dot[4];
105 vpImagePoint cog;
106 for (int i = 0; i < 4; i++) {
107 dot[i].setGraphics(true); // to display the tracking results
108 std::cout << "Click on dot " << i << std::endl;
109 dot[i].initTracking(I);
110 // The tracker computes the sub-pixels coordinates in the image
111 // i ----> u
112 // |
113 // |
114 // \/
115 // v
116 std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
117 // Flush the tracking results in the viewer
119 }
120
121 // Create an intrinsic camera parameters structure
123
124 // Create a robot access
125 vpRobotAfma6 robot;
126
127 // Load the end-effector to camera frame transformation obtained
128 // using a camera intrinsic model with distortion
130
131 // Get the intrinsic camera parameters associated to the image
132 robot.getCameraParameters(cam, I);
133
134 // Using the camera parameters, compute the perspective projection
135 // (transform the dot sub-pixel coordinates into coordinates in the camera
136 // frame in meter)
137 for (int i = 0; i < 4; i++) {
138 double x = 0, y = 0; // coordinates of the dots in the camera frame
139 // c ----> x
140 // |
141 // |
142 // \/
143 // y
144 // pixel to meter conversion
145 cog = dot[i].getCog();
147 target[i].set_x(x);
148 target[i].set_y(y);
149 }
150
151 // From now, in target[i], we have the 3D coordinates of a point in the
152 // object frame, and their correspondances in the camera frame. We can now
153 // compute the pose cMo between the camera and the object.
154 vpPose pose;
155 // Add the 4 points to compute the pose
156 for (int i = 0; i < 4; i++) {
157 pose.addPoint(target[i]);
158 }
159 // Create an homogeneous matrix for the camera to object transformation
160 // computed just bellow
163 vpRxyzVector r;
164 // Compute the pose: initialisation is done by Dementhon or Lagrange method, and the
165 // final pose is computed by the more accurate Virtual Visual Servoing method.
167
168 std::cout << "Pose cMo: " << std::endl << cMo;
169 cMo.extract(R);
170 r.buildFrom(R);
171 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
172 << std::endl
173 << std::endl;
174
175 // Get the robot position in the reference frame
177 vpColVector p; // position x,y,z,rx,ry,rz
178 robot.getPosition(vpRobotAfma6::REFERENCE_FRAME, p);
179 std::cout << "Robot pose in reference frame: " << p << std::endl;
181 t[0] = p[0];
182 t[1] = p[1];
183 t[2] = p[2];
184 r[0] = p[3];
185 r[1] = p[4];
186 r[2] = p[5];
187 R.buildFrom(r);
188 rMc.buildFrom(t, R);
189 std::cout << "Pose rMc: " << std::endl << rMc;
190 rMc.extract(R);
191 r.buildFrom(R);
192 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
193 << std::endl
194 << std::endl;
195
196 robot.getPosition(vpRobotAfma6::ARTICULAR_FRAME, p);
197 std::cout << "Robot pose in articular: " << p << std::endl;
198
199 robot.get_fMc(p, rMc);
200 std::cout << "Pose rMc from MGD: " << std::endl << rMc;
201 rMc.extract(R);
202 r.buildFrom(R);
203 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
204 << std::endl
205 << std::endl;
206
208 rMo = rMc * cMo;
209 std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
210 rMo.extract(R);
211 r.buildFrom(R);
212 std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
213 << std::endl
214 << std::endl;
215 return EXIT_SUCCESS;
216 } catch (const vpException &e) {
217 std::cout << "Catch an exception: " << e << std::endl;
218 return EXIT_FAILURE;
219 }
220}
221#else
222int main()
223{
224 std::cout << "Sorry, test not valid. You should have an Afma6 robot..." << std::endl;
225 return EXIT_SUCCESS;
226}
227
228#endif
Class for firewire ieee1394 video devices using libdc1394-2.x api.
void acquire(vpImage< unsigned char > &I)
@ TOOL_CCMOP
Definition vpAfma6.h:124
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
Control of Irisa's gantry robot named Afma6.
@ 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.