Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpAfma4.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 * Interface for the Irisa's Afma4 robot.
33 *
34*****************************************************************************/
35
44#include <visp3/core/vpCameraParameters.h>
45#include <visp3/core/vpDebug.h>
46#include <visp3/core/vpRotationMatrix.h>
47#include <visp3/core/vpRxyzVector.h>
48#include <visp3/core/vpTranslationVector.h>
49#include <visp3/core/vpVelocityTwistMatrix.h>
50#include <visp3/robot/vpAfma4.h>
51#include <visp3/robot/vpRobotException.h>
52
53/* ----------------------------------------------------------------------- */
54/* --- STATIC ------------------------------------------------------------ */
55/* ---------------------------------------------------------------------- */
56const unsigned int vpAfma4::njoint = 4;
57
63vpAfma4::vpAfma4() : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
64{
65 // Set the default parameters in case of the config files are not available.
66
67 //
68 // Geometric model constant parameters
69 //
70 this->_a1 = 0.205; // distance along x2
71 this->_d3 = 0.403; // distance along z2
72 this->_d4 = 0.14; // distance along z3
73
74 // Maximal value of the joints
75 this->_joint_max[0] = 1.8; // rad
76 this->_joint_max[1] = 0.9; // meter
77 this->_joint_max[2] = 0.9; // rad
78 this->_joint_max[3] = 0.76; // rad
79 // Minimal value of the joints
80 this->_joint_min[0] = -1.5; // rad
81 this->_joint_min[1] = -0.9; // meter
82 this->_joint_min[2] = -3.5; // rad
83 this->_joint_min[3] = -0.76; // rad
84
85 // Camera extrinsic parameters: effector to camera frame
86 this->_etc[0] = 0.; // Translation
87 this->_etc[1] = 0.;
88 this->_etc[2] = 0.;
89 this->_erc[0] = 0.; // Rotation
90 this->_erc[1] = -M_PI / 2.;
91 this->_erc[2] = 0;
92
94 this->_eMc.buildFrom(_etc, eRc);
95
96 init();
97}
98
103void vpAfma4::init(void) { return; }
104
137{
139 fMc = get_fMc(q);
140
141 return fMc;
142}
143
174{
176 get_fMc(q, fMc);
177
178 return fMc;
179}
180
208{
209
210 // Compute the direct geometric model: fMe = transformation between
211 // fix and end effector frame.
213
214 get_fMe(q, fMe);
215
216 fMc = fMe * this->_eMc;
217
218 return;
219}
220
257{
258 double q1 = q[0]; // rot touret
259 double q2 = q[1]; // vertical translation
260 double q4 = q[2]; // pan
261 double q5 = q[3]; // tilt
262
263 double c1 = cos(q1);
264 double s1 = sin(q1);
265 double c4 = cos(q4);
266 double s4 = sin(q4);
267 double c5 = cos(q5);
268 double s5 = sin(q5);
269
270 /* Calcul du modele d'apres les angles. */
271 fMe[0][0] = c1 * s4 * c5 + s1 * c4 * c5;
272 fMe[0][1] = -c1 * s4 * s5 - s1 * c4 * s5;
273 fMe[0][2] = c1 * c4 - s1 * s4;
274 fMe[0][3] = c1 * this->_a1 - s1 * (this->_d3);
275
276 fMe[1][0] = s1 * s4 * c5 - c1 * c4 * c5;
277 fMe[1][1] = -s1 * s4 * s5 + c1 * c4 * s5;
278 fMe[1][2] = s1 * c4 + c1 * s4;
279 fMe[1][3] = s1 * this->_a1 + c1 * (this->_d3);
280
281 fMe[2][0] = -s5;
282 fMe[2][1] = -c5;
283 fMe[2][2] = 0.f;
284 fMe[2][3] = this->_d4 + q2;
285
286 fMe[3][0] = 0.f;
287 fMe[3][1] = 0.f;
288 fMe[3][2] = 0.f;
289 fMe[3][3] = 1;
290
291 // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
292
293 return;
294}
295
306void vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
307
318{
320 get_cMe(cMe);
321
322 cVe.buildFrom(cMe);
323
324 return;
325}
326
346{
347 vpHomogeneousMatrix fMc, cMf;
348 get_fMc(q, fMc);
349 cMf = fMc.inverse();
350
351 cVf.buildFrom(cMf);
352
353 return;
354}
355
392void vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
393{
394 double q4 = q[2]; // pan
395 double q5 = q[3]; // tilt
396
397 double c4 = cos(q4);
398 double s4 = sin(q4);
399 double c5 = cos(q5);
400 double s5 = sin(q5);
401
402 eJe.resize(6, 4);
403
404 eJe = 0;
405
406 eJe[0][0] = -(this->_a1 * c4 + this->_d3 * s4) * c5;
407 eJe[0][1] = -s5;
408 eJe[1][0] = (this->_a1 * c4 + this->_d3 * s4) * s5;
409 eJe[1][1] = -c5;
410 eJe[2][0] = (this->_a1 * s4 - this->_d3 * c4);
411 eJe[3][0] = eJe[3][2] = -s5;
412 eJe[4][0] = eJe[4][2] = -c5;
413 eJe[5][3] = 1.;
414}
415
447void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
448{
449 fJe.resize(6, 4);
450
451 double q1 = q[0]; // rot touret
452 double q4 = q[2]; // pan
453
454 double c1 = cos(q1);
455 double s1 = sin(q1);
456 double c14 = cos(q1 + q4);
457 double s14 = sin(q1 + q4);
458
459 fJe = 0;
460
461 fJe[0][0] = -s1 * this->_a1 - c1 * this->_d3;
462
463 fJe[1][0] = c1 * this->_a1 - s1 * this->_d3;
464
465 fJe[2][1] = 1.0;
466
467 fJe[3][3] = c14;
468
469 fJe[4][3] = s14;
470
471 fJe[5][0] = fJe[5][2] = 1.0;
472}
473
503void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
504{
505 fJe_inverse.resize(4, 6);
506 fJe_inverse = 0;
507
508 double q1 = q[0]; // rot touret
509 double q4 = q[2]; // pan
510
511 double c1 = cos(q1);
512 double s1 = sin(q1);
513 double c14 = cos(q1 + q4);
514 double s14 = sin(q1 + q4);
515
516 double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
517
518 fJe_inverse[0][0] = (-s1 * this->_a1 - c1 * this->_d3) / det;
519 fJe_inverse[0][1] = (c1 * this->_a1 - s1 * this->_d3) / det;
520
521 fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
522
523 fJe_inverse[2][0] = -fJe_inverse[0][0];
524 fJe_inverse[2][1] = -fJe_inverse[0][1];
525
526 fJe_inverse[3][3] = c14;
527 fJe_inverse[3][4] = s14;
528}
529
539{
540 vpColVector qmin(4);
541 for (unsigned int i = 0; i < 4; i++)
542 qmin[i] = this->_joint_min[i];
543 return qmin;
544}
545
555{
556 vpColVector qmax(4);
557 for (unsigned int i = 0; i < 4; i++)
558 qmax[i] = this->_joint_max[i];
559 return qmax;
560}
561
571VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
572{
574 afma4._eMc.extract(eRc);
575 vpRxyzVector rxyz(eRc);
576
577 os << "Joint Max:" << std::endl
578 << "\t" << afma4._joint_max[0] << "\t" << afma4._joint_max[1] << "\t" << afma4._joint_max[2] << "\t"
579 << afma4._joint_max[3] << "\t" << std::endl
580
581 << "Joint Min: " << std::endl
582 << "\t" << afma4._joint_min[0] << "\t" << afma4._joint_min[1] << "\t" << afma4._joint_min[2] << "\t"
583 << afma4._joint_min[3] << "\t" << std::endl
584
585 << "a1: " << std::endl
586 << "\t" << afma4._a1 << "\t" << std::endl
587
588 << "d3: " << std::endl
589 << "\t" << afma4._d3 << "\t" << std::endl
590
591 << "d4: " << std::endl
592 << "\t" << afma4._d4 << "\t" << std::endl
593
594 << "eMc: " << std::endl
595 << "\tTranslation (m): " << afma4._eMc[0][3] << " " << afma4._eMc[1][3] << " " << afma4._eMc[2][3] << "\t"
596 << std::endl
597 << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
598 << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
599 << "\t" << std::endl;
600
601 return os;
602}
Modelisation of Irisa's cylindrical robot named Afma4.
Definition vpAfma4.h:108
double _d3
Definition vpAfma4.h:142
vpColVector getJointMin() const
Definition vpAfma4.cpp:538
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition vpAfma4.cpp:447
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition vpAfma4.cpp:173
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition vpAfma4.cpp:306
vpTranslationVector _etc
Definition vpAfma4.h:148
static const unsigned int njoint
Number of joint.
Definition vpAfma4.h:137
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition vpAfma4.cpp:317
vpRxyzVector _erc
Definition vpAfma4.h:149
double _a1
Definition vpAfma4.h:141
void init(void)
Definition vpAfma4.cpp:103
double _d4
Definition vpAfma4.h:143
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition vpAfma4.cpp:392
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
Definition vpAfma4.cpp:503
void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
Definition vpAfma4.cpp:345
vpColVector getJointMax() const
Definition vpAfma4.cpp:554
vpHomogeneousMatrix _eMc
Definition vpAfma4.h:151
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition vpAfma4.cpp:256
double _joint_min[4]
Definition vpAfma4.h:145
double _joint_max[4]
Definition vpAfma4.h:144
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition vpAfma4.cpp:136
vpAfma4()
Definition vpAfma4.cpp:63
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)