Visual Servoing Platform  version 3.6.1 under development (2024-12-10)
vpAfma6.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 Afma6 robot.
33  *
34 *****************************************************************************/
35 
44 #include <iostream>
45 #include <sstream>
46 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpRotationMatrix.h>
50 #include <visp3/core/vpRxyzVector.h>
51 #include <visp3/core/vpTranslationVector.h>
52 #include <visp3/core/vpVelocityTwistMatrix.h>
53 #include <visp3/core/vpXmlParserCamera.h>
54 #include <visp3/robot/vpAfma6.h>
55 #include <visp3/robot/vpRobotException.h>
56 
57 /* ----------------------------------------------------------------------- */
58 /* --- STATIC ------------------------------------------------------------ */
59 /* ---------------------------------------------------------------------- */
60 
61 BEGIN_VISP_NAMESPACE
62 static const char *opt_Afma6[] = { "JOINT_MAX", "JOINT_MIN", "LONG_56", "COUPL_56",
63  "CAMERA", "eMc_ROT_XYZ", "eMc_TRANS_XYZ", nullptr };
64 
65 #ifdef VISP_HAVE_AFMA6_DATA
66 const std::string vpAfma6::CONST_AFMA6_FILENAME =
67 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_Afma6.cnf");
68 
70 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_without_distortion_Afma6.cnf");
71 
73 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_ccmop_with_distortion_Afma6.cnf");
74 
76 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_without_distortion_Afma6.cnf");
77 
79 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_gripper_with_distortion_Afma6.cnf");
80 
82 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_without_distortion_Afma6.cnf");
83 
85 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_vacuum_with_distortion_Afma6.cnf");
86 
88 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_without_distortion_Afma6.cnf");
89 
91 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_generic_with_distortion_Afma6.cnf");
92 
94 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_without_distortion_Afma6.cnf");
95 
97 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_eMc_Intel_D435_with_distortion_Afma6.cnf");
98 
99 const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
100 std::string(VISP_AFMA6_DATA_PATH) + std::string("/include/const_camera_Afma6.xml");
101 
102 #endif // VISP_HAVE_AFMA6_DATA
103 
104 const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
105 const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
106 const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
107 const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
108 const char *const vpAfma6::CONST_INTEL_D435_CAMERA_NAME = "Intel-D435";
109 
111 
112 const unsigned int vpAfma6::njoint = 6;
113 
120  : _coupl_56(0), _long_56(0), _etc(), _erc(), _eMc(), tool_current(vpAfma6::defaultTool),
121  projModel(vpCameraParameters::perspectiveProjWithoutDistortion)
122 {
123  // Set the default parameters in case of the config files are not available.
124 
125  //
126  // Geometric model constant parameters
127  //
128  // coupling between join 5 and 6
129  this->_coupl_56 = 0.009091;
130  // distance between join 5 and 6
131  this->_long_56 = -0.06924;
132  // Camera extrinsic parameters: effector to camera frame
133  this->_eMc.eye(); // Default values are initialized ...
134  // ... in init (vpAfma6::vpAfma6ToolType tool,
135  // vpCameraParameters::vpCameraParametersProjType projModel)
136  // Maximal value of the joints
137  this->_joint_min[0] = -0.7501;
138  this->_joint_min[1] = -0.6501;
139  this->_joint_min[2] = -0.5001;
140  this->_joint_min[3] = -2.7301;
141  this->_joint_min[4] = -0.3001;
142  this->_joint_min[5] = -1.5901;
143  //_joint_max.resize(njoint);
144  this->_joint_max[0] = 0.6001;
145  this->_joint_max[1] = 0.6701;
146  this->_joint_max[2] = 0.4601;
147  this->_joint_max[3] = 2.7301;
148  this->_joint_max[4] = 2.4801;
149  this->_joint_max[5] = 1.5901;
150  init();
151 }
152 
157 void vpAfma6::init(void)
158 {
159  this->init(vpAfma6::defaultTool);
160  return;
161 }
162 
175 void vpAfma6::init(const std::string &camera_extrinsic_parameters, const std::string &camera_intrinsic_parameters)
176 {
177  this->parseConfigFile(camera_extrinsic_parameters);
178 
179  this->parseConfigFile(camera_intrinsic_parameters);
180 }
181 
212 void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
213 {
214  this->setToolType(tool);
215  this->parseConfigFile(filename.c_str());
216 }
217 
227 void vpAfma6::init(const std::string &camera_extrinsic_parameters)
228 {
229  this->parseConfigFile(camera_extrinsic_parameters);
230 }
231 
248 {
249  this->setToolType(tool);
250  this->set_eMc(eMc);
251 }
252 
271 {
272 
273  this->projModel = proj_model;
274 
275 #ifdef VISP_HAVE_AFMA6_DATA
276  // Read the robot parameters from files
277  std::string filename_eMc;
278  switch (tool) {
279  case vpAfma6::TOOL_CCMOP: {
280  switch (projModel) {
283  break;
286  break;
289  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
290  break;
291  }
292  break;
293  }
294  case vpAfma6::TOOL_GRIPPER: {
295  switch (projModel) {
298  break;
301  break;
304  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
305  break;
306  }
307  break;
308  }
309  case vpAfma6::TOOL_VACUUM: {
310  switch (projModel) {
313  break;
316  break;
319  "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
320  break;
321  }
322  break;
323  }
325  switch (projModel) {
328  break;
331  break;
334  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
335  break;
336  }
337  break;
338  }
340  switch (projModel) {
343  break;
346  break;
349  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
350  break;
351  }
352  break;
353  }
354  default: {
355  vpERROR_TRACE("This error should not occur!");
356  break;
357  }
358  }
359 
360  this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
361 
362 #else // VISP_HAVE_AFMA6_DATA
363 
364  // Use here default values of the robot constant parameters.
365  switch (tool) {
366  case vpAfma6::TOOL_CCMOP: {
367  switch (projModel) {
369  _erc[0] = vpMath::rad(164.35); // rx
370  _erc[1] = vpMath::rad(89.64); // ry
371  _erc[2] = vpMath::rad(-73.05); // rz
372  _etc[0] = 0.0117; // tx
373  _etc[1] = 0.0033; // ty
374  _etc[2] = 0.2272; // tz
375  break;
377  _erc[0] = vpMath::rad(33.54); // rx
378  _erc[1] = vpMath::rad(89.34); // ry
379  _erc[2] = vpMath::rad(57.83); // rz
380  _etc[0] = 0.0373; // tx
381  _etc[1] = 0.0024; // ty
382  _etc[2] = 0.2286; // tz
383  break;
386  "Feature TOOL_CCMOP is not implemented for Kannala-Brandt projection model yet.");
387  break;
388  }
389  break;
390  }
391  case vpAfma6::TOOL_GRIPPER: {
392  switch (projModel) {
394  _erc[0] = vpMath::rad(88.33); // rx
395  _erc[1] = vpMath::rad(72.07); // ry
396  _erc[2] = vpMath::rad(2.53); // rz
397  _etc[0] = 0.0783; // tx
398  _etc[1] = 0.1234; // ty
399  _etc[2] = 0.1638; // tz
400  break;
402  _erc[0] = vpMath::rad(86.69); // rx
403  _erc[1] = vpMath::rad(71.93); // ry
404  _erc[2] = vpMath::rad(4.17); // rz
405  _etc[0] = 0.1034; // tx
406  _etc[1] = 0.1142; // ty
407  _etc[2] = 0.1642; // tz
408  break;
411  "Feature TOOL_GRIPPER is not implemented for Kannala-Brandt projection model yet.");
412  break;
413  }
414  break;
415  }
416  case vpAfma6::TOOL_VACUUM: {
417  switch (projModel) {
419  _erc[0] = vpMath::rad(90.40); // rx
420  _erc[1] = vpMath::rad(75.11); // ry
421  _erc[2] = vpMath::rad(0.18); // rz
422  _etc[0] = 0.0038; // tx
423  _etc[1] = 0.1281; // ty
424  _etc[2] = 0.1658; // tz
425  break;
427  _erc[0] = vpMath::rad(91.61); // rx
428  _erc[1] = vpMath::rad(76.17); // ry
429  _erc[2] = vpMath::rad(-0.98); // rz
430  _etc[0] = 0.0815; // tx
431  _etc[1] = 0.1162; // ty
432  _etc[2] = 0.1658; // tz
433  break;
436  "Feature TOOL_VACUUM is not implemented for Kannala-Brandt projection model yet.");
437  break;
438  }
439  break;
440  }
442  switch (projModel) {
444  _erc[0] = vpMath::rad(-71.41); // rx
445  _erc[1] = vpMath::rad(89.49); // ry
446  _erc[2] = vpMath::rad(162.07); // rz
447  _etc[0] = 0.0038; // tx
448  _etc[1] = 0.1281; // ty
449  _etc[2] = 0.1658; // tz
450  break;
452  _erc[0] = vpMath::rad(-52.79); // rx
453  _erc[1] = vpMath::rad(89.55); // ry
454  _erc[2] = vpMath::rad(143.34); // rz
455  _etc[0] = 0.0693; // tx
456  _etc[1] = -0.0297; // ty
457  _etc[2] = 0.1357; // tz
458  break;
461  "Feature TOOL_INTEL_D435_CAMERA is not implemented for Kannala-Brandt projection model yet.");
462  break;
463  }
464  break;
465  }
468  switch (projModel) {
471  // set eMc to identity
472  _erc[0] = 0; // rx
473  _erc[1] = 0; // ry
474  _erc[2] = 0; // rz
475  _etc[0] = 0; // tx
476  _etc[1] = 0; // ty
477  _etc[2] = 0; // tz
478  break;
481  "Feature TOOL_GENERIC_CAMERA is not implemented for Kannala-Brandt projection model yet.");
482  break;
483  }
484  break;
485  }
486  }
487  vpRotationMatrix eRc(_erc);
488  this->_eMc.buildFrom(_etc, eRc);
489 #endif // VISP_HAVE_AFMA6_DATA
490 
491  setToolType(tool);
492  return;
493 }
494 
520 {
522  fMc = get_fMc(q);
523 
524  return fMc;
525 }
526 
603 int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
604  const bool &verbose) const
605 {
607  double q_[2][6], d[2], t;
608  int ok[2];
609  double cord[6];
610 
611  int nbsol = 0;
612 
613  if (q.getRows() != njoint)
614  q.resize(6);
615 
616  // for(unsigned int i=0;i<3;i++) {
617  // fMe[i][3] = fMc[i][3];
618  // for(int j=0;j<3;j++) {
619  // fMe[i][j] = 0.0;
620  // for (int k=0;k<3;k++) fMe[i][j] += fMc[i][k]*rpi[j][k];
621  // fMe[i][3] -= fMe[i][j]*rpi[j][3];
622  // }
623  // }
624 
625  // std::cout << "\n\nfMc: " << fMc;
626  // std::cout << "\n\neMc: " << _eMc;
627 
628  fMe = fMc * this->_eMc.inverse();
629  // std::cout << "\n\nfMe: " << fMe;
630 
631  if (fMe[2][2] >= .99999f) {
632  vpTRACE("singularity\n");
633  q_[0][4] = q_[1][4] = M_PI / 2.f;
634  t = atan2(fMe[0][0], fMe[0][1]);
635  q_[1][3] = q_[0][3] = q[3];
636  q_[1][5] = q_[0][5] = t - q_[0][3];
637 
638  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
639  /* -> a cause du couplage 4/5 */
640  {
641  q_[1][5] -= vpMath::rad(10);
642  q_[1][3] += vpMath::rad(10);
643  }
644  while (q_[1][5] <= this->_joint_min[5]) {
645  q_[1][5] += vpMath::rad(10);
646  q_[1][3] -= vpMath::rad(10);
647  }
648  }
649  else if (fMe[2][2] <= -.99999) {
650  vpTRACE("singularity\n");
651  q_[0][4] = q_[1][4] = -M_PI / 2;
652  t = atan2(fMe[1][1], fMe[1][0]);
653  q_[1][3] = q_[0][3] = q[3];
654  q_[1][5] = q_[0][5] = q_[0][3] - t;
655  while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
656  /* -> a cause du couplage 4/5 */
657  {
658  q_[1][5] -= vpMath::rad(10);
659  q_[1][3] -= vpMath::rad(10);
660  }
661  while (q_[1][5] <= this->_joint_min[5]) {
662  q_[1][5] += vpMath::rad(10);
663  q_[1][3] += vpMath::rad(10);
664  }
665  }
666  else {
667  q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
668  if (q_[0][3] >= 0.0)
669  q_[1][3] = q_[0][3] - M_PI;
670  else
671  q_[1][3] = q_[0][3] + M_PI;
672 
673  q_[0][4] = asin(fMe[2][2]);
674  if (q_[0][4] >= 0.0)
675  q_[1][4] = M_PI - q_[0][4];
676  else
677  q_[1][4] = -M_PI - q_[0][4];
678 
679  q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
680  if (q_[0][5] >= 0.0)
681  q_[1][5] = q_[0][5] - M_PI;
682  else
683  q_[1][5] = q_[0][5] + M_PI;
684  }
685  q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
686  q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
687  q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
688  q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
689  q_[0][2] = q_[1][2] = fMe[2][3];
690 
691  /* prise en compte du couplage axes 5/6 */
692  q_[0][5] += this->_coupl_56 * q_[0][4];
693  q_[1][5] += this->_coupl_56 * q_[1][4];
694 
695  for (int j = 0; j < 2; j++) {
696  ok[j] = 1;
697  // test is position is reachable
698  for (unsigned int i = 0; i < 6; i++) {
699  if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
700  if (verbose) {
701  if (i < 3)
702  std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < "
703  << this->_joint_max[i] << std::endl;
704  else
705  std::cout << "Joint " << i << " not in limits: " << vpMath::deg(this->_joint_min[i]) << " < "
706  << vpMath::deg(q_[j][i]) << " < " << vpMath::deg(this->_joint_max[i]) << std::endl;
707  }
708  ok[j] = 0;
709  }
710  }
711  }
712  if (ok[0] == 0) {
713  if (ok[1] == 0) {
714  std::cout << "No solution..." << std::endl;
715  nbsol = 0;
716  return nbsol;
717  }
718  else if (ok[1] == 1) {
719  for (unsigned int i = 0; i < 6; i++)
720  cord[i] = q_[1][i];
721  nbsol = 1;
722  }
723  }
724  else {
725  if (ok[1] == 0) {
726  for (unsigned int i = 0; i < 6; i++)
727  cord[i] = q_[0][i];
728  nbsol = 1;
729  }
730  else {
731  nbsol = 2;
732  // vpTRACE("2 solutions\n");
733  for (int j = 0; j < 2; j++) {
734  d[j] = 0.0;
735  for (unsigned int i = 3; i < 6; i++)
736  d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
737  }
738  if (nearest == true) {
739  if (d[0] <= d[1])
740  for (unsigned int i = 0; i < 6; i++)
741  cord[i] = q_[0][i];
742  else
743  for (unsigned int i = 0; i < 6; i++)
744  cord[i] = q_[1][i];
745  }
746  else {
747  if (d[0] <= d[1])
748  for (unsigned int i = 0; i < 6; i++)
749  cord[i] = q_[1][i];
750  else
751  for (unsigned int i = 0; i < 6; i++)
752  cord[i] = q_[0][i];
753  }
754  }
755  }
756  for (unsigned int i = 0; i < 6; i++)
757  q[i] = cord[i];
758 
759  return nbsol;
760 }
761 
785 {
787  get_fMc(q, fMc);
788 
789  return fMc;
790 }
791 
812 {
813 
814  // Compute the direct geometric model: fMe = transformation between
815  // fix and end effector frame.
817 
818  get_fMe(q, fMe);
819 
820  fMc = fMe * this->_eMc;
821 
822  return;
823 }
824 
845 {
846  double q0 = q[0]; // meter
847  double q1 = q[1]; // meter
848  double q2 = q[2]; // meter
849 
850  /* Decouplage liaisons 2 et 3. */
851  double q5 = q[5] - this->_coupl_56 * q[4];
852 
853  double c1 = cos(q[3]);
854  double s1 = sin(q[3]);
855  double c2 = cos(q[4]);
856  double s2 = sin(q[4]);
857  double c3 = cos(q5);
858  double s3 = sin(q5);
859 
860  // Compute the direct geometric model: fMe = transformation betwee
861  // fix and end effector frame.
862  fMe[0][0] = s1 * s2 * c3 + c1 * s3;
863  fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
864  fMe[0][2] = -s1 * c2;
865  fMe[0][3] = q0 + this->_long_56 * c1;
866 
867  fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
868  fMe[1][1] = c1 * s2 * s3 + s1 * c3;
869  fMe[1][2] = c1 * c2;
870  fMe[1][3] = q1 + this->_long_56 * s1;
871 
872  fMe[2][0] = c2 * c3;
873  fMe[2][1] = -c2 * s3;
874  fMe[2][2] = s2;
875  fMe[2][3] = q2;
876 
877  fMe[3][0] = 0;
878  fMe[3][1] = 0;
879  fMe[3][2] = 0;
880  fMe[3][3] = 1;
881 
882  // vpCTRACE << "Effector position fMe: " << std::endl << fMe;
883 
884  return;
885 }
886 
897 void vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = this->_eMc.inverse(); }
908 vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
909 
920 {
922  get_cMe(cMe);
923 
924  cVe.buildFrom(cMe);
925 
926  return;
927 }
928 
941 void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
942 {
943 
944  eJe.resize(6, 6);
945 
946  double s4, c4, s5, c5, s6, c6;
947 
948  s4 = sin(q[3]);
949  c4 = cos(q[3]);
950  s5 = sin(q[4]);
951  c5 = cos(q[4]);
952  s6 = sin(q[5] - this->_coupl_56 * q[4]);
953  c6 = cos(q[5] - this->_coupl_56 * q[4]);
954 
955  eJe = 0;
956  eJe[0][0] = s4 * s5 * c6 + c4 * s6;
957  eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
958  eJe[0][2] = c5 * c6;
959  eJe[0][3] = -this->_long_56 * s5 * c6;
960 
961  eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
962  eJe[1][1] = c4 * s5 * s6 + s4 * c6;
963  eJe[1][2] = -c5 * s6;
964  eJe[1][3] = this->_long_56 * s5 * s6;
965 
966  eJe[2][0] = -s4 * c5;
967  eJe[2][1] = c4 * c5;
968  eJe[2][2] = s5;
969  eJe[2][3] = this->_long_56 * c5;
970 
971  eJe[3][3] = c5 * c6;
972  eJe[3][4] = s6;
973 
974  eJe[4][3] = -c5 * s6;
975  eJe[4][4] = c6;
976 
977  eJe[5][3] = s5;
978  eJe[5][4] = -this->_coupl_56;
979  eJe[5][5] = 1;
980 
981  return;
982 }
983 
1011 void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
1012 {
1013 
1014  fJe.resize(6, 6);
1015 
1016  // block superieur gauche
1017  fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
1018 
1019  double s4 = sin(q[3]);
1020  double c4 = cos(q[3]);
1021 
1022  // block superieur droit
1023  fJe[0][3] = -this->_long_56 * s4;
1024  fJe[1][3] = this->_long_56 * c4;
1025 
1026  double s5 = sin(q[4]);
1027  double c5 = cos(q[4]);
1028  // block inferieur droit
1029  fJe[3][4] = c4;
1030  fJe[3][5] = -s4 * c5;
1031  fJe[4][4] = s4;
1032  fJe[4][5] = c4 * c5;
1033  fJe[5][3] = 1;
1034  fJe[5][5] = s5;
1035 
1036  // coupling between joint 5 and 6
1037  fJe[3][4] += this->_coupl_56 * s4 * c5;
1038  fJe[4][4] += -this->_coupl_56 * c4 * c5;
1039  fJe[5][4] += -this->_coupl_56 * s5;
1040 
1041  return;
1042 }
1043 
1053 {
1054  vpColVector qmin(6);
1055  for (unsigned int i = 0; i < 6; i++)
1056  qmin[i] = this->_joint_min[i];
1057  return qmin;
1058 }
1059 
1069 {
1070  vpColVector qmax(6);
1071  for (unsigned int i = 0; i < 6; i++)
1072  qmax[i] = this->_joint_max[i];
1073  return qmax;
1074 }
1075 
1082 double vpAfma6::getCoupl56() const { return _coupl_56; }
1083 
1090 double vpAfma6::getLong56() const { return _long_56; }
1091 
1102 void vpAfma6::parseConfigFile(const std::string &filename)
1103 {
1104  vpRxyzVector erc; // eMc rotation
1105  vpTranslationVector etc; // eMc translation
1106 
1107  std::ifstream fdconfig(filename.c_str(), std::ios::in);
1108 
1109  if (!fdconfig.is_open()) {
1110  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
1111  filename.c_str());
1112  }
1113 
1114  std::string line;
1115  int lineNum = 0;
1116  bool get_erc = false;
1117  bool get_etc = false;
1118  int code;
1119 
1120  while (std::getline(fdconfig, line)) {
1121  lineNum++;
1122  if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
1123  continue;
1124  }
1125  std::istringstream ss(line);
1126  std::string key;
1127  ss >> key;
1128 
1129  for (code = 0; nullptr != opt_Afma6[code]; ++code) {
1130  if (key.compare(opt_Afma6[code]) == 0) {
1131  break;
1132  }
1133  }
1134 
1135  switch (code) {
1136  case 0:
1137  ss >> this->_joint_max[0] >> this->_joint_max[1] >> this->_joint_max[2] >> this->_joint_max[3] >>
1138  this->_joint_max[4] >> this->_joint_max[5];
1139  break;
1140 
1141  case 1:
1142  ss >> this->_joint_min[0] >> this->_joint_min[1] >> this->_joint_min[2] >> this->_joint_min[3] >>
1143  this->_joint_min[4] >> this->_joint_min[5];
1144  break;
1145 
1146  case 2:
1147  ss >> this->_long_56;
1148  break;
1149 
1150  case 3:
1151  ss >> this->_coupl_56;
1152  break;
1153 
1154  case 4:
1155  break; // Nothing to do: camera name
1156 
1157  case 5:
1158  ss >> erc[0] >> erc[1] >> erc[2];
1159 
1160  // Convert rotation from degrees to radians
1161  erc = erc * M_PI / 180.0;
1162  get_erc = true;
1163  break;
1164 
1165  case 6:
1166  ss >> etc[0] >> etc[1] >> etc[2];
1167  get_etc = true;
1168  break;
1169 
1170  default:
1171  throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
1172  filename.c_str(), lineNum));
1173  }
1174  }
1175 
1176  fdconfig.close();
1177 
1178  // Compute the eMc matrix from the translations and rotations
1179  if (get_etc && get_erc) {
1180  _erc = erc;
1181  _etc = etc;
1182 
1183  vpRotationMatrix eRc(_erc);
1184  this->_eMc.buildFrom(_etc, eRc);
1185  }
1186 }
1187 
1197 {
1198  this->_eMc = eMc;
1199  this->_etc = eMc.getTranslationVector();
1200 
1201  vpRotationMatrix R(eMc);
1202  this->_erc.buildFrom(R);
1203 }
1204 
1269 void vpAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
1270  const unsigned int &image_height) const
1271 {
1272 #if defined(VISP_HAVE_AFMA6_DATA) && defined(VISP_HAVE_PUGIXML)
1273  vpXmlParserCamera parser;
1274  switch (getToolType()) {
1275  case vpAfma6::TOOL_CCMOP: {
1276  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl
1277  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1279  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1280  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1281  }
1282  break;
1283  }
1284  case vpAfma6::TOOL_GRIPPER: {
1285  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl
1286  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1288  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1289  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1290  }
1291  break;
1292  }
1293  case vpAfma6::TOOL_VACUUM: {
1294  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl
1295  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1297  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1298  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1299  }
1300  break;
1301  }
1303  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\"" << std::endl
1304  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1306  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1307  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1308  }
1309  break;
1310  }
1312  std::cout << "Get camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl
1313  << "from the XML file: \"" << vpAfma6::CONST_CAMERA_AFMA6_FILENAME << "\"" << std::endl;
1315  image_width, image_height) != vpXmlParserCamera::SEQUENCE_OK) {
1316  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1317  }
1318  break;
1319  }
1320  default: {
1321  vpERROR_TRACE("This error should not occur!");
1322  // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
1323  // "que les specs de la classe ont ete modifiee, "
1324  // "et que le code n'a pas ete mis a jour "
1325  // "correctement.");
1326  // vpERROR_TRACE ("Verifiez les valeurs possibles du type "
1327  // "vpAfma6::vpAfma6ToolType, et controlez que "
1328  // "tous les cas ont ete pris en compte dans la "
1329  // "fonction init(camera).");
1330  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1331  }
1332  }
1333 #else
1334  // Set default parameters
1335  switch (getToolType()) {
1336  case vpAfma6::TOOL_CCMOP: {
1337  // Set default intrinsic camera parameters for 640x480 images
1338  if (image_width == 640 && image_height == 480) {
1339  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
1340  << std::endl;
1341  switch (this->projModel) {
1343  cam.initPersProjWithoutDistortion(1108.0, 1110.0, 314.5, 243.2);
1344  break;
1346  cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
1347  break;
1350  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1351  break;
1352  }
1353  }
1354  else {
1355  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1356  "resolution");
1357  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1358  }
1359  break;
1360  }
1361  case vpAfma6::TOOL_GRIPPER: {
1362  // Set default intrinsic camera parameters for 640x480 images
1363  if (image_width == 640 && image_height == 480) {
1364  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
1365  << std::endl;
1366  switch (this->projModel) {
1368  cam.initPersProjWithoutDistortion(850.9, 853.0, 311.1, 243.6);
1369  break;
1371  cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
1372  break;
1375  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1376  break;
1377  }
1378  }
1379  else {
1380  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1381  "resolution");
1382  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1383  }
1384  break;
1385  }
1386  case vpAfma6::TOOL_VACUUM: {
1387  // Set default intrinsic camera parameters for 640x480 images
1388  if (image_width == 640 && image_height == 480) {
1389  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\""
1390  << std::endl;
1391  switch (this->projModel) {
1393  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1394  break;
1396  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1397  break;
1400  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1401  break;
1402  }
1403  }
1404  else {
1405  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1406  "resolution");
1407  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1408  }
1409  break;
1410  }
1412  // Set default intrinsic camera parameters for 640x480 images
1413  if (image_width == 640 && image_height == 480) {
1414  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_INTEL_D435_CAMERA_NAME << "\""
1415  << std::endl;
1416  switch (this->projModel) {
1418  cam.initPersProjWithoutDistortion(605.4, 605.6, 328.6, 241.0);
1419  break;
1421  cam.initPersProjWithDistortion(611.8, 612.6, 327.8, 241.7, 0.0436, -0.04265);
1422  break;
1425  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1426  break;
1427  }
1428  }
1429  else {
1430  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1431  "resolution");
1432  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1433  }
1434  break;
1435  }
1437  // Set default intrinsic camera parameters for 640x480 images
1438  if (image_width == 640 && image_height == 480) {
1439  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\""
1440  << std::endl;
1441  switch (this->projModel) {
1443  cam.initPersProjWithoutDistortion(853.5, 856.0, 307.8, 236.8);
1444  break;
1446  cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
1447  break;
1450  "Feature getCameraParameters is not implemented for Kannala-Brandt projection model yet.");
1451  break;
1452  }
1453  }
1454  else {
1455  vpTRACE("Cannot get default intrinsic camera parameters for this image "
1456  "resolution");
1457  throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
1458  }
1459  break;
1460  }
1461  default:
1462  vpERROR_TRACE("This error should not occur!");
1463  break;
1464  }
1465 #endif
1466  return;
1467 }
1468 
1516 {
1517  getCameraParameters(cam, I.getWidth(), I.getHeight());
1518 }
1567 {
1568  getCameraParameters(cam, I.getWidth(), I.getHeight());
1569 }
1570 
1580 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
1581 {
1582  vpRotationMatrix eRc;
1583  afma6._eMc.extract(eRc);
1584  vpRxyzVector rxyz(eRc);
1585 
1586  os << "Joint Max:" << std::endl
1587  << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
1588  << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
1589 
1590  << "Joint Min: " << std::endl
1591  << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
1592  << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
1593 
1594  << "Long 5-6: " << std::endl
1595  << "\t" << afma6._long_56 << "\t" << std::endl
1596 
1597  << "Coupling 5-6:" << std::endl
1598  << "\t" << afma6._coupl_56 << "\t" << std::endl
1599 
1600  << "eMc: " << std::endl
1601  << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
1602  << std::endl
1603  << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
1604  << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
1605  << "\t" << std::endl;
1606 
1607  return os;
1608 }
1609 END_VISP_NAMESPACE
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:78
static const std::string CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:93
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:101
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:86
virtual void set_eMc(const vpHomogeneousMatrix &eMc)
Definition: vpAfma6.cpp:1196
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:85
void get_cVe(vpVelocityTwistMatrix &cVe) const
Definition: vpAfma6.cpp:919
static const std::string CONST_EMC_INTEL_D435_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:91
vpColVector getJointMax() const
Definition: vpAfma6.cpp:1068
static const std::string CONST_CAMERA_AFMA6_FILENAME
Definition: vpAfma6.h:95
double _joint_min[6]
Definition: vpAfma6.h:204
vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const
Definition: vpAfma6.cpp:519
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
void init(void)
Definition: vpAfma6.cpp:157
vpRxyzVector _erc
Definition: vpAfma6.h:207
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:603
static const char *const CONST_VACUUM_CAMERA_NAME
Definition: vpAfma6.h:111
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Definition: vpAfma6.cpp:844
static const std::string CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:87
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:897
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
double _coupl_56
Definition: vpAfma6.h:201
static const std::string CONST_EMC_INTEL_D435_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:92
vpColVector getJointMin() const
Definition: vpAfma6.cpp:1052
static const char *const CONST_INTEL_D435_CAMERA_NAME
Definition: vpAfma6.h:122
double getCoupl56() const
Definition: vpAfma6.cpp:1082
static const vpAfma6ToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpAfma6.h:136
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:106
static const std::string CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:88
vpAfma6()
Definition: vpAfma6.cpp:119
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double _long_56
Definition: vpAfma6.h:202
void parseConfigFile(const std::string &filename)
Definition: vpAfma6.cpp:1102
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_eMc() const
Definition: vpAfma6.cpp:908
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height) const
Definition: vpAfma6.cpp:1269
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:784
static const char *const CONST_GENERIC_CAMERA_NAME
Definition: vpAfma6.h:116
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
static const std::string CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME
Definition: vpAfma6.h:89
static const std::string CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:94
double _joint_max[6]
Definition: vpAfma6.h:203
static const std::string CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME
Definition: vpAfma6.h:90
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:941
static const std::string CONST_AFMA6_FILENAME
Definition: vpAfma6.h:84
double getLong56() const
Definition: vpAfma6.cpp:1090
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1011
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
@ TOOL_CCMOP
Definition: vpAfma6.h:127
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:130
@ TOOL_CUSTOM
Definition: vpAfma6.h:132
@ TOOL_VACUUM
Definition: vpAfma6.h:129
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:131
@ TOOL_GRIPPER
Definition: vpAfma6.h:128
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:614
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ notImplementedError
Not implemented.
Definition: vpException.h:69
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void extract(vpRotationMatrix &R) const
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static double rad(double deg)
Definition: vpMath.h:129
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Error that can be emitted by the vpRobot class and its derivatives.
@ readingParametersError
Cannot parse parameters.
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:183
vpRxyzVector & buildFrom(const vpRotationMatrix &R)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)