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