Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpSimulatorAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Class which provides a simulator for the robot Afma6.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 #include <string>
44 #include <visp3/core/vpImagePoint.h>
45 #include <visp3/core/vpIoTools.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPoint.h>
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotException.h>
50 #include <visp3/robot/vpSimulatorAfma6.h>
51 
52 #include "../wireframe-simulator/vpBound.h"
53 #include "../wireframe-simulator/vpRfstack.h"
54 #include "../wireframe-simulator/vpScene.h"
55 #include "../wireframe-simulator/vpVwstack.h"
56 
58 
63  : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true),
64  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
65 {
66  init();
67  initDisplay();
68 
70 
71 #if defined(_WIN32)
72  DWORD dwThreadIdArray;
73  hThread = CreateThread(NULL, // default security attributes
74  0, // use default stack size
75  launcher, // thread function name
76  this, // argument to thread function
77  0, // use default creation flags
78  &dwThreadIdArray); // returns the thread identifier
79 #elif defined(VISP_HAVE_PTHREAD)
80  pthread_attr_init(&attr);
81  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
82 
83  pthread_create(&thread, NULL, launcher, (void *)this);
84 #endif
85 
86  compute_fMi();
87 }
88 
96  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
97  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
98 {
99  init();
100  initDisplay();
101 
103 
104 #if defined(_WIN32)
105  DWORD dwThreadIdArray;
106  hThread = CreateThread(NULL, // default security attributes
107  0, // use default stack size
108  launcher, // thread function name
109  this, // argument to thread function
110  0, // use default creation flags
111  &dwThreadIdArray); // returns the thread identifier
112 #elif defined(VISP_HAVE_PTHREAD)
113  pthread_attr_init(&attr);
114  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
115 
116  pthread_create(&thread, NULL, launcher, (void *)this);
117 #endif
118 
119  compute_fMi();
120 }
121 
126 {
128  robotStop = true;
130 
131 #if defined(_WIN32)
132 #if defined(WINRT_8_1)
133  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
134 #else // pure win32
135  WaitForSingleObject(hThread, INFINITE);
136 #endif
137  CloseHandle(hThread);
138 #elif defined(VISP_HAVE_PTHREAD)
139  pthread_attr_destroy(&attr);
140  pthread_join(thread, NULL);
141 #endif
142 
143  if (robotArms != NULL) {
144  for (int i = 0; i < 6; i++)
145  free_Bound_scene(&(robotArms[i]));
146  }
147 
148  delete[] robotArms;
149  delete[] fMi;
150 }
151 
161 {
162  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
163  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
164  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
165  bool armDirExists = false;
166  for (size_t i = 0; i < arm_dirs.size(); i++)
167  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
168  arm_dir = arm_dirs[i];
169  armDirExists = true;
170  break;
171  }
172  if (!armDirExists) {
173  try {
174  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
175  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
176  } catch (...) {
177  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
178  }
179  }
180 
181  this->init(vpAfma6::TOOL_CCMOP);
182  toolCustom = false;
183 
184  size_fMi = 8;
185  fMi = new vpHomogeneousMatrix[8];
188 
189  zeroPos.resize(njoint);
190  zeroPos = 0;
191  reposPos.resize(njoint);
192  reposPos = 0;
193  reposPos[1] = -M_PI / 2;
194  reposPos[2] = M_PI;
195  reposPos[4] = M_PI / 2;
196 
197  artCoord = zeroPos;
198  artVel = 0;
199 
200  q_prev_getdis.resize(njoint);
201  q_prev_getdis = 0;
202  first_time_getdis = true;
203 
204  positioningVelocity = defaultPositioningVelocity;
205 
208 
209  // Software joint limits in radians
210  //_joint_min.resize(njoint);
211  _joint_min[0] = -0.6501;
212  _joint_min[1] = -0.6001;
213  _joint_min[2] = -0.5001;
214  _joint_min[3] = -2.7301;
215  _joint_min[4] = -0.1001;
216  _joint_min[5] = -1.5901;
217  //_joint_max.resize(njoint);
218  _joint_max[0] = 0.7001;
219  _joint_max[1] = 0.5201;
220  _joint_max[2] = 0.4601;
221  _joint_max[3] = 2.7301;
222  _joint_max[4] = 2.4801;
223  _joint_max[5] = 1.5901;
224 }
225 
230 {
231  robotArms = NULL;
232  robotArms = new Bound_scene[6];
233  initArms();
235  vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
236  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
238  vpCameraParameters tmp;
239  getCameraParameters(tmp, 640, 480);
240  px_int = tmp.get_px();
241  py_int = tmp.get_py();
242  sceneInitialized = true;
243 }
244 
261 {
262  this->projModel = proj_model;
263  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
264  if (arm_dir.size() > FILENAME_MAX)
265  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
266  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
267  if (full_length > FILENAME_MAX)
268  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
269 
270  // Use here default values of the robot constant parameters.
271  switch (tool) {
272  case vpAfma6::TOOL_CCMOP: {
273  _erc[0] = vpMath::rad(164.35); // rx
274  _erc[1] = vpMath::rad(89.64); // ry
275  _erc[2] = vpMath::rad(-73.05); // rz
276  _etc[0] = 0.0117; // tx
277  _etc[1] = 0.0033; // ty
278  _etc[2] = 0.2272; // tz
279 
280  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
281 
282  if (robotArms != NULL) {
283  while (get_displayBusy())
284  vpTime::wait(2);
285  free_Bound_scene(&(robotArms[5]));
286  char *name_arm = new char[full_length];
287  strcpy(name_arm, arm_dir.c_str());
288  strcat(name_arm, "/afma6_tool_ccmop.bnd");
289  set_scene(name_arm, robotArms + 5, 1.0);
290  set_displayBusy(false);
291  delete[] name_arm;
292  }
293  break;
294  }
295  case vpAfma6::TOOL_GRIPPER: {
296  _erc[0] = vpMath::rad(88.33); // rx
297  _erc[1] = vpMath::rad(72.07); // ry
298  _erc[2] = vpMath::rad(2.53); // rz
299  _etc[0] = 0.0783; // tx
300  _etc[1] = 0.1234; // ty
301  _etc[2] = 0.1638; // tz
302 
303  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
304 
305  if (robotArms != NULL) {
306  while (get_displayBusy())
307  vpTime::wait(2);
308  free_Bound_scene(&(robotArms[5]));
309  char *name_arm = new char[full_length];
310  strcpy(name_arm, arm_dir.c_str());
311  strcat(name_arm, "/afma6_tool_gripper.bnd");
312  set_scene(name_arm, robotArms + 5, 1.0);
313  set_displayBusy(false);
314  delete[] name_arm;
315  }
316  break;
317  }
318  case vpAfma6::TOOL_VACUUM: {
319  _erc[0] = vpMath::rad(90.40); // rx
320  _erc[1] = vpMath::rad(75.11); // ry
321  _erc[2] = vpMath::rad(0.18); // rz
322  _etc[0] = 0.0038; // tx
323  _etc[1] = 0.1281; // ty
324  _etc[2] = 0.1658; // tz
325 
326  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
327 
328  if (robotArms != NULL) {
329  while (get_displayBusy())
330  vpTime::wait(2);
331  free_Bound_scene(&(robotArms[5]));
332 
333  char *name_arm = new char[full_length];
334 
335  strcpy(name_arm, arm_dir.c_str());
336  strcat(name_arm, "/afma6_tool_vacuum.bnd");
337  set_scene(name_arm, robotArms + 5, 1.0);
338  set_displayBusy(false);
339  delete[] name_arm;
340  }
341  break;
342  }
343  case vpAfma6::TOOL_CUSTOM: {
344  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
345  break;
346  }
348  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
349  break;
350  }
352  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
353  break;
354  }
355  }
356 
357  vpRotationMatrix eRc(_erc);
358 
359  m_mutex_eMc.lock();
360  this->_eMc.buildFrom(_etc, eRc);
362 
363  setToolType(tool);
364  return;
365 }
366 
377 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
378  const unsigned int &image_height)
379 {
380  if (toolCustom) {
381  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
382  }
383  // Set default parameters
384  switch (getToolType()) {
385  case vpAfma6::TOOL_CCMOP: {
386  // Set default intrinsic camera parameters for 640x480 images
387  if (image_width == 640 && image_height == 480) {
388  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
389  << std::endl;
390  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
391  } else {
392  vpTRACE("Cannot get default intrinsic camera parameters for this image "
393  "resolution");
394  }
395  break;
396  }
397  case vpAfma6::TOOL_GRIPPER: {
398  // Set default intrinsic camera parameters for 640x480 images
399  if (image_width == 640 && image_height == 480) {
400  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
401  << std::endl;
402  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
403  } else {
404  vpTRACE("Cannot get default intrinsic camera parameters for this image "
405  "resolution");
406  }
407  break;
408  }
409  case vpAfma6::TOOL_CUSTOM: {
410  std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
411  break;
412  }
414  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
415  break;
416  }
418  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
419  break;
420  }
421  case vpAfma6::TOOL_VACUUM: {
422  std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
423  break;
424  }
425  default:
426  vpERROR_TRACE("This error should not occur!");
427  break;
428  }
429  return;
430 }
431 
441 {
442  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
443 }
444 
454 {
455  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
456 }
457 
464 {
465  px_int = cam.get_px();
466  py_int = cam.get_py();
467  toolCustom = true;
468 }
469 
475 {
476  double tcur_1 = tcur; // temporary variable used to store the last time
477  // since the last command
478  bool stop = false;
479  bool setVelocityCalled_ = false;
480  while (!stop) {
481  // Get current time
482  tprev = tcur_1;
484 
486  setVelocityCalled_ = setVelocityCalled;
488 
489  if (setVelocityCalled_ || !constantSamplingTimeMode) {
491  setVelocityCalled = false;
493 
495 
496  double ellapsedTime = (tcur - tprev) * 1e-3;
497  if (constantSamplingTimeMode) { // if we want a constant velocity, we
498  // force the ellapsed time to the given
499  // samplingTime
500  ellapsedTime = getSamplingTime(); // in second
501  }
502 
503  vpColVector articularCoordinates = get_artCoord();
504  vpColVector articularVelocities = get_artVel();
505 
506  if (jointLimit) {
507  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
508  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
509  if (verbose_) {
510  std::cout << "Joint " << jointLimitArt - 1
511  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
512  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
513  }
514 
515  articularVelocities = 0.0;
516  } else
517  jointLimit = false;
518  }
519 
520  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
521  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
522  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
523  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
524  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
525  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
526 
527  int jl = isInJointLimit();
528 
529  if (jl != 0 && jointLimit == false) {
530  if (jl < 0)
531  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
532  (articularVelocities[(unsigned int)(-jl - 1)]);
533  else
534  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
535  (articularVelocities[(unsigned int)(jl - 1)]);
536 
537  for (unsigned int i = 0; i < 6; i++)
538  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
539 
540  jointLimit = true;
541  jointLimitArt = (unsigned int)fabs((double)jl);
542  }
543 
544  set_artCoord(articularCoordinates);
545  set_artVel(articularVelocities);
546 
547  compute_fMi();
548 
549  if (displayAllowed) {
553  }
554 
555  if (displayType == MODEL_3D && displayAllowed) {
556  while (get_displayBusy()) {
557  vpTime::wait(2);
558  }
560  set_displayBusy(false);
561  }
562 
563  if (displayType == MODEL_DH && displayAllowed) {
564  vpHomogeneousMatrix fMit[8];
565  get_fMi(fMit);
566 
567  // vpDisplay::displayFrame(I,getExternalCameraPosition
568  // ()*fMi[6],cameraParam,0.2,vpColor::none);
569 
570  vpImagePoint iP, iP_1;
571  vpPoint pt(0, 0, 0);
572 
575  pt.track(getExternalCameraPosition() * fMit[0]);
578  for (unsigned int k = 1; k < 7; k++) {
579  pt.track(getExternalCameraPosition() * fMit[k - 1]);
581 
582  pt.track(getExternalCameraPosition() * fMit[k]);
584 
586  }
588  thickness_);
589  }
590 
592 
593  vpTime::wait(tcur, 1000 * getSamplingTime());
594  tcur_1 = tcur;
595  } else {
597  }
598 
600  stop = robotStop;
602  }
603 }
604 
619 {
620  // vpColVector q = get_artCoord();
621  vpColVector q(6); //; = get_artCoord();
622  q = get_artCoord();
623 
624  vpHomogeneousMatrix fMit[8];
625 
626  double q1 = q[0];
627  double q2 = q[1];
628  double q3 = q[2];
629  double q4 = q[3];
630  double q5 = q[4];
631  double q6 = q[5];
632 
633  double c4 = cos(q4);
634  double s4 = sin(q4);
635  double c5 = cos(q5);
636  double s5 = sin(q5);
637  double c6 = cos(q6);
638  double s6 = sin(q6);
639 
640  fMit[0][0][0] = 1;
641  fMit[0][1][0] = 0;
642  fMit[0][2][0] = 0;
643  fMit[0][0][1] = 0;
644  fMit[0][1][1] = 1;
645  fMit[0][2][1] = 0;
646  fMit[0][0][2] = 0;
647  fMit[0][1][2] = 0;
648  fMit[0][2][2] = 1;
649  fMit[0][0][3] = q1;
650  fMit[0][1][3] = 0;
651  fMit[0][2][3] = 0;
652 
653  fMit[1][0][0] = 1;
654  fMit[1][1][0] = 0;
655  fMit[1][2][0] = 0;
656  fMit[1][0][1] = 0;
657  fMit[1][1][1] = 1;
658  fMit[1][2][1] = 0;
659  fMit[1][0][2] = 0;
660  fMit[1][1][2] = 0;
661  fMit[1][2][2] = 1;
662  fMit[1][0][3] = q1;
663  fMit[1][1][3] = q2;
664  fMit[1][2][3] = 0;
665 
666  fMit[2][0][0] = 1;
667  fMit[2][1][0] = 0;
668  fMit[2][2][0] = 0;
669  fMit[2][0][1] = 0;
670  fMit[2][1][1] = 1;
671  fMit[2][2][1] = 0;
672  fMit[2][0][2] = 0;
673  fMit[2][1][2] = 0;
674  fMit[2][2][2] = 1;
675  fMit[2][0][3] = q1;
676  fMit[2][1][3] = q2;
677  fMit[2][2][3] = q3;
678 
679  fMit[3][0][0] = s4;
680  fMit[3][1][0] = -c4;
681  fMit[3][2][0] = 0;
682  fMit[3][0][1] = c4;
683  fMit[3][1][1] = s4;
684  fMit[3][2][1] = 0;
685  fMit[3][0][2] = 0;
686  fMit[3][1][2] = 0;
687  fMit[3][2][2] = 1;
688  fMit[3][0][3] = q1;
689  fMit[3][1][3] = q2;
690  fMit[3][2][3] = q3;
691 
692  fMit[4][0][0] = s4 * s5;
693  fMit[4][1][0] = -c4 * s5;
694  fMit[4][2][0] = c5;
695  fMit[4][0][1] = s4 * c5;
696  fMit[4][1][1] = -c4 * c5;
697  fMit[4][2][1] = -s5;
698  fMit[4][0][2] = c4;
699  fMit[4][1][2] = s4;
700  fMit[4][2][2] = 0;
701  fMit[4][0][3] = c4 * this->_long_56 + q1;
702  fMit[4][1][3] = s4 * this->_long_56 + q2;
703  fMit[4][2][3] = q3;
704 
705  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
706  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
707  fMit[5][2][0] = c5 * c6;
708  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
709  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
710  fMit[5][2][1] = -c5 * s6;
711  fMit[5][0][2] = -s4 * c5;
712  fMit[5][1][2] = c4 * c5;
713  fMit[5][2][2] = s5;
714  fMit[5][0][3] = c4 * this->_long_56 + q1;
715  fMit[5][1][3] = s4 * this->_long_56 + q2;
716  fMit[5][2][3] = q3;
717 
718  fMit[6][0][0] = fMit[5][0][0];
719  fMit[6][1][0] = fMit[5][1][0];
720  fMit[6][2][0] = fMit[5][2][0];
721  fMit[6][0][1] = fMit[5][0][1];
722  fMit[6][1][1] = fMit[5][1][1];
723  fMit[6][2][1] = fMit[5][2][1];
724  fMit[6][0][2] = fMit[5][0][2];
725  fMit[6][1][2] = fMit[5][1][2];
726  fMit[6][2][2] = fMit[5][2][2];
727  fMit[6][0][3] = fMit[5][0][3];
728  fMit[6][1][3] = fMit[5][1][3];
729  fMit[6][2][3] = fMit[5][2][3];
730 
731  // vpHomogeneousMatrix cMe;
732  // get_cMe(cMe);
733  // cMe = cMe.inverse();
734  // fMit[7] = fMit[6] * cMe;
735  m_mutex_eMc.lock();
736  vpAfma6::get_fMc(q, fMit[7]);
738 
739  m_mutex_fMi.lock();
740  for (int i = 0; i < 8; i++) {
741  fMi[i] = fMit[i];
742  }
744 }
745 
752 {
753  switch (newState) {
754  case vpRobot::STATE_STOP: {
755  // Start primitive STOP only if the current state is Velocity
757  stopMotion();
758  }
759  break;
760  }
763  std::cout << "Change the control mode from velocity to position control.\n";
764  stopMotion();
765  } else {
766  // std::cout << "Change the control mode from stop to position
767  // control.\n";
768  }
769  break;
770  }
773  std::cout << "Change the control mode from stop to velocity control.\n";
774  }
775  break;
776  }
778  default:
779  break;
780  }
781 
782  return vpRobot::setRobotState(newState);
783 }
784 
859 {
861  vpERROR_TRACE("Cannot send a velocity to the robot "
862  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
864  "Cannot send a velocity to the robot "
865  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
866  }
867 
868  vpColVector vel_sat(6);
869 
870  double scale_sat = 1;
871  double vel_trans_max = getMaxTranslationVelocity();
872  double vel_rot_max = getMaxRotationVelocity();
873 
874  double vel_abs; // Absolute value
875 
876  // Velocity saturation
877  switch (frame) {
878  // saturation in cartesian space
881  if (vel.getRows() != 6) {
882  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
883  throw;
884  }
885 
886  for (unsigned int i = 0; i < 3; ++i) {
887  vel_abs = fabs(vel[i]);
888  if (vel_abs > vel_trans_max && !jointLimit) {
889  vel_trans_max = vel_abs;
890  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
891  "(axis nr. %d).",
892  vel[i], i + 1);
893  }
894 
895  vel_abs = fabs(vel[i + 3]);
896  if (vel_abs > vel_rot_max && !jointLimit) {
897  vel_rot_max = vel_abs;
898  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
899  "(axis nr. %d).",
900  vel[i + 3], i + 4);
901  }
902  }
903 
904  double scale_trans_sat = 1;
905  double scale_rot_sat = 1;
906  if (vel_trans_max > getMaxTranslationVelocity())
907  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
908 
909  if (vel_rot_max > getMaxRotationVelocity())
910  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
911 
912  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
913  if (scale_trans_sat < scale_rot_sat)
914  scale_sat = scale_trans_sat;
915  else
916  scale_sat = scale_rot_sat;
917  }
918  break;
919  }
920 
921  // saturation in joint space
923  if (vel.getRows() != 6) {
924  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
925  throw;
926  }
927  for (unsigned int i = 0; i < 6; ++i) {
928  vel_abs = fabs(vel[i]);
929  if (vel_abs > vel_rot_max && !jointLimit) {
930  vel_rot_max = vel_abs;
931  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
932  "(axis nr. %d).",
933  vel[i], i + 1);
934  }
935  }
936  double scale_rot_sat = 1;
937  if (vel_rot_max > getMaxRotationVelocity())
938  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
939  if (scale_rot_sat < 1)
940  scale_sat = scale_rot_sat;
941  break;
942  }
943  case vpRobot::MIXT_FRAME: {
944  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
945  "functionality not implemented");
946  }
948  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOR_FRAME frame:"
949  "functionality not implemented");
950  }
951  }
952 
953  set_velocity(vel * scale_sat);
954 
956  setRobotFrame(frame);
958 
960  setVelocityCalled = true;
962 }
963 
968 {
970 
972  frame = getRobotFrame();
974 
975  double vel_rot_max = getMaxRotationVelocity();
976 
977  vpColVector articularCoordinates = get_artCoord();
978  vpColVector velocityframe = get_velocity();
979  vpColVector articularVelocity;
980 
981  switch (frame) {
982  case vpRobot::CAMERA_FRAME: {
983  vpMatrix eJe_;
985  vpAfma6::get_eJe(articularCoordinates, eJe_);
986  eJe_ = eJe_.pseudoInverse();
988  singularityTest(articularCoordinates, eJe_);
989  articularVelocity = eJe_ * eVc * velocityframe;
990  set_artVel(articularVelocity);
991  break;
992  }
994  vpMatrix fJe_;
995  vpAfma6::get_fJe(articularCoordinates, fJe_);
996  fJe_ = fJe_.pseudoInverse();
998  singularityTest(articularCoordinates, fJe_);
999  articularVelocity = fJe_ * velocityframe;
1000  set_artVel(articularVelocity);
1001  break;
1002  }
1003  case vpRobot::ARTICULAR_FRAME: {
1004  articularVelocity = velocityframe;
1005  set_artVel(articularVelocity);
1006  break;
1007  }
1009  case vpRobot::MIXT_FRAME: {
1010  break;
1011  }
1012  }
1013 
1014  switch (frame) {
1015  case vpRobot::CAMERA_FRAME:
1016  case vpRobot::REFERENCE_FRAME: {
1017  for (unsigned int i = 0; i < 6; ++i) {
1018  double vel_abs = fabs(articularVelocity[i]);
1019  if (vel_abs > vel_rot_max && !jointLimit) {
1020  vel_rot_max = vel_abs;
1021  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1022  "(axis nr. %d).",
1023  articularVelocity[i], i + 1);
1024  }
1025  }
1026  double scale_rot_sat = 1;
1027  double scale_sat = 1;
1028  if (vel_rot_max > getMaxRotationVelocity())
1029  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1030  if (scale_rot_sat < 1)
1031  scale_sat = scale_rot_sat;
1032 
1033  set_artVel(articularVelocity * scale_sat);
1034  break;
1035  }
1038  case vpRobot::MIXT_FRAME: {
1039  break;
1040  }
1041  }
1042 }
1043 
1090 {
1091  vel.resize(6);
1092 
1093  vpColVector articularCoordinates = get_artCoord();
1094  vpColVector articularVelocity = get_artVel();
1095 
1096  switch (frame) {
1097  case vpRobot::CAMERA_FRAME: {
1098  vpMatrix eJe_;
1100  vpAfma6::get_eJe(articularCoordinates, eJe_);
1101  vel = cVe * eJe_ * articularVelocity;
1102  break;
1103  }
1104  case vpRobot::ARTICULAR_FRAME: {
1105  vel = articularVelocity;
1106  break;
1107  }
1108  case vpRobot::REFERENCE_FRAME: {
1109  vpMatrix fJe_;
1110  vpAfma6::get_fJe(articularCoordinates, fJe_);
1111  vel = fJe_ * articularVelocity;
1112  break;
1113  }
1115  case vpRobot::MIXT_FRAME: {
1116  break;
1117  }
1118  default: {
1119  vpERROR_TRACE("Error in spec of vpRobot. "
1120  "Case not taken in account.");
1121  return;
1122  }
1123  }
1124 }
1125 
1143 {
1144  timestamp = vpTime::measureTimeSecond();
1145  getVelocity(frame, vel);
1146 }
1147 
1190 {
1191  vpColVector vel(6);
1192  getVelocity(frame, vel);
1193 
1194  return vel;
1195 }
1196 
1210 {
1211  timestamp = vpTime::measureTimeSecond();
1212  vpColVector vel(6);
1213  getVelocity(frame, vel);
1214 
1215  return vel;
1216 }
1217 
1219 {
1220  double vel_rot_max = getMaxRotationVelocity();
1221  double velmax = fabs(q[0]);
1222  for (unsigned int i = 1; i < 6; i++) {
1223  if (velmax < fabs(q[i]))
1224  velmax = fabs(q[i]);
1225  }
1226 
1227  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1228  q = q * alpha;
1229 }
1230 
1306 {
1308  vpERROR_TRACE("Robot was not in position-based control\n"
1309  "Modification of the robot state");
1310  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1311  }
1312 
1313  vpColVector articularCoordinates = get_artCoord();
1314 
1315  vpColVector error(6);
1316  double errsqr = 0;
1317  switch (frame) {
1318  case vpRobot::CAMERA_FRAME: {
1319  int nbSol;
1320  vpColVector qdes(6);
1321 
1322  vpTranslationVector txyz;
1323  vpRxyzVector rxyz;
1324  for (unsigned int i = 0; i < 3; i++) {
1325  txyz[i] = q[i];
1326  rxyz[i] = q[i + 3];
1327  }
1328 
1329  vpRotationMatrix cRc2(rxyz);
1330  vpHomogeneousMatrix cMc2(txyz, cRc2);
1331 
1332  vpHomogeneousMatrix fMc_;
1333  vpAfma6::get_fMc(articularCoordinates, fMc_);
1334 
1335  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1336 
1337  do {
1338  articularCoordinates = get_artCoord();
1339  qdes = articularCoordinates;
1340  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1341 
1343  setVelocityCalled = true;
1345 
1346  if (nbSol > 0) {
1347  error = qdes - articularCoordinates;
1348  errsqr = error.sumSquare();
1349  // findHighestPositioningSpeed(error);
1350  set_artVel(error);
1351  if (errsqr < 1e-4) {
1352  set_artCoord(qdes);
1353  error = 0;
1354  set_artVel(error);
1355  set_velocity(error);
1356  break;
1357  }
1358  } else {
1359  vpERROR_TRACE("Positioning error.");
1360  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1361  }
1362  } while (errsqr > 1e-8 && nbSol > 0);
1363 
1364  break;
1365  }
1366 
1367  case vpRobot::ARTICULAR_FRAME: {
1368  do {
1369  articularCoordinates = get_artCoord();
1370  error = q - articularCoordinates;
1371  errsqr = error.sumSquare();
1372  // findHighestPositioningSpeed(error);
1373  set_artVel(error);
1375  setVelocityCalled = true;
1377  if (errsqr < 1e-4) {
1378  set_artCoord(q);
1379  error = 0;
1380  set_artVel(error);
1381  set_velocity(error);
1382  break;
1383  }
1384  } while (errsqr > 1e-8);
1385  break;
1386  }
1387 
1388  case vpRobot::REFERENCE_FRAME: {
1389  int nbSol;
1390  vpColVector qdes(6);
1391 
1392  vpTranslationVector txyz;
1393  vpRxyzVector rxyz;
1394  for (unsigned int i = 0; i < 3; i++) {
1395  txyz[i] = q[i];
1396  rxyz[i] = q[i + 3];
1397  }
1398 
1399  vpRotationMatrix fRc(rxyz);
1400  vpHomogeneousMatrix fMc_(txyz, fRc);
1401 
1402  do {
1403  articularCoordinates = get_artCoord();
1404  qdes = articularCoordinates;
1405  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1407  setVelocityCalled = true;
1409  if (nbSol > 0) {
1410  error = qdes - articularCoordinates;
1411  errsqr = error.sumSquare();
1412  // findHighestPositioningSpeed(error);
1413  set_artVel(error);
1414  if (errsqr < 1e-4) {
1415  set_artCoord(qdes);
1416  error = 0;
1417  set_artVel(error);
1418  set_velocity(error);
1419  break;
1420  }
1421  } else
1422  vpERROR_TRACE("Positioning error. Position unreachable");
1423  } while (errsqr > 1e-8 && nbSol > 0);
1424  break;
1425  }
1426  case vpRobot::MIXT_FRAME: {
1427  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1428  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1429  "MIXT_FRAME not implemented.");
1430  }
1432  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1433  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1434  "END_EFFECTOR_FRAME not implemented.");
1435  }
1436  }
1437 }
1438 
1501 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1502  double pos4, double pos5, double pos6)
1503 {
1504  try {
1505  vpColVector position(6);
1506  position[0] = pos1;
1507  position[1] = pos2;
1508  position[2] = pos3;
1509  position[3] = pos4;
1510  position[4] = pos5;
1511  position[5] = pos6;
1512 
1513  setPosition(frame, position);
1514  } catch (...) {
1515  vpERROR_TRACE("Error caught");
1516  throw;
1517  }
1518 }
1519 
1554 void vpSimulatorAfma6::setPosition(const char *filename)
1555 {
1556  vpColVector q;
1557  bool ret;
1558 
1559  ret = this->readPosFile(filename, q);
1560 
1561  if (ret == false) {
1562  vpERROR_TRACE("Bad position in \"%s\"", filename);
1563  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1564  }
1567 }
1568 
1629 {
1630  q.resize(6);
1631 
1632  switch (frame) {
1633  case vpRobot::CAMERA_FRAME: {
1634  q = 0;
1635  break;
1636  }
1637 
1638  case vpRobot::ARTICULAR_FRAME: {
1639  q = get_artCoord();
1640  break;
1641  }
1642 
1643  case vpRobot::REFERENCE_FRAME: {
1644  vpHomogeneousMatrix fMc_;
1645  vpAfma6::get_fMc(get_artCoord(), fMc_);
1646 
1647  vpRotationMatrix fRc;
1648  fMc_.extract(fRc);
1649  vpRxyzVector rxyz(fRc);
1650 
1651  vpTranslationVector txyz;
1652  fMc_.extract(txyz);
1653 
1654  for (unsigned int i = 0; i < 3; i++) {
1655  q[i] = txyz[i];
1656  q[i + 3] = rxyz[i];
1657  }
1658  break;
1659  }
1660 
1661  case vpRobot::MIXT_FRAME: {
1662  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1663  "Mixt frame not implemented.");
1664  }
1666  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1667  "End-effector frame not implemented.");
1668  }
1669  }
1670 }
1671 
1699 {
1700  timestamp = vpTime::measureTimeSecond();
1701  getPosition(frame, q);
1702 }
1703 
1716 {
1717  vpColVector posRxyz;
1718  // recupere position en Rxyz
1719  this->getPosition(frame, posRxyz);
1720 
1721  // recupere le vecteur thetaU correspondant
1722  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1723 
1724  // remplit le vpPoseVector avec translation et rotation ThetaU
1725  for (unsigned int j = 0; j < 3; j++) {
1726  position[j] = posRxyz[j];
1727  position[j + 3] = RtuVect[j];
1728  }
1729 }
1730 
1742 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1743 {
1744  timestamp = vpTime::measureTimeSecond();
1745  getPosition(frame, position);
1746 }
1747 
1758 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1759 {
1760  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1761  vpTRACE("Joint limit vector has not a size of 6 !");
1762  return;
1763  }
1764 
1765  _joint_min[0] = limitMin[0];
1766  _joint_min[1] = limitMin[1];
1767  _joint_min[2] = limitMin[2];
1768  _joint_min[3] = limitMin[3];
1769  _joint_min[4] = limitMin[4];
1770  _joint_min[5] = limitMin[5];
1771 
1772  _joint_max[0] = limitMax[0];
1773  _joint_max[1] = limitMax[1];
1774  _joint_max[2] = limitMax[2];
1775  _joint_max[3] = limitMax[3];
1776  _joint_max[4] = limitMax[4];
1777  _joint_max[5] = limitMax[5];
1778 }
1779 
1786 {
1787  double q5 = q[4];
1788 
1789  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1790 
1791  if (cond) {
1792  J[0][3] = 0;
1793  J[0][4] = 0;
1794  J[1][3] = 0;
1795  J[1][4] = 0;
1796  J[3][3] = 0;
1797  J[3][4] = 0;
1798  J[5][3] = 0;
1799  J[5][4] = 0;
1800  return true;
1801  }
1802 
1803  return false;
1804 }
1805 
1810 {
1811  int artNumb = 0;
1812  double diff = 0;
1813  double difft = 0;
1814 
1815  vpColVector articularCoordinates = get_artCoord();
1816 
1817  for (unsigned int i = 0; i < 6; i++) {
1818  if (articularCoordinates[i] <= _joint_min[i]) {
1819  difft = _joint_min[i] - articularCoordinates[i];
1820  if (difft > diff) {
1821  diff = difft;
1822  artNumb = -(int)i - 1;
1823  }
1824  }
1825  }
1826 
1827  for (unsigned int i = 0; i < 6; i++) {
1828  if (articularCoordinates[i] >= _joint_max[i]) {
1829  difft = articularCoordinates[i] - _joint_max[i];
1830  if (difft > diff) {
1831  diff = difft;
1832  artNumb = (int)(i + 1);
1833  }
1834  }
1835  }
1836 
1837  if (artNumb != 0)
1838  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1839  << std::endl;
1840 
1841  return artNumb;
1842 }
1843 
1862 {
1863  displacement.resize(6);
1864  displacement = 0;
1865  vpColVector q_cur(6);
1866 
1867  q_cur = get_artCoord();
1868 
1869  if (!first_time_getdis) {
1870  switch (frame) {
1871  case vpRobot::CAMERA_FRAME: {
1872  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1873  return;
1874  }
1875  case vpRobot::ARTICULAR_FRAME: {
1876  displacement = q_cur - q_prev_getdis;
1877  break;
1878  }
1879  case vpRobot::REFERENCE_FRAME: {
1880  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1881  return;
1882  }
1883  case vpRobot::MIXT_FRAME: {
1884  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1885  return;
1886  }
1888  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1889  return;
1890  }
1891  }
1892  } else {
1893  first_time_getdis = false;
1894  }
1895 
1896  // Memorize the joint position for the next call
1897  q_prev_getdis = q_cur;
1898 }
1899 
1947 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1948 {
1949  std::ifstream fd(filename.c_str(), std::ios::in);
1950 
1951  if (!fd.is_open()) {
1952  return false;
1953  }
1954 
1955  std::string line;
1956  std::string key("R:");
1957  std::string id("#AFMA6 - Position");
1958  bool pos_found = false;
1959  int lineNum = 0;
1960 
1961  q.resize(njoint);
1962 
1963  while (std::getline(fd, line)) {
1964  lineNum++;
1965  if (lineNum == 1) {
1966  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1967  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1968  return false;
1969  }
1970  }
1971  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1972  continue;
1973  }
1974  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1975  // check if there are at least njoint values in the line
1976  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1977  if (chain.size() < njoint + 1) // try to split with tab separator
1978  chain = vpIoTools::splitChain(line, std::string("\t"));
1979  if (chain.size() < njoint + 1)
1980  continue;
1981 
1982  std::istringstream ss(line);
1983  std::string key_;
1984  ss >> key_;
1985  for (unsigned int i = 0; i < njoint; i++)
1986  ss >> q[i];
1987  pos_found = true;
1988  break;
1989  }
1990  }
1991 
1992  // converts rotations from degrees into radians
1993  q[3] = vpMath::rad(q[3]);
1994  q[4] = vpMath::rad(q[4]);
1995  q[5] = vpMath::rad(q[5]);
1996 
1997  fd.close();
1998 
1999  if (!pos_found) {
2000  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2001  return false;
2002  }
2003 
2004  return true;
2005 }
2006 
2029 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2030 {
2031  FILE *fd;
2032  fd = fopen(filename.c_str(), "w");
2033  if (fd == NULL)
2034  return false;
2035 
2036  fprintf(fd, "\
2037 #AFMA6 - Position - Version 2.01\n\
2038 #\n\
2039 # R: X Y Z A B C\n\
2040 # Joint position: X, Y, Z: translations in meters\n\
2041 # A, B, C: rotations in degrees\n\
2042 #\n\
2043 #\n\n");
2044 
2045  // Save positions in mm and deg
2046  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2047  vpMath::deg(q[5]));
2048 
2049  fclose(fd);
2050  return (true);
2051 }
2052 
2060 void vpSimulatorAfma6::move(const char *filename)
2061 {
2062  vpColVector q;
2063 
2064  try {
2065  this->readPosFile(filename, q);
2068  } catch (...) {
2069  throw;
2070  }
2071 }
2072 
2083 
2092 {
2093  vpHomogeneousMatrix cMe;
2094  vpAfma6::get_cMe(cMe);
2095 
2096  cVe.buildFrom(cMe);
2097 }
2098 
2109 {
2110  try {
2111  vpAfma6::get_eJe(get_artCoord(), eJe_);
2112  } catch (...) {
2113  vpERROR_TRACE("catch exception ");
2114  throw;
2115  }
2116 }
2117 
2129 {
2130  try {
2131  vpColVector articularCoordinates = get_artCoord();
2132  vpAfma6::get_fJe(articularCoordinates, fJe_);
2133  } catch (...) {
2134  vpERROR_TRACE("Error caught");
2135  throw;
2136  }
2137 }
2138 
2143 {
2145  return;
2146 
2147  vpColVector stop(6);
2148  stop = 0;
2149  set_artVel(stop);
2150  set_velocity(stop);
2152 }
2153 
2154 /**********************************************************************************/
2155 /**********************************************************************************/
2156 /**********************************************************************************/
2157 /**********************************************************************************/
2158 
2168 {
2169  // set scene_dir from #define VISP_SCENE_DIR if it exists
2170  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2171  std::string scene_dir_;
2172  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2173  bool sceneDirExists = false;
2174  for (size_t i = 0; i < scene_dirs.size(); i++)
2175  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2176  scene_dir_ = scene_dirs[i];
2177  sceneDirExists = true;
2178  break;
2179  }
2180  if (!sceneDirExists) {
2181  try {
2182  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2183  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2184  } catch (...) {
2185  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2186  }
2187  }
2188 
2189  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2190  if (scene_dir_.size() > FILENAME_MAX)
2191  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2192  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2193  if (full_length > FILENAME_MAX)
2194  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2195 
2196  char *name_cam = new char[full_length];
2197 
2198  strcpy(name_cam, scene_dir_.c_str());
2199  strcat(name_cam, "/camera.bnd");
2200  set_scene(name_cam, &camera, cameraFactor);
2201 
2202  if (arm_dir.size() > FILENAME_MAX)
2203  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2204  full_length = (unsigned int)arm_dir.size() + name_length;
2205  if (full_length > FILENAME_MAX)
2206  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2207 
2208  char *name_arm = new char[full_length];
2209  strcpy(name_arm, arm_dir.c_str());
2210  strcat(name_arm, "/afma6_gate.bnd");
2211  std::cout << "name arm: " << name_arm << std::endl;
2212  set_scene(name_arm, robotArms, 1.0);
2213  strcpy(name_arm, arm_dir.c_str());
2214  strcat(name_arm, "/afma6_arm1.bnd");
2215  set_scene(name_arm, robotArms + 1, 1.0);
2216  strcpy(name_arm, arm_dir.c_str());
2217  strcat(name_arm, "/afma6_arm2.bnd");
2218  set_scene(name_arm, robotArms + 2, 1.0);
2219  strcpy(name_arm, arm_dir.c_str());
2220  strcat(name_arm, "/afma6_arm3.bnd");
2221  set_scene(name_arm, robotArms + 3, 1.0);
2222  strcpy(name_arm, arm_dir.c_str());
2223  strcat(name_arm, "/afma6_arm4.bnd");
2224  set_scene(name_arm, robotArms + 4, 1.0);
2225 
2227  tool = getToolType();
2228  strcpy(name_arm, arm_dir.c_str());
2229  switch (tool) {
2230  case vpAfma6::TOOL_CCMOP: {
2231  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2232  break;
2233  }
2234  case vpAfma6::TOOL_GRIPPER: {
2235  strcat(name_arm, "/afma6_tool_gripper.bnd");
2236  break;
2237  }
2238  case vpAfma6::TOOL_VACUUM: {
2239  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2240  break;
2241  }
2242  case vpAfma6::TOOL_CUSTOM: {
2243  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2244  break;
2245  }
2247  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2248  break;
2249  }
2251  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2252  break;
2253  }
2254  }
2255  set_scene(name_arm, robotArms + 5, 1.0);
2256 
2257  add_rfstack(IS_BACK);
2258 
2259  add_vwstack("start", "depth", 0.0, 100.0);
2260  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2261  add_vwstack("start", "type", PERSPECTIVE);
2262  //
2263  // sceneInitialized = true;
2264  // displayObject = true;
2265  displayCamera = true;
2266 
2267  delete[] name_cam;
2268  delete[] name_arm;
2269 }
2270 
2272 {
2273  m_mutex_scene.lock();
2274  bool changed = false;
2275  vpHomogeneousMatrix displacement = navigation(I_, changed);
2276 
2277  // if (displacement[2][3] != 0)
2278  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2279  camMf2 = camMf2 * displacement;
2280 
2281  f2Mf = camMf2.inverse() * camMf;
2282 
2283  camMf = camMf2 * displacement * f2Mf;
2284 
2285  double u;
2286  double v;
2287  // if(px_ext != 1 && py_ext != 1)
2288  // we assume px_ext and py_ext > 0
2289  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2290  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2291  u = (double)I_.getWidth() / (2 * px_ext);
2292  v = (double)I_.getHeight() / (2 * py_ext);
2293  } else {
2294  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2295  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2296  }
2297 
2298  float w44o[4][4], w44cext[4][4], x, y, z;
2299 
2300  vp2jlc_matrix(camMf.inverse(), w44cext);
2301 
2302  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2303  x = w44cext[2][0] + w44cext[3][0];
2304  y = w44cext[2][1] + w44cext[3][1];
2305  z = w44cext[2][2] + w44cext[3][2];
2306  add_vwstack("start", "vrp", x, y, z);
2307  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2308  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2309  add_vwstack("start", "window", -u, u, -v, v);
2310 
2311  vpHomogeneousMatrix fMit[8];
2312  get_fMi(fMit);
2313 
2314  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2315  display_scene(w44o, robotArms[0], I_, curColor);
2316 
2317  vp2jlc_matrix(fMit[0], w44o);
2318  display_scene(w44o, robotArms[1], I_, curColor);
2319 
2320  vp2jlc_matrix(fMit[2], w44o);
2321  display_scene(w44o, robotArms[2], I_, curColor);
2322 
2323  vp2jlc_matrix(fMit[3], w44o);
2324  display_scene(w44o, robotArms[3], I_, curColor);
2325 
2326  vp2jlc_matrix(fMit[4], w44o);
2327  display_scene(w44o, robotArms[4], I_, curColor);
2328 
2329  vp2jlc_matrix(fMit[5], w44o);
2330  display_scene(w44o, robotArms[5], I_, curColor);
2331 
2332  if (displayCamera) {
2333  vpHomogeneousMatrix cMe;
2334  get_cMe(cMe);
2335  cMe = cMe.inverse();
2336  cMe = fMit[6] * cMe;
2337  vp2jlc_matrix(cMe, w44o);
2338  display_scene(w44o, camera, I_, camColor);
2339  }
2340 
2341  if (displayObject) {
2342  vp2jlc_matrix(fMo, w44o);
2343  display_scene(w44o, scene, I_, curColor);
2344  }
2346 }
2347 
2366 {
2367  vpColVector stop(6);
2368  bool status = true;
2369  stop = 0;
2370  m_mutex_scene.lock();
2371  set_artVel(stop);
2372  set_velocity(stop);
2373  vpHomogeneousMatrix fMc_;
2374  fMc_ = fMo * cMo_.inverse();
2375 
2376  vpColVector articularCoordinates = get_artCoord();
2377  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2378 
2379  if (nbSol == 0) {
2380  status = false;
2381  vpERROR_TRACE("Positioning error. Position unreachable");
2382  }
2383 
2384  if (verbose_)
2385  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2386 
2387  set_artCoord(articularCoordinates);
2388 
2389  compute_fMi();
2391 
2392  return status;
2393 }
2394 
2409 {
2410  vpColVector stop(6);
2411  stop = 0;
2412  m_mutex_scene.lock();
2413  set_artVel(stop);
2414  set_velocity(stop);
2415  vpHomogeneousMatrix fMit[8];
2416  get_fMi(fMit);
2417  fMo = fMit[7] * cMo_;
2419 }
2420 
2432 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2433 {
2434  // get rid of max velocity
2435  double vMax = getMaxTranslationVelocity();
2436  double wMax = getMaxRotationVelocity();
2437  setMaxTranslationVelocity(10. * vMax);
2438  setMaxRotationVelocity(10. * wMax);
2439 
2440  vpColVector v(3), w(3), vel(6);
2441  vpHomogeneousMatrix cdMc;
2442  vpTranslationVector cdTc;
2443  vpRotationMatrix cdRc;
2444  vpThetaUVector cdTUc;
2445  vpColVector err(6);
2446  err = 1.;
2447  const double lambda = 5.;
2448 
2450 
2451  unsigned int i, iter = 0;
2452  while ((iter++ < 300) && (err.frobeniusNorm() > errMax)) {
2453  double t = vpTime::measureTimeMs();
2454 
2455  // update image
2456  if (Iint != NULL) {
2457  vpDisplay::display(*Iint);
2458  getInternalView(*Iint);
2459  vpDisplay::flush(*Iint);
2460  }
2461 
2462  // update pose error
2463  cdMc = cdMo_ * get_cMo().inverse();
2464  cdMc.extract(cdRc);
2465  cdMc.extract(cdTc);
2466  cdTUc.buildFrom(cdRc);
2467 
2468  // compute v,w and velocity
2469  v = -lambda * cdRc.t() * cdTc;
2470  w = -lambda * cdTUc;
2471  for (i = 0; i < 3; ++i) {
2472  vel[i] = v[i];
2473  vel[i + 3] = w[i];
2474  err[i] = cdTc[i];
2475  err[i + 3] = cdTUc[i];
2476  }
2477 
2478  // update feat
2480 
2481  // wait for it
2482  vpTime::wait(t, 10);
2483  }
2484  vel = 0.;
2485  set_velocity(vel);
2486  set_artVel(vel);
2488  setMaxRotationVelocity(wMax);
2489 
2490  // std::cout << "setPosition: final error " << err.t() << std::endl;
2491  return (err.frobeniusNorm() <= errMax);
2492 }
2493 
2494 #elif !defined(VISP_BUILD_SHARED_LIBS)
2495 // Work around to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2496 // no symbols
2497 void dummy_vpSimulatorAfma6(){};
2498 #endif
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:79
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
double _joint_min[6]
Definition: vpAfma6.h:204
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
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:610
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:898
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
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double _long_56
Definition: vpAfma6.h:202
vpTranslationVector _etc
Definition: vpAfma6.h:206
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:785
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
double _joint_max[6]
Definition: vpAfma6.h:203
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:942
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1012
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
unsigned int getRows() const
Definition: vpArray2D.h:292
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:172
double sumSquare() const
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
static const vpColor none
Definition: vpColor.h:229
static const vpColor green
Definition: vpColor.h:220
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 emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
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:89
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1994
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:435
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:365
static double rad(double deg)
Definition: vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:170
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:178
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void unlock()
Definition: vpMutex.h:111
void lock()
Definition: vpMutex.h:95
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:194
Error that can be emited by the vpRobot class and its derivates.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
void set_velocity(const vpColVector &vel)
void set_displayBusy(const bool &status)
void getInternalView(vpImage< vpRGBa > &I)
vpHomogeneousMatrix getExternalCameraPosition() const
static void * launcher(void *arg)
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:145
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:173
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:69
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
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:184
void get_eJe(vpMatrix &eJe)
static bool savePosFile(const std::string &filename, const vpColVector &q)
void setCameraParameters(const vpCameraParameters &cam)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
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)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void get_fMi(vpHomogeneousMatrix *fMit)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void move(const char *filename)
static const double defaultPositioningVelocity
bool singularityTest(const vpColVector &q, vpMatrix &J)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void get_fJe(vpMatrix &fJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
double getPositioningVelocity(void)
void get_cMe(vpHomogeneousMatrix &cMe)
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:416
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()