Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRobotViper850-frames.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 Viper 650 robot.
33 *
34*****************************************************************************/
35
42#include <iostream>
43#include <visp3/robot/vpRobotViper850.h>
44
45#ifdef VISP_HAVE_VIPER850
46
47bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, double epsilon = 1e-6)
48{
50 M1.extract(t1);
51 M2.extract(t2);
52 vpThetaUVector tu1, tu2;
53 M1.extract(tu1);
54 M2.extract(tu2);
55
56 for (unsigned int i = 0; i < 3; i++) {
57 if (std::fabs(t1[i] - t2[i]) > epsilon)
58 return false;
59 if (std::fabs(tu1[i] - tu2[i]) > epsilon)
60 return false;
61 }
62 return true;
63}
64
65bool joint_equal(const vpColVector &q1, const vpColVector &q2, double epsilon = 1e-6)
66{
67 for (unsigned int i = 0; i < q1.size(); i++) {
68 if (std::fabs(q1[i] - q2[i]) > epsilon) {
69 return false;
70 }
71 }
72 return true;
73}
74
75int main()
76{
77 try {
78 //********* Define transformation from end effector to tool frame
80
81#if 0
82 // In this case, we set tool frame to the end of the two fingers pneumatic gripper
83 eMt[0][0] = 0;
84 eMt[1][0] = 0;
85 eMt[2][0] = -1;
86
87 eMt[0][1] = -sqrt(2)/2;
88 eMt[1][1] = -sqrt(2)/2;
89 eMt[2][1] = 0;
90
91 eMt[0][2] = -sqrt(2)/2;
92 eMt[1][2] = sqrt(2)/2;
93 eMt[2][2] = 0;
94
95 eMt[0][3] = -0.177;
96 eMt[1][3] = 0.177;
97 eMt[2][3] = 0.077;
98#else
99 // In this case, we set tool frame to the PTGrey Flea2 camera frame
100 vpTranslationVector etc(-0.04437278107, -0.001192883711, 0.07808296844);
101 vpRxyzVector erxyzc(vpMath::rad(0.7226737722), vpMath::rad(2.103893926), vpMath::rad(-90.46213439));
102 eMt.buildFrom(etc, vpRotationMatrix(erxyzc));
103#endif
104 std::cout << "eMt:\n" << eMt << std::endl;
105
106 //********* Init robot
107 std::cout << "Connection to Viper 850 robot" << std::endl;
108 vpRobotViper850 robot;
109 robot.init(vpViper850::TOOL_CUSTOM, eMt);
110
111 // Move robot to repos position
112 vpColVector repos(6); // q1, q4, q6 = 0
113 repos[1] = vpMath::rad(-90); // q2
114 repos[2] = vpMath::rad(180); // q3
115 repos[4] = vpMath::rad(90); // q5
116
117 robot.setPosition(vpRobot::ARTICULAR_FRAME, repos);
118
119 vpColVector q;
120 robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, q);
121
122 std::cout << "q: " << q.t() << std::endl;
123
124 vpHomogeneousMatrix fMw, fMe, fMt, cMe;
125 robot.get_fMw(q, fMw);
126 robot.get_fMe(q, fMe);
127 robot.get_fMc(q, fMt);
128 robot.get_cMe(cMe);
129
130 std::cout << "fMw:\n" << fMw << std::endl;
131 std::cout << "fMe:\n" << fMe << std::endl;
132 std::cout << "fMt:\n" << fMt << std::endl;
133 std::cout << "eMc:\n" << cMe.inverse() << std::endl;
134
135 //********* Check if retrieved eMt transformation is the one that was set
136 // during init
137 if (1) {
138 vpHomogeneousMatrix eMt_ = fMe.inverse() * fMt;
139 std::cout << "eMt_:\n" << eMt_ << std::endl;
140
141 // Compare pose
142 std::cout << "Compare pose eMt and eMt_:" << std::endl;
143 if (!pose_equal(eMt, eMt_, 1e-4)) {
144 std::cout << " Error: Pose eMt differ" << std::endl;
145 std::cout << "\nTest failed" << std::endl;
146 return EXIT_FAILURE;
147 }
148 std::cout << " They are the same, we can continue" << std::endl;
149
150 //********* Check if retrieved eMc transformation is the one that was
151 // set
152
153 std::cout << "eMc:\n" << cMe.inverse() << std::endl;
154 // Compare pose
155 std::cout << "Compare pose eMt and eMc:" << std::endl;
156 if (!pose_equal(eMt, cMe.inverse(), 1e-4)) {
157 std::cout << " Error: Pose eMc differ" << std::endl;
158 std::cout << "\nTest failed" << std::endl;
159 return EXIT_FAILURE;
160 }
161 std::cout << " They are the same, we can continue" << std::endl;
162 }
163
164 //********* Check if position in reference frame is equal to fMt
165 if (1) {
166 vpColVector f_pose_t; // translation vector + rxyz vector
167 robot.getPosition(vpRobot::REFERENCE_FRAME, f_pose_t);
168 // Compute homogeneous transformation
170 vpRxyzVector f_rxyz_t;
171 for (unsigned int i = 0; i < 3; i++) {
172 f_t_t[i] = f_pose_t[i];
173 f_rxyz_t[i] = f_pose_t[i + 3];
174 }
175 vpHomogeneousMatrix fMt_(f_t_t, vpRotationMatrix(f_rxyz_t));
176 std::cout << "fMt_ (from ref frame):\n" << fMt_ << std::endl;
177
178 std::cout << "Compare pose fMt and fMt_:" << std::endl;
179 if (!pose_equal(fMt, fMt_, 1e-4)) {
180 std::cout << " Error: Pose fMt differ" << std::endl;
181 std::cout << "\nTest failed" << std::endl;
182 return EXIT_FAILURE;
183 }
184 std::cout << " They are the same, we can continue" << std::endl;
185 }
186
187 //********* Test inverse kinematics
188 if (1) {
189 vpColVector q1;
190 robot.getInverseKinematics(fMt, q1);
191
192 std::cout << "Move robot in joint (the robot should not move)" << std::endl;
194 robot.setPosition(vpRobotViper850::ARTICULAR_FRAME, q1);
195
196 vpColVector q2;
197 robot.getPosition(vpRobot::ARTICULAR_FRAME, q2);
198 std::cout << "Reach joint position q2: " << q2.t() << std::endl;
199
200 std::cout << "Compare joint position q and q2:" << std::endl;
201 if (!joint_equal(q, q2, 1e-4)) {
202 std::cout << " Error: Joint position differ" << std::endl;
203 std::cout << "\nTest failed" << std::endl;
204 return EXIT_FAILURE;
205 }
206 std::cout << " They are the same, we can continue" << std::endl;
207 }
208
209 //********* Check if fMt position can be set in reference frame
210 if (1) {
211 vpColVector f_pose_t(6);
213 vpRxyzVector f_rxyz_t(fMt.getRotationMatrix());
214 for (unsigned int i = 0; i < 3; i++) {
215 f_pose_t[i] = f_t_t[i];
216 f_pose_t[i + 3] = f_rxyz_t[i];
217 }
218
219 std::cout << "Move robot in reference frame (the robot should not move)" << std::endl;
220 robot.setPosition(vpRobot::REFERENCE_FRAME, f_pose_t);
221 vpColVector q3;
222 robot.getPosition(vpRobot::ARTICULAR_FRAME, q3);
223 std::cout << "Reach joint position q3: " << q3.t() << std::endl;
224 std::cout << "Compare joint position q and q3:" << std::endl;
225 if (!joint_equal(q, q3, 1e-4)) {
226 std::cout << " Error: Joint position differ" << std::endl;
227 std::cout << "\nTest failed" << std::endl;
228 return EXIT_FAILURE;
229 }
230 std::cout << " They are the same, we can continue" << std::endl;
231 }
232
233 //********* Position control in tool frame
234 if (1) {
235 // from the current position move the tool frame
237 // tMt[0][3] = 0.05; // along x_t
238 tMt[1][3] = 0.05; // along y_t
239 // tMt[2][3] = 0.05; // along z_t
240
241 vpHomogeneousMatrix fMt_ = fMt * tMt; // New position to reach
242 robot.getInverseKinematics(fMt_, q);
243
244 std::cout << "fMt_:\n" << fMt_ << std::endl;
245
246 std::cout << "Move robot in joint position to reach fMt_" << std::endl;
248 robot.setPosition(vpRobotViper850::ARTICULAR_FRAME, q);
249
250 vpPoseVector fpt_;
251 robot.getPosition(vpRobot::REFERENCE_FRAME, fpt_);
252
253 std::cout << "fpt_:\n" << vpHomogeneousMatrix(fpt_) << std::endl;
254
255 std::cout << "Compare pose fMt_ and fpt_:" << std::endl;
256 if (!pose_equal(fMt_, vpHomogeneousMatrix(fpt_), 1e-4)) {
257 std::cout << " Error: Pose fMt_ differ" << std::endl;
258 std::cout << "\nTest failed" << std::endl;
259 return EXIT_FAILURE;
260 }
261 std::cout << " They are the same, we can continue" << std::endl;
262 }
263
264 //********* Velocity control in tool frame along z
265 if (0) {
266 double t_init = vpTime::measureTimeMs();
267 vpColVector v_t(6);
268 v_t = 0;
269 // v_t[2] = 0.01; // translation velocity along z_t
270 v_t[5] = vpMath::rad(5); // rotation velocity along z_t
271
272 std::cout << "Move robot in camera velocity" << std::endl;
274 while (vpTime::measureTimeMs() - t_init < 6000) {
275 // std::cout << "send vel: " << v_t() << std::endl;
277 }
278 }
279
280 //********* Velocity control in tool frame along z using joint velocity
281 if (0) {
282 // We need to stop the robot before changing velocity control from joint
283 // to cartesian
286 vpMatrix eJe;
287
288 double t_init = vpTime::measureTimeMs();
289 vpColVector v_t(6), q_dot;
290 v_t = 0;
291 // v_t[2] = -0.01; // translation velocity along z_t
292 v_t[5] = vpMath::rad(-5); // rotation velocity along z_t
293
294 std::cout << "Move robot in joint velocity" << std::endl;
296 while (vpTime::measureTimeMs() - t_init < 6000) {
297 robot.get_eJe(eJe);
298 vpMatrix tJt = tVe * eJe;
299 q_dot = tJt.pseudoInverse() * v_t;
300 // std::cout << "send vel: " << q_dot.t() << std::endl;
302 }
303 }
304
305 //********* Velocity control in tool frame along x
306 if (1) {
308 double t_init = vpTime::measureTimeMs();
309 vpColVector v_t(6);
310 v_t = 0;
311 v_t[3] = vpMath::rad(5); // rotation velocity along x_t
312
313 std::cout << "Move robot in camera velocity" << std::endl;
315 while (vpTime::measureTimeMs() - t_init < 6000) {
316 // std::cout << "send vel: " << v_t() << std::endl;
318 }
319 }
320
321 //********* Velocity control in tool frame along x using joint velocity
322 if (1) {
323 // We need to stop the robot before changing velocity control from joint
324 // to cartesian
327 vpMatrix eJe;
328
329 double t_init = vpTime::measureTimeMs();
330 vpColVector v_t(6), q_dot;
331 v_t = 0;
332 v_t[3] = vpMath::rad(-5); // rotation velocity along x_t
333
334 std::cout << "Move robot in joint velocity" << std::endl;
336 while (vpTime::measureTimeMs() - t_init < 6000) {
337 robot.get_eJe(eJe);
338 vpMatrix tJt = tVe * eJe;
339 q_dot = tJt.pseudoInverse() * v_t;
340 // std::cout << "send vel: " << q_dot.t() << std::endl;
342 }
343 }
344
345 //********* Position control in tool frame
346 if (1) {
348 // get current position
349 robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, q);
350
351 robot.get_fMc(q, fMt);
352
353 vpHomogeneousMatrix tMt; // initialized to identity
354 // tMt[0][3] = -0.05; // along x_t
355 tMt[1][3] = -0.05; // along y_t
356 // tMt[2][3] = -0.05; // along z_t
357
358 robot.getInverseKinematics(fMt * tMt, q);
359
360 std::cout << "Move robot in joint position" << std::endl;
362 robot.setPosition(vpRobotViper850::ARTICULAR_FRAME, q);
363 }
364 std::cout << "The end" << std::endl;
365 std::cout << "Test succeed" << std::endl;
366 return EXIT_SUCCESS;
367 } catch (const vpException &e) {
368 std::cout << "Test failed with exception: " << e.getMessage() << std::endl;
369 return EXIT_FAILURE;
370 }
371}
372
373#else
374int main()
375{
376 std::cout << "The real Viper850 robot controller is not available." << std::endl;
377 return EXIT_SUCCESS;
378}
379
380#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
const char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpHomogeneousMatrix get_cMe() const
Definition vpUnicycle.h:71
VISP_EXPORT double measureTimeMs()