Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpSimulatorAfma6.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  * Class which provides a simulator for the robot Afma6.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
38 #include <cmath> // std::fabs
39 #include <limits> // numeric_limits
40 #include <string>
41 #include <visp3/core/vpImagePoint.h>
42 #include <visp3/core/vpIoTools.h>
43 #include <visp3/core/vpMeterPixelConversion.h>
44 #include <visp3/core/vpPoint.h>
45 #include <visp3/core/vpTime.h>
46 #include <visp3/robot/vpRobotException.h>
47 #include <visp3/robot/vpSimulatorAfma6.h>
48 
49 #include "../wireframe-simulator/vpBound.h"
50 #include "../wireframe-simulator/vpRfstack.h"
51 #include "../wireframe-simulator/vpScene.h"
52 #include "../wireframe-simulator/vpVwstack.h"
53 
55 
60  : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true),
61  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
62 {
63  init();
64  initDisplay();
65 
67 
68  m_thread = new std::thread(&launcher, std::ref(*this));
69 
70  compute_fMi();
71 }
72 
80  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
81  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
82 {
83  init();
84  initDisplay();
85 
87 
88  m_thread = new std::thread(&launcher, std::ref(*this));
89 
90  compute_fMi();
91 }
92 
97 {
98  m_mutex_robotStop.lock();
99  robotStop = true;
100  m_mutex_robotStop.unlock();
101 
102  m_thread->join();
103 
104  if (robotArms != nullptr) {
105  for (int i = 0; i < 6; i++)
106  free_Bound_scene(&(robotArms[i]));
107  }
108 
109  delete[] robotArms;
110  delete[] fMi;
111  delete m_thread;
112 }
113 
123 {
124  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
125  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
126  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
127  bool armDirExists = false;
128  for (size_t i = 0; i < arm_dirs.size(); i++)
129  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
130  arm_dir = arm_dirs[i];
131  armDirExists = true;
132  break;
133  }
134  if (!armDirExists) {
135  try {
136  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
137  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
138  }
139  catch (...) {
140  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
141  }
142  }
143 
144  this->init(vpAfma6::TOOL_CCMOP);
145  toolCustom = false;
146 
147  size_fMi = 8;
148  fMi = new vpHomogeneousMatrix[8];
151 
152  zeroPos.resize(njoint);
153  zeroPos = 0;
154  reposPos.resize(njoint);
155  reposPos = 0;
156  reposPos[1] = -M_PI / 2;
157  reposPos[2] = M_PI;
158  reposPos[4] = M_PI / 2;
159 
160  artCoord = zeroPos;
161  artVel = 0;
162 
163  q_prev_getdis.resize(njoint);
164  q_prev_getdis = 0;
165  first_time_getdis = true;
166 
167  positioningVelocity = defaultPositioningVelocity;
168 
171 
172  // Software joint limits in radians
173  //_joint_min.resize(njoint);
174  _joint_min[0] = -0.6501;
175  _joint_min[1] = -0.6001;
176  _joint_min[2] = -0.5001;
177  _joint_min[3] = -2.7301;
178  _joint_min[4] = -0.1001;
179  _joint_min[5] = -1.5901;
180  //_joint_max.resize(njoint);
181  _joint_max[0] = 0.7001;
182  _joint_max[1] = 0.5201;
183  _joint_max[2] = 0.4601;
184  _joint_max[3] = 2.7301;
185  _joint_max[4] = 2.4801;
186  _joint_max[5] = 1.5901;
187 }
188 
193 {
194  robotArms = nullptr;
195  robotArms = new Bound_scene[6];
196  initArms();
198  vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
199  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
201  vpCameraParameters tmp;
202  getCameraParameters(tmp, 640, 480);
203  px_int = tmp.get_px();
204  py_int = tmp.get_py();
205  sceneInitialized = true;
206 }
207 
224 {
225  this->projModel = proj_model;
226  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
227  if (arm_dir.size() > FILENAME_MAX)
228  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
229  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
230  if (full_length > FILENAME_MAX)
231  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
232 
233  // Use here default values of the robot constant parameters.
234  switch (tool) {
235  case vpAfma6::TOOL_CCMOP: {
236  _erc[0] = vpMath::rad(164.35); // rx
237  _erc[1] = vpMath::rad(89.64); // ry
238  _erc[2] = vpMath::rad(-73.05); // rz
239  _etc[0] = 0.0117; // tx
240  _etc[1] = 0.0033; // ty
241  _etc[2] = 0.2272; // tz
242 
243  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
244 
245  if (robotArms != nullptr) {
246  while (get_displayBusy())
247  vpTime::wait(2);
248  free_Bound_scene(&(robotArms[5]));
249  char *name_arm = new char[full_length];
250  strcpy(name_arm, arm_dir.c_str());
251  strcat(name_arm, "/afma6_tool_ccmop.bnd");
252  set_scene(name_arm, robotArms + 5, 1.0);
253  set_displayBusy(false);
254  delete[] name_arm;
255  }
256  break;
257  }
258  case vpAfma6::TOOL_GRIPPER: {
259  _erc[0] = vpMath::rad(88.33); // rx
260  _erc[1] = vpMath::rad(72.07); // ry
261  _erc[2] = vpMath::rad(2.53); // rz
262  _etc[0] = 0.0783; // tx
263  _etc[1] = 0.1234; // ty
264  _etc[2] = 0.1638; // tz
265 
266  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
267 
268  if (robotArms != nullptr) {
269  while (get_displayBusy())
270  vpTime::wait(2);
271  free_Bound_scene(&(robotArms[5]));
272  char *name_arm = new char[full_length];
273  strcpy(name_arm, arm_dir.c_str());
274  strcat(name_arm, "/afma6_tool_gripper.bnd");
275  set_scene(name_arm, robotArms + 5, 1.0);
276  set_displayBusy(false);
277  delete[] name_arm;
278  }
279  break;
280  }
281  case vpAfma6::TOOL_VACUUM: {
282  _erc[0] = vpMath::rad(90.40); // rx
283  _erc[1] = vpMath::rad(75.11); // ry
284  _erc[2] = vpMath::rad(0.18); // rz
285  _etc[0] = 0.0038; // tx
286  _etc[1] = 0.1281; // ty
287  _etc[2] = 0.1658; // tz
288 
289  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
290 
291  if (robotArms != nullptr) {
292  while (get_displayBusy())
293  vpTime::wait(2);
294  free_Bound_scene(&(robotArms[5]));
295 
296  char *name_arm = new char[full_length];
297 
298  strcpy(name_arm, arm_dir.c_str());
299  strcat(name_arm, "/afma6_tool_vacuum.bnd");
300  set_scene(name_arm, robotArms + 5, 1.0);
301  set_displayBusy(false);
302  delete[] name_arm;
303  }
304  break;
305  }
306  case vpAfma6::TOOL_CUSTOM: {
307  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
308  break;
309  }
311  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
312  break;
313  }
315  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
316  break;
317  }
318  }
319 
320  vpRotationMatrix eRc(_erc);
321 
322  m_mutex_eMc.lock();
323  this->_eMc.buildFrom(_etc, eRc);
324  m_mutex_eMc.unlock();
325 
326  setToolType(tool);
327  return;
328 }
329 
340 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
341  const unsigned int &image_height)
342 {
343  if (toolCustom) {
344  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
345  }
346  // Set default parameters
347  switch (getToolType()) {
348  case vpAfma6::TOOL_CCMOP: {
349  // Set default intrinsic camera parameters for 640x480 images
350  if (image_width == 640 && image_height == 480) {
351  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
352  << std::endl;
353  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
354  }
355  else {
356  vpTRACE("Cannot get default intrinsic camera parameters for this image "
357  "resolution");
358  }
359  break;
360  }
361  case vpAfma6::TOOL_GRIPPER: {
362  // Set default intrinsic camera parameters for 640x480 images
363  if (image_width == 640 && image_height == 480) {
364  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
365  << std::endl;
366  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
367  }
368  else {
369  vpTRACE("Cannot get default intrinsic camera parameters for this image "
370  "resolution");
371  }
372  break;
373  }
374  case vpAfma6::TOOL_CUSTOM: {
375  std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
376  break;
377  }
379  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
380  break;
381  }
383  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
384  break;
385  }
386  case vpAfma6::TOOL_VACUUM: {
387  std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
388  break;
389  }
390  default:
391  vpERROR_TRACE("This error should not occur!");
392  break;
393  }
394  return;
395 }
396 
406 {
407  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
408 }
409 
419 {
420  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
421 }
422 
429 {
430  px_int = cam.get_px();
431  py_int = cam.get_py();
432  toolCustom = true;
433 }
434 
440 {
441  double tcur_1 = tcur; // temporary variable used to store the last time
442  // since the last command
443  bool stop = false;
444  bool setVelocityCalled_ = false;
445  while (!stop) {
446  // Get current time
447  tprev = tcur_1;
449 
451  setVelocityCalled_ = setVelocityCalled;
452  m_mutex_setVelocityCalled.unlock();
453 
454  if (setVelocityCalled_ || !constantSamplingTimeMode) {
456  setVelocityCalled = false;
457  m_mutex_setVelocityCalled.unlock();
458 
460 
461  double ellapsedTime = (tcur - tprev) * 1e-3;
462  if (constantSamplingTimeMode) { // if we want a constant velocity, we
463  // force the ellapsed time to the given
464  // samplingTime
465  ellapsedTime = getSamplingTime(); // in second
466  }
467 
468  vpColVector articularCoordinates = get_artCoord();
469  vpColVector articularVelocities = get_artVel();
470 
471  if (jointLimit) {
472  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
473  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
474  if (verbose_) {
475  std::cout << "Joint " << jointLimitArt - 1
476  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
477  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
478  }
479 
480  articularVelocities = 0.0;
481  }
482  else
483  jointLimit = false;
484  }
485 
486  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
487  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
488  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
489  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
490  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
491  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
492 
493  int jl = isInJointLimit();
494 
495  if (jl != 0 && jointLimit == false) {
496  if (jl < 0)
497  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
498  (articularVelocities[(unsigned int)(-jl - 1)]);
499  else
500  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
501  (articularVelocities[(unsigned int)(jl - 1)]);
502 
503  for (unsigned int i = 0; i < 6; i++)
504  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
505 
506  jointLimit = true;
507  jointLimitArt = (unsigned int)fabs((double)jl);
508  }
509 
510  set_artCoord(articularCoordinates);
511  set_artVel(articularVelocities);
512 
513  compute_fMi();
514 
515  if (displayAllowed) {
519  }
520 
521  if (displayType == MODEL_3D && displayAllowed) {
522  while (get_displayBusy()) {
523  vpTime::wait(2);
524  }
526  set_displayBusy(false);
527  }
528 
529  if (displayType == MODEL_DH && displayAllowed) {
530  vpHomogeneousMatrix fMit[8];
531  get_fMi(fMit);
532 
533  // vpDisplay::displayFrame(I,getExternalCameraPosition
534  // ()*fMi[6],cameraParam,0.2,vpColor::none);
535 
536  vpImagePoint iP, iP_1;
537  vpPoint pt(0, 0, 0);
538 
541  pt.track(getExternalCameraPosition() * fMit[0]);
544  for (unsigned int k = 1; k < 7; k++) {
545  pt.track(getExternalCameraPosition() * fMit[k - 1]);
547 
548  pt.track(getExternalCameraPosition() * fMit[k]);
550 
552  }
554  thickness_);
555  }
556 
558 
559  vpTime::wait(tcur, 1000 * getSamplingTime());
560  tcur_1 = tcur;
561  }
562  else {
564  }
565 
566  m_mutex_robotStop.lock();
567  stop = robotStop;
568  m_mutex_robotStop.unlock();
569  }
570 }
571 
586 {
587  // vpColVector q = get_artCoord();
588  vpColVector q(6); //; = get_artCoord();
589  q = get_artCoord();
590 
591  vpHomogeneousMatrix fMit[8];
592 
593  double q1 = q[0];
594  double q2 = q[1];
595  double q3 = q[2];
596  double q4 = q[3];
597  double q5 = q[4];
598  double q6 = q[5];
599 
600  double c4 = cos(q4);
601  double s4 = sin(q4);
602  double c5 = cos(q5);
603  double s5 = sin(q5);
604  double c6 = cos(q6);
605  double s6 = sin(q6);
606 
607  fMit[0][0][0] = 1;
608  fMit[0][1][0] = 0;
609  fMit[0][2][0] = 0;
610  fMit[0][0][1] = 0;
611  fMit[0][1][1] = 1;
612  fMit[0][2][1] = 0;
613  fMit[0][0][2] = 0;
614  fMit[0][1][2] = 0;
615  fMit[0][2][2] = 1;
616  fMit[0][0][3] = q1;
617  fMit[0][1][3] = 0;
618  fMit[0][2][3] = 0;
619 
620  fMit[1][0][0] = 1;
621  fMit[1][1][0] = 0;
622  fMit[1][2][0] = 0;
623  fMit[1][0][1] = 0;
624  fMit[1][1][1] = 1;
625  fMit[1][2][1] = 0;
626  fMit[1][0][2] = 0;
627  fMit[1][1][2] = 0;
628  fMit[1][2][2] = 1;
629  fMit[1][0][3] = q1;
630  fMit[1][1][3] = q2;
631  fMit[1][2][3] = 0;
632 
633  fMit[2][0][0] = 1;
634  fMit[2][1][0] = 0;
635  fMit[2][2][0] = 0;
636  fMit[2][0][1] = 0;
637  fMit[2][1][1] = 1;
638  fMit[2][2][1] = 0;
639  fMit[2][0][2] = 0;
640  fMit[2][1][2] = 0;
641  fMit[2][2][2] = 1;
642  fMit[2][0][3] = q1;
643  fMit[2][1][3] = q2;
644  fMit[2][2][3] = q3;
645 
646  fMit[3][0][0] = s4;
647  fMit[3][1][0] = -c4;
648  fMit[3][2][0] = 0;
649  fMit[3][0][1] = c4;
650  fMit[3][1][1] = s4;
651  fMit[3][2][1] = 0;
652  fMit[3][0][2] = 0;
653  fMit[3][1][2] = 0;
654  fMit[3][2][2] = 1;
655  fMit[3][0][3] = q1;
656  fMit[3][1][3] = q2;
657  fMit[3][2][3] = q3;
658 
659  fMit[4][0][0] = s4 * s5;
660  fMit[4][1][0] = -c4 * s5;
661  fMit[4][2][0] = c5;
662  fMit[4][0][1] = s4 * c5;
663  fMit[4][1][1] = -c4 * c5;
664  fMit[4][2][1] = -s5;
665  fMit[4][0][2] = c4;
666  fMit[4][1][2] = s4;
667  fMit[4][2][2] = 0;
668  fMit[4][0][3] = c4 * this->_long_56 + q1;
669  fMit[4][1][3] = s4 * this->_long_56 + q2;
670  fMit[4][2][3] = q3;
671 
672  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
673  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
674  fMit[5][2][0] = c5 * c6;
675  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
676  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
677  fMit[5][2][1] = -c5 * s6;
678  fMit[5][0][2] = -s4 * c5;
679  fMit[5][1][2] = c4 * c5;
680  fMit[5][2][2] = s5;
681  fMit[5][0][3] = c4 * this->_long_56 + q1;
682  fMit[5][1][3] = s4 * this->_long_56 + q2;
683  fMit[5][2][3] = q3;
684 
685  fMit[6][0][0] = fMit[5][0][0];
686  fMit[6][1][0] = fMit[5][1][0];
687  fMit[6][2][0] = fMit[5][2][0];
688  fMit[6][0][1] = fMit[5][0][1];
689  fMit[6][1][1] = fMit[5][1][1];
690  fMit[6][2][1] = fMit[5][2][1];
691  fMit[6][0][2] = fMit[5][0][2];
692  fMit[6][1][2] = fMit[5][1][2];
693  fMit[6][2][2] = fMit[5][2][2];
694  fMit[6][0][3] = fMit[5][0][3];
695  fMit[6][1][3] = fMit[5][1][3];
696  fMit[6][2][3] = fMit[5][2][3];
697 
698  // vpHomogeneousMatrix cMe;
699  // get_cMe(cMe);
700  // cMe = cMe.inverse();
701  // fMit[7] = fMit[6] * cMe;
702  m_mutex_eMc.lock();
703  vpAfma6::get_fMc(q, fMit[7]);
704  m_mutex_eMc.unlock();
705 
706  m_mutex_fMi.lock();
707  for (int i = 0; i < 8; i++) {
708  fMi[i] = fMit[i];
709  }
710  m_mutex_fMi.unlock();
711 }
712 
719 {
720  switch (newState) {
721  case vpRobot::STATE_STOP: {
722  // Start primitive STOP only if the current state is Velocity
724  stopMotion();
725  }
726  break;
727  }
730  std::cout << "Change the control mode from velocity to position control.\n";
731  stopMotion();
732  }
733  else {
734  // std::cout << "Change the control mode from stop to position
735  // control.\n";
736  }
737  break;
738  }
741  std::cout << "Change the control mode from stop to velocity control.\n";
742  }
743  break;
744  }
746  default:
747  break;
748  }
749 
750  return vpRobot::setRobotState(newState);
751 }
752 
827 {
829  vpERROR_TRACE("Cannot send a velocity to the robot "
830  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
832  "Cannot send a velocity to the robot "
833  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
834  }
835 
836  vpColVector vel_sat(6);
837 
838  double scale_sat = 1;
839  double vel_trans_max = getMaxTranslationVelocity();
840  double vel_rot_max = getMaxRotationVelocity();
841 
842  double vel_abs; // Absolute value
843 
844  // Velocity saturation
845  switch (frame) {
846  // saturation in cartesian space
849  if (vel.getRows() != 6) {
850  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
851  throw;
852  }
853 
854  for (unsigned int i = 0; i < 3; ++i) {
855  vel_abs = fabs(vel[i]);
856  if (vel_abs > vel_trans_max && !jointLimit) {
857  vel_trans_max = vel_abs;
858  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
859  "(axis nr. %d).",
860  vel[i], i + 1);
861  }
862 
863  vel_abs = fabs(vel[i + 3]);
864  if (vel_abs > vel_rot_max && !jointLimit) {
865  vel_rot_max = vel_abs;
866  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
867  "(axis nr. %d).",
868  vel[i + 3], i + 4);
869  }
870  }
871 
872  double scale_trans_sat = 1;
873  double scale_rot_sat = 1;
874  if (vel_trans_max > getMaxTranslationVelocity())
875  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
876 
877  if (vel_rot_max > getMaxRotationVelocity())
878  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
879 
880  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
881  if (scale_trans_sat < scale_rot_sat)
882  scale_sat = scale_trans_sat;
883  else
884  scale_sat = scale_rot_sat;
885  }
886  break;
887  }
888 
889  // saturation in joint space
891  if (vel.getRows() != 6) {
892  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
893  throw;
894  }
895  for (unsigned int i = 0; i < 6; ++i) {
896  vel_abs = fabs(vel[i]);
897  if (vel_abs > vel_rot_max && !jointLimit) {
898  vel_rot_max = vel_abs;
899  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
900  "(axis nr. %d).",
901  vel[i], i + 1);
902  }
903  }
904  double scale_rot_sat = 1;
905  if (vel_rot_max > getMaxRotationVelocity())
906  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
907  if (scale_rot_sat < 1)
908  scale_sat = scale_rot_sat;
909  break;
910  }
911  case vpRobot::MIXT_FRAME: {
912  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
913  "functionality not implemented");
914  }
916  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOR_FRAME frame:"
917  "functionality not implemented");
918  }
919  }
920 
921  set_velocity(vel * scale_sat);
922 
923  m_mutex_frame.lock();
924  setRobotFrame(frame);
925  m_mutex_frame.unlock();
926 
928  setVelocityCalled = true;
929  m_mutex_setVelocityCalled.unlock();
930 }
931 
936 {
938 
939  m_mutex_frame.lock();
940  frame = getRobotFrame();
941  m_mutex_frame.unlock();
942 
943  double vel_rot_max = getMaxRotationVelocity();
944 
945  vpColVector articularCoordinates = get_artCoord();
946  vpColVector velocityframe = get_velocity();
947  vpColVector articularVelocity;
948 
949  switch (frame) {
950  case vpRobot::CAMERA_FRAME: {
951  vpMatrix eJe_;
953  vpAfma6::get_eJe(articularCoordinates, eJe_);
954  eJe_ = eJe_.pseudoInverse();
956  singularityTest(articularCoordinates, eJe_);
957  articularVelocity = eJe_ * eVc * velocityframe;
958  set_artVel(articularVelocity);
959  break;
960  }
962  vpMatrix fJe_;
963  vpAfma6::get_fJe(articularCoordinates, fJe_);
964  fJe_ = fJe_.pseudoInverse();
966  singularityTest(articularCoordinates, fJe_);
967  articularVelocity = fJe_ * velocityframe;
968  set_artVel(articularVelocity);
969  break;
970  }
972  articularVelocity = velocityframe;
973  set_artVel(articularVelocity);
974  break;
975  }
977  case vpRobot::MIXT_FRAME: {
978  break;
979  }
980  }
981 
982  switch (frame) {
985  for (unsigned int i = 0; i < 6; ++i) {
986  double vel_abs = fabs(articularVelocity[i]);
987  if (vel_abs > vel_rot_max && !jointLimit) {
988  vel_rot_max = vel_abs;
989  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
990  "(axis nr. %d).",
991  articularVelocity[i], i + 1);
992  }
993  }
994  double scale_rot_sat = 1;
995  double scale_sat = 1;
996  if (vel_rot_max > getMaxRotationVelocity())
997  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
998  if (scale_rot_sat < 1)
999  scale_sat = scale_rot_sat;
1000 
1001  set_artVel(articularVelocity * scale_sat);
1002  break;
1003  }
1006  case vpRobot::MIXT_FRAME: {
1007  break;
1008  }
1009  }
1010 }
1011 
1058 {
1059  vel.resize(6);
1060 
1061  vpColVector articularCoordinates = get_artCoord();
1062  vpColVector articularVelocity = get_artVel();
1063 
1064  switch (frame) {
1065  case vpRobot::CAMERA_FRAME: {
1066  vpMatrix eJe_;
1068  vpAfma6::get_eJe(articularCoordinates, eJe_);
1069  vel = cVe * eJe_ * articularVelocity;
1070  break;
1071  }
1072  case vpRobot::ARTICULAR_FRAME: {
1073  vel = articularVelocity;
1074  break;
1075  }
1076  case vpRobot::REFERENCE_FRAME: {
1077  vpMatrix fJe_;
1078  vpAfma6::get_fJe(articularCoordinates, fJe_);
1079  vel = fJe_ * articularVelocity;
1080  break;
1081  }
1083  case vpRobot::MIXT_FRAME: {
1084  break;
1085  }
1086  default: {
1087  vpERROR_TRACE("Error in spec of vpRobot. "
1088  "Case not taken in account.");
1089  return;
1090  }
1091  }
1092 }
1093 
1111 {
1112  timestamp = vpTime::measureTimeSecond();
1113  getVelocity(frame, vel);
1114 }
1115 
1158 {
1159  vpColVector vel(6);
1160  getVelocity(frame, vel);
1161 
1162  return vel;
1163 }
1164 
1178 {
1179  timestamp = vpTime::measureTimeSecond();
1180  vpColVector vel(6);
1181  getVelocity(frame, vel);
1182 
1183  return vel;
1184 }
1185 
1187 {
1188  double vel_rot_max = getMaxRotationVelocity();
1189  double velmax = fabs(q[0]);
1190  for (unsigned int i = 1; i < 6; i++) {
1191  if (velmax < fabs(q[i]))
1192  velmax = fabs(q[i]);
1193  }
1194 
1195  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1196  q = q * alpha;
1197 }
1198 
1274 {
1276  vpERROR_TRACE("Robot was not in position-based control\n"
1277  "Modification of the robot state");
1278  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1279  }
1280 
1281  vpColVector articularCoordinates = get_artCoord();
1282 
1283  vpColVector error(6);
1284  double errsqr = 0;
1285  switch (frame) {
1286  case vpRobot::CAMERA_FRAME: {
1287  int nbSol;
1288  vpColVector qdes(6);
1289 
1290  vpTranslationVector txyz;
1291  vpRxyzVector rxyz;
1292  for (unsigned int i = 0; i < 3; i++) {
1293  txyz[i] = q[i];
1294  rxyz[i] = q[i + 3];
1295  }
1296 
1297  vpRotationMatrix cRc2(rxyz);
1298  vpHomogeneousMatrix cMc2(txyz, cRc2);
1299 
1300  vpHomogeneousMatrix fMc_;
1301  vpAfma6::get_fMc(articularCoordinates, fMc_);
1302 
1303  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1304 
1305  do {
1306  articularCoordinates = get_artCoord();
1307  qdes = articularCoordinates;
1308  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1309 
1311  setVelocityCalled = true;
1312  m_mutex_setVelocityCalled.unlock();
1313 
1314  if (nbSol > 0) {
1315  error = qdes - articularCoordinates;
1316  errsqr = error.sumSquare();
1317  // findHighestPositioningSpeed(error);
1318  set_artVel(error);
1319  if (errsqr < 1e-4) {
1320  set_artCoord(qdes);
1321  error = 0;
1322  set_artVel(error);
1323  set_velocity(error);
1324  break;
1325  }
1326  }
1327  else {
1328  vpERROR_TRACE("Positioning error.");
1329  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1330  }
1331  } while (errsqr > 1e-8 && nbSol > 0);
1332 
1333  break;
1334  }
1335 
1336  case vpRobot::ARTICULAR_FRAME: {
1337  do {
1338  articularCoordinates = get_artCoord();
1339  error = q - articularCoordinates;
1340  errsqr = error.sumSquare();
1341  // findHighestPositioningSpeed(error);
1342  set_artVel(error);
1344  setVelocityCalled = true;
1345  m_mutex_setVelocityCalled.unlock();
1346  if (errsqr < 1e-4) {
1347  set_artCoord(q);
1348  error = 0;
1349  set_artVel(error);
1350  set_velocity(error);
1351  break;
1352  }
1353  } while (errsqr > 1e-8);
1354  break;
1355  }
1356 
1357  case vpRobot::REFERENCE_FRAME: {
1358  int nbSol;
1359  vpColVector qdes(6);
1360 
1361  vpTranslationVector txyz;
1362  vpRxyzVector rxyz;
1363  for (unsigned int i = 0; i < 3; i++) {
1364  txyz[i] = q[i];
1365  rxyz[i] = q[i + 3];
1366  }
1367 
1368  vpRotationMatrix fRc(rxyz);
1369  vpHomogeneousMatrix fMc_(txyz, fRc);
1370 
1371  do {
1372  articularCoordinates = get_artCoord();
1373  qdes = articularCoordinates;
1374  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1376  setVelocityCalled = true;
1377  m_mutex_setVelocityCalled.unlock();
1378  if (nbSol > 0) {
1379  error = qdes - articularCoordinates;
1380  errsqr = error.sumSquare();
1381  // findHighestPositioningSpeed(error);
1382  set_artVel(error);
1383  if (errsqr < 1e-4) {
1384  set_artCoord(qdes);
1385  error = 0;
1386  set_artVel(error);
1387  set_velocity(error);
1388  break;
1389  }
1390  }
1391  else
1392  vpERROR_TRACE("Positioning error. Position unreachable");
1393  } while (errsqr > 1e-8 && nbSol > 0);
1394  break;
1395  }
1396  case vpRobot::MIXT_FRAME: {
1397  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1398  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1399  "MIXT_FRAME not implemented.");
1400  }
1402  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1403  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1404  "END_EFFECTOR_FRAME not implemented.");
1405  }
1406  }
1407 }
1408 
1471 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1472  double pos4, double pos5, double pos6)
1473 {
1474  try {
1475  vpColVector position(6);
1476  position[0] = pos1;
1477  position[1] = pos2;
1478  position[2] = pos3;
1479  position[3] = pos4;
1480  position[4] = pos5;
1481  position[5] = pos6;
1482 
1483  setPosition(frame, position);
1484  }
1485  catch (...) {
1486  vpERROR_TRACE("Error caught");
1487  throw;
1488  }
1489 }
1490 
1525 void vpSimulatorAfma6::setPosition(const char *filename)
1526 {
1527  vpColVector q;
1528  bool ret;
1529 
1530  ret = this->readPosFile(filename, q);
1531 
1532  if (ret == false) {
1533  vpERROR_TRACE("Bad position in \"%s\"", filename);
1534  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1535  }
1538 }
1539 
1600 {
1601  q.resize(6);
1602 
1603  switch (frame) {
1604  case vpRobot::CAMERA_FRAME: {
1605  q = 0;
1606  break;
1607  }
1608 
1609  case vpRobot::ARTICULAR_FRAME: {
1610  q = get_artCoord();
1611  break;
1612  }
1613 
1614  case vpRobot::REFERENCE_FRAME: {
1615  vpHomogeneousMatrix fMc_;
1616  vpAfma6::get_fMc(get_artCoord(), fMc_);
1617 
1618  vpRotationMatrix fRc;
1619  fMc_.extract(fRc);
1620  vpRxyzVector rxyz(fRc);
1621 
1622  vpTranslationVector txyz;
1623  fMc_.extract(txyz);
1624 
1625  for (unsigned int i = 0; i < 3; i++) {
1626  q[i] = txyz[i];
1627  q[i + 3] = rxyz[i];
1628  }
1629  break;
1630  }
1631 
1632  case vpRobot::MIXT_FRAME: {
1633  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1634  "Mixt frame not implemented.");
1635  }
1637  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1638  "End-effector frame not implemented.");
1639  }
1640  }
1641 }
1642 
1670 {
1671  timestamp = vpTime::measureTimeSecond();
1672  getPosition(frame, q);
1673 }
1674 
1687 {
1688  vpColVector posRxyz;
1689  // recupere position en Rxyz
1690  this->getPosition(frame, posRxyz);
1691 
1692  // recupere le vecteur thetaU correspondant
1693  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1694 
1695  // remplit le vpPoseVector avec translation et rotation ThetaU
1696  for (unsigned int j = 0; j < 3; j++) {
1697  position[j] = posRxyz[j];
1698  position[j + 3] = RtuVect[j];
1699  }
1700 }
1701 
1713 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1714 {
1715  timestamp = vpTime::measureTimeSecond();
1716  getPosition(frame, position);
1717 }
1718 
1729 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1730 {
1731  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1732  vpTRACE("Joint limit vector has not a size of 6 !");
1733  return;
1734  }
1735 
1736  _joint_min[0] = limitMin[0];
1737  _joint_min[1] = limitMin[1];
1738  _joint_min[2] = limitMin[2];
1739  _joint_min[3] = limitMin[3];
1740  _joint_min[4] = limitMin[4];
1741  _joint_min[5] = limitMin[5];
1742 
1743  _joint_max[0] = limitMax[0];
1744  _joint_max[1] = limitMax[1];
1745  _joint_max[2] = limitMax[2];
1746  _joint_max[3] = limitMax[3];
1747  _joint_max[4] = limitMax[4];
1748  _joint_max[5] = limitMax[5];
1749 }
1750 
1757 {
1758  double q5 = q[4];
1759 
1760  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1761 
1762  if (cond) {
1763  J[0][3] = 0;
1764  J[0][4] = 0;
1765  J[1][3] = 0;
1766  J[1][4] = 0;
1767  J[3][3] = 0;
1768  J[3][4] = 0;
1769  J[5][3] = 0;
1770  J[5][4] = 0;
1771  return true;
1772  }
1773 
1774  return false;
1775 }
1776 
1781 {
1782  int artNumb = 0;
1783  double diff = 0;
1784  double difft = 0;
1785 
1786  vpColVector articularCoordinates = get_artCoord();
1787 
1788  for (unsigned int i = 0; i < 6; i++) {
1789  if (articularCoordinates[i] <= _joint_min[i]) {
1790  difft = _joint_min[i] - articularCoordinates[i];
1791  if (difft > diff) {
1792  diff = difft;
1793  artNumb = -(int)i - 1;
1794  }
1795  }
1796  }
1797 
1798  for (unsigned int i = 0; i < 6; i++) {
1799  if (articularCoordinates[i] >= _joint_max[i]) {
1800  difft = articularCoordinates[i] - _joint_max[i];
1801  if (difft > diff) {
1802  diff = difft;
1803  artNumb = (int)(i + 1);
1804  }
1805  }
1806  }
1807 
1808  if (artNumb != 0)
1809  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1810  << std::endl;
1811 
1812  return artNumb;
1813 }
1814 
1833 {
1834  displacement.resize(6);
1835  displacement = 0;
1836  vpColVector q_cur(6);
1837 
1838  q_cur = get_artCoord();
1839 
1840  if (!first_time_getdis) {
1841  switch (frame) {
1842  case vpRobot::CAMERA_FRAME: {
1843  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1844  return;
1845  }
1846  case vpRobot::ARTICULAR_FRAME: {
1847  displacement = q_cur - q_prev_getdis;
1848  break;
1849  }
1850  case vpRobot::REFERENCE_FRAME: {
1851  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1852  return;
1853  }
1854  case vpRobot::MIXT_FRAME: {
1855  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1856  return;
1857  }
1859  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1860  return;
1861  }
1862  }
1863  }
1864  else {
1865  first_time_getdis = false;
1866  }
1867 
1868  // Memorize the joint position for the next call
1869  q_prev_getdis = q_cur;
1870 }
1871 
1919 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1920 {
1921  std::ifstream fd(filename.c_str(), std::ios::in);
1922 
1923  if (!fd.is_open()) {
1924  return false;
1925  }
1926 
1927  std::string line;
1928  std::string key("R:");
1929  std::string id("#AFMA6 - Position");
1930  bool pos_found = false;
1931  int lineNum = 0;
1932 
1933  q.resize(njoint);
1934 
1935  while (std::getline(fd, line)) {
1936  lineNum++;
1937  if (lineNum == 1) {
1938  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1939  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1940  return false;
1941  }
1942  }
1943  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1944  continue;
1945  }
1946  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1947  // check if there are at least njoint values in the line
1948  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1949  if (chain.size() < njoint + 1) // try to split with tab separator
1950  chain = vpIoTools::splitChain(line, std::string("\t"));
1951  if (chain.size() < njoint + 1)
1952  continue;
1953 
1954  std::istringstream ss(line);
1955  std::string key_;
1956  ss >> key_;
1957  for (unsigned int i = 0; i < njoint; i++)
1958  ss >> q[i];
1959  pos_found = true;
1960  break;
1961  }
1962  }
1963 
1964  // converts rotations from degrees into radians
1965  q[3] = vpMath::rad(q[3]);
1966  q[4] = vpMath::rad(q[4]);
1967  q[5] = vpMath::rad(q[5]);
1968 
1969  fd.close();
1970 
1971  if (!pos_found) {
1972  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
1973  return false;
1974  }
1975 
1976  return true;
1977 }
1978 
2001 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2002 {
2003  FILE *fd;
2004  fd = fopen(filename.c_str(), "w");
2005  if (fd == nullptr)
2006  return false;
2007 
2008  fprintf(fd, "\
2009 #AFMA6 - Position - Version 2.01\n\
2010 #\n\
2011 # R: X Y Z A B C\n\
2012 # Joint position: X, Y, Z: translations in meters\n\
2013 # A, B, C: rotations in degrees\n\
2014 #\n\
2015 #\n\n");
2016 
2017  // Save positions in mm and deg
2018  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2019  vpMath::deg(q[5]));
2020 
2021  fclose(fd);
2022  return (true);
2023 }
2024 
2032 void vpSimulatorAfma6::move(const char *filename)
2033 {
2034  vpColVector q;
2035 
2036  try {
2037  this->readPosFile(filename, q);
2040  }
2041  catch (...) {
2042  throw;
2043  }
2044 }
2045 
2056 
2065 {
2066  vpHomogeneousMatrix cMe;
2067  vpAfma6::get_cMe(cMe);
2068 
2069  cVe.buildFrom(cMe);
2070 }
2071 
2082 {
2083  try {
2084  vpAfma6::get_eJe(get_artCoord(), eJe_);
2085  }
2086  catch (...) {
2087  vpERROR_TRACE("catch exception ");
2088  throw;
2089  }
2090 }
2091 
2103 {
2104  try {
2105  vpColVector articularCoordinates = get_artCoord();
2106  vpAfma6::get_fJe(articularCoordinates, fJe_);
2107  }
2108  catch (...) {
2109  vpERROR_TRACE("Error caught");
2110  throw;
2111  }
2112 }
2113 
2118 {
2120  return;
2121 
2122  vpColVector stop(6);
2123  stop = 0;
2124  set_artVel(stop);
2125  set_velocity(stop);
2127 }
2128 
2129 /**********************************************************************************/
2130 /**********************************************************************************/
2131 /**********************************************************************************/
2132 /**********************************************************************************/
2133 
2143 {
2144  // set scene_dir from #define VISP_SCENE_DIR if it exists
2145  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2146  std::string scene_dir_;
2147  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2148  bool sceneDirExists = false;
2149  for (size_t i = 0; i < scene_dirs.size(); i++)
2150  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2151  scene_dir_ = scene_dirs[i];
2152  sceneDirExists = true;
2153  break;
2154  }
2155  if (!sceneDirExists) {
2156  try {
2157  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2158  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2159  }
2160  catch (...) {
2161  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2162  }
2163  }
2164 
2165  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2166  if (scene_dir_.size() > FILENAME_MAX)
2167  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2168  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2169  if (full_length > FILENAME_MAX)
2170  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2171 
2172  char *name_cam = new char[full_length];
2173 
2174  strcpy(name_cam, scene_dir_.c_str());
2175  strcat(name_cam, "/camera.bnd");
2176  set_scene(name_cam, &camera, cameraFactor);
2177 
2178  if (arm_dir.size() > FILENAME_MAX)
2179  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2180  full_length = (unsigned int)arm_dir.size() + name_length;
2181  if (full_length > FILENAME_MAX)
2182  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2183 
2184  char *name_arm = new char[full_length];
2185  strcpy(name_arm, arm_dir.c_str());
2186  strcat(name_arm, "/afma6_gate.bnd");
2187  std::cout << "name arm: " << name_arm << std::endl;
2188  set_scene(name_arm, robotArms, 1.0);
2189  strcpy(name_arm, arm_dir.c_str());
2190  strcat(name_arm, "/afma6_arm1.bnd");
2191  set_scene(name_arm, robotArms + 1, 1.0);
2192  strcpy(name_arm, arm_dir.c_str());
2193  strcat(name_arm, "/afma6_arm2.bnd");
2194  set_scene(name_arm, robotArms + 2, 1.0);
2195  strcpy(name_arm, arm_dir.c_str());
2196  strcat(name_arm, "/afma6_arm3.bnd");
2197  set_scene(name_arm, robotArms + 3, 1.0);
2198  strcpy(name_arm, arm_dir.c_str());
2199  strcat(name_arm, "/afma6_arm4.bnd");
2200  set_scene(name_arm, robotArms + 4, 1.0);
2201 
2203  tool = getToolType();
2204  strcpy(name_arm, arm_dir.c_str());
2205  switch (tool) {
2206  case vpAfma6::TOOL_CCMOP: {
2207  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2208  break;
2209  }
2210  case vpAfma6::TOOL_GRIPPER: {
2211  strcat(name_arm, "/afma6_tool_gripper.bnd");
2212  break;
2213  }
2214  case vpAfma6::TOOL_VACUUM: {
2215  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2216  break;
2217  }
2218  case vpAfma6::TOOL_CUSTOM: {
2219  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2220  break;
2221  }
2223  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2224  break;
2225  }
2227  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2228  break;
2229  }
2230  }
2231  set_scene(name_arm, robotArms + 5, 1.0);
2232 
2233  add_rfstack(IS_BACK);
2234 
2235  add_vwstack("start", "depth", 0.0, 100.0);
2236  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2237  add_vwstack("start", "type", PERSPECTIVE);
2238  //
2239  // sceneInitialized = true;
2240  // displayObject = true;
2241  displayCamera = true;
2242 
2243  delete[] name_cam;
2244  delete[] name_arm;
2245 }
2246 
2248 {
2249  m_mutex_scene.lock();
2250  bool changed = false;
2251  vpHomogeneousMatrix displacement = navigation(I_, changed);
2252 
2253  // if (displacement[2][3] != 0)
2254  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2255  camMf2 = camMf2 * displacement;
2256 
2257  f2Mf = camMf2.inverse() * camMf;
2258 
2259  camMf = camMf2 * displacement * f2Mf;
2260 
2261  double u;
2262  double v;
2263  // if(px_ext != 1 && py_ext != 1)
2264  // we assume px_ext and py_ext > 0
2265  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2266  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2267  u = (double)I_.getWidth() / (2 * px_ext);
2268  v = (double)I_.getHeight() / (2 * py_ext);
2269  }
2270  else {
2271  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2272  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2273  }
2274 
2275  float w44o[4][4], w44cext[4][4], x, y, z;
2276 
2277  vp2jlc_matrix(camMf.inverse(), w44cext);
2278 
2279  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2280  x = w44cext[2][0] + w44cext[3][0];
2281  y = w44cext[2][1] + w44cext[3][1];
2282  z = w44cext[2][2] + w44cext[3][2];
2283  add_vwstack("start", "vrp", x, y, z);
2284  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2285  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2286  add_vwstack("start", "window", -u, u, -v, v);
2287 
2288  vpHomogeneousMatrix fMit[8];
2289  get_fMi(fMit);
2290 
2291  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2292  display_scene(w44o, robotArms[0], I_, curColor);
2293 
2294  vp2jlc_matrix(fMit[0], w44o);
2295  display_scene(w44o, robotArms[1], I_, curColor);
2296 
2297  vp2jlc_matrix(fMit[2], w44o);
2298  display_scene(w44o, robotArms[2], I_, curColor);
2299 
2300  vp2jlc_matrix(fMit[3], w44o);
2301  display_scene(w44o, robotArms[3], I_, curColor);
2302 
2303  vp2jlc_matrix(fMit[4], w44o);
2304  display_scene(w44o, robotArms[4], I_, curColor);
2305 
2306  vp2jlc_matrix(fMit[5], w44o);
2307  display_scene(w44o, robotArms[5], I_, curColor);
2308 
2309  if (displayCamera) {
2310  vpHomogeneousMatrix cMe;
2311  get_cMe(cMe);
2312  cMe = cMe.inverse();
2313  cMe = fMit[6] * cMe;
2314  vp2jlc_matrix(cMe, w44o);
2315  display_scene(w44o, camera, I_, camColor);
2316  }
2317 
2318  if (displayObject) {
2319  vp2jlc_matrix(fMo, w44o);
2320  display_scene(w44o, scene, I_, curColor);
2321  }
2322  m_mutex_scene.unlock();
2323 }
2324 
2343 {
2344  vpColVector stop(6);
2345  bool status = true;
2346  stop = 0;
2347  m_mutex_scene.lock();
2348  set_artVel(stop);
2349  set_velocity(stop);
2350  vpHomogeneousMatrix fMc_;
2351  fMc_ = fMo * cMo_.inverse();
2352 
2353  vpColVector articularCoordinates = get_artCoord();
2354  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2355 
2356  if (nbSol == 0) {
2357  status = false;
2358  vpERROR_TRACE("Positioning error. Position unreachable");
2359  }
2360 
2361  if (verbose_)
2362  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2363 
2364  set_artCoord(articularCoordinates);
2365 
2366  compute_fMi();
2367  m_mutex_scene.unlock();
2368 
2369  return status;
2370 }
2371 
2386 {
2387  vpColVector stop(6);
2388  stop = 0;
2389  m_mutex_scene.lock();
2390  set_artVel(stop);
2391  set_velocity(stop);
2392  vpHomogeneousMatrix fMit[8];
2393  get_fMi(fMit);
2394  fMo = fMit[7] * cMo_;
2395  m_mutex_scene.unlock();
2396 }
2397 
2409 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2410 {
2411  // get rid of max velocity
2412  double vMax = getMaxTranslationVelocity();
2413  double wMax = getMaxRotationVelocity();
2414  setMaxTranslationVelocity(10. * vMax);
2415  setMaxRotationVelocity(10. * wMax);
2416 
2417  vpColVector v(3), w(3), vel(6);
2418  vpHomogeneousMatrix cdMc;
2419  vpTranslationVector cdTc;
2420  vpRotationMatrix cdRc;
2421  vpThetaUVector cdTUc;
2422  vpColVector err(6);
2423  err = 1.;
2424  const double lambda = 5.;
2425 
2427 
2428  unsigned int i, iter = 0;
2429  while ((iter++ < 300) && (err.frobeniusNorm() > errMax)) {
2430  double t = vpTime::measureTimeMs();
2431 
2432  // update image
2433  if (Iint != nullptr) {
2434  vpDisplay::display(*Iint);
2435  getInternalView(*Iint);
2436  vpDisplay::flush(*Iint);
2437  }
2438 
2439  // update pose error
2440  cdMc = cdMo_ * get_cMo().inverse();
2441  cdMc.extract(cdRc);
2442  cdMc.extract(cdTc);
2443  cdTUc.buildFrom(cdRc);
2444 
2445  // compute v,w and velocity
2446  v = -lambda * cdRc.t() * cdTc;
2447  w = -lambda * cdTUc;
2448  for (i = 0; i < 3; ++i) {
2449  vel[i] = v[i];
2450  vel[i + 3] = w[i];
2451  err[i] = cdTc[i];
2452  err[i + 3] = cdTUc[i];
2453  }
2454 
2455  // update feat
2457 
2458  // wait for it
2459  vpTime::wait(t, 10);
2460  }
2461  vel = 0.;
2462  set_velocity(vel);
2463  set_artVel(vel);
2465  setMaxRotationVelocity(wMax);
2466 
2467  // std::cout << "setPosition: final error " << err.t() << std::endl;
2468  return (err.frobeniusNorm() <= errMax);
2469 }
2470 
2471 #elif !defined(VISP_BUILD_SHARED_LIBS)
2472 // Work around to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2473 // no symbols
2474 void dummy_vpSimulatorAfma6() { };
2475 #endif
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:76
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:99
double _joint_min[6]
Definition: vpAfma6.h:201
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:191
vpRxyzVector _erc
Definition: vpAfma6.h:204
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:599
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:893
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:191
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:166
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:104
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:206
double _long_56
Definition: vpAfma6.h:199
vpTranslationVector _etc
Definition: vpAfma6.h:203
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:780
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:212
double _joint_max[6]
Definition: vpAfma6.h:200
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:937
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1007
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:123
@ TOOL_CCMOP
Definition: vpAfma6.h:124
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:127
@ TOOL_CUSTOM
Definition: vpAfma6.h:129
@ TOOL_VACUUM
Definition: vpAfma6.h:126
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:128
@ TOOL_GRIPPER
Definition: vpAfma6.h:125
unsigned int getRows() const
Definition: vpArray2D.h:284
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
static const vpColor none
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:214
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
void track(const vpHomogeneousMatrix &cMo)
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
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:184
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2377
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:818
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:748
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2286
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:461
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:459
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
void set_velocity(const vpColVector &vel)
static void launcher(vpRobotWireFrameSimulator &simulator)
void set_displayBusy(const bool &status)
void getInternalView(vpImage< vpRGBa > &I)
vpHomogeneousMatrix getExternalCameraPosition() const
void set_artCoord(const vpColVector &coord)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:181
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:257
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:204
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:236
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setCameraParameters(const vpCameraParameters &cam)
void get_eJe(vpMatrix &eJe) vp_override
void get_cVe(vpVelocityTwistMatrix &cVe)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
void get_fMi(vpHomogeneousMatrix *fMit) vp_override
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement) vp_override
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) vp_override
void computeArticularVelocity() vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
void findHighestPositioningSpeed(vpColVector &q)
void init() vp_override
void move(const char *filename)
static const double defaultPositioningVelocity
bool singularityTest(const vpColVector &q, vpMatrix &J)
void updateArticularPosition() vp_override
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
virtual ~vpSimulatorAfma6() vp_override
void get_fJe(vpMatrix &fJe) vp_override
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
double getPositioningVelocity(void)
int isInJointLimit() vp_override
void get_cMe(vpHomogeneousMatrix &cMe)
void initArms() vp_override
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()