Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
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(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
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 #if defined(_WIN32)
69  DWORD dwThreadIdArray;
70  hThread = CreateThread(NULL, // default security attributes
71  0, // use default stack size
72  launcher, // thread function name
73  this, // argument to thread function
74  0, // use default creation flags
75  &dwThreadIdArray); // returns the thread identifier
76 #elif defined(VISP_HAVE_PTHREAD)
77  pthread_attr_init(&attr);
78  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
79 
80  pthread_create(&thread, NULL, launcher, (void *)this);
81 #endif
82 
83  compute_fMi();
84 }
85 
93  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
94  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
95 {
96  init();
97  initDisplay();
98 
100 
101 #if defined(_WIN32)
102  DWORD dwThreadIdArray;
103  hThread = CreateThread(NULL, // default security attributes
104  0, // use default stack size
105  launcher, // thread function name
106  this, // argument to thread function
107  0, // use default creation flags
108  &dwThreadIdArray); // returns the thread identifier
109 #elif defined(VISP_HAVE_PTHREAD)
110  pthread_attr_init(&attr);
111  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
112 
113  pthread_create(&thread, NULL, launcher, (void *)this);
114 #endif
115 
116  compute_fMi();
117 }
118 
123 {
125  robotStop = true;
127 
128 #if defined(_WIN32)
129 #if defined(WINRT_8_1)
130  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
131 #else // pure win32
132  WaitForSingleObject(hThread, INFINITE);
133 #endif
134  CloseHandle(hThread);
135 #elif defined(VISP_HAVE_PTHREAD)
136  pthread_attr_destroy(&attr);
137  pthread_join(thread, NULL);
138 #endif
139 
140  if (robotArms != NULL) {
141  for (int i = 0; i < 6; i++)
142  free_Bound_scene(&(robotArms[i]));
143  }
144 
145  delete[] robotArms;
146  delete[] fMi;
147 }
148 
158 {
159  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
160  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
161  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
162  bool armDirExists = false;
163  for (size_t i = 0; i < arm_dirs.size(); i++)
164  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
165  arm_dir = arm_dirs[i];
166  armDirExists = true;
167  break;
168  }
169  if (!armDirExists) {
170  try {
171  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
172  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
173  } catch (...) {
174  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
175  }
176  }
177 
178  this->init(vpAfma6::TOOL_CCMOP);
179  toolCustom = false;
180 
181  size_fMi = 8;
182  fMi = new vpHomogeneousMatrix[8];
185 
186  zeroPos.resize(njoint);
187  zeroPos = 0;
188  reposPos.resize(njoint);
189  reposPos = 0;
190  reposPos[1] = -M_PI / 2;
191  reposPos[2] = M_PI;
192  reposPos[4] = M_PI / 2;
193 
194  artCoord = zeroPos;
195  artVel = 0;
196 
197  q_prev_getdis.resize(njoint);
198  q_prev_getdis = 0;
199  first_time_getdis = true;
200 
201  positioningVelocity = defaultPositioningVelocity;
202 
205 
206  // Software joint limits in radians
207  //_joint_min.resize(njoint);
208  _joint_min[0] = -0.6501;
209  _joint_min[1] = -0.6001;
210  _joint_min[2] = -0.5001;
211  _joint_min[3] = -2.7301;
212  _joint_min[4] = -0.1001;
213  _joint_min[5] = -1.5901;
214  //_joint_max.resize(njoint);
215  _joint_max[0] = 0.7001;
216  _joint_max[1] = 0.5201;
217  _joint_max[2] = 0.4601;
218  _joint_max[3] = 2.7301;
219  _joint_max[4] = 2.4801;
220  _joint_max[5] = 1.5901;
221 }
222 
227 {
228  robotArms = NULL;
229  robotArms = new Bound_scene[6];
230  initArms();
232  vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
233  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
235  vpCameraParameters tmp;
236  getCameraParameters(tmp, 640, 480);
237  px_int = tmp.get_px();
238  py_int = tmp.get_py();
239  sceneInitialized = true;
240 }
241 
258 {
259  this->projModel = proj_model;
260  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
261  if (arm_dir.size() > FILENAME_MAX)
262  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
263  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
264  if (full_length > FILENAME_MAX)
265  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
266 
267  // Use here default values of the robot constant parameters.
268  switch (tool) {
269  case vpAfma6::TOOL_CCMOP: {
270  _erc[0] = vpMath::rad(164.35); // rx
271  _erc[1] = vpMath::rad(89.64); // ry
272  _erc[2] = vpMath::rad(-73.05); // rz
273  _etc[0] = 0.0117; // tx
274  _etc[1] = 0.0033; // ty
275  _etc[2] = 0.2272; // tz
276 
277  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
278 
279  if (robotArms != NULL) {
280  while (get_displayBusy())
281  vpTime::wait(2);
282  free_Bound_scene(&(robotArms[5]));
283  char *name_arm = new char[full_length];
284  strcpy(name_arm, arm_dir.c_str());
285  strcat(name_arm, "/afma6_tool_ccmop.bnd");
286  set_scene(name_arm, robotArms + 5, 1.0);
287  set_displayBusy(false);
288  delete[] name_arm;
289  }
290  break;
291  }
292  case vpAfma6::TOOL_GRIPPER: {
293  _erc[0] = vpMath::rad(88.33); // rx
294  _erc[1] = vpMath::rad(72.07); // ry
295  _erc[2] = vpMath::rad(2.53); // rz
296  _etc[0] = 0.0783; // tx
297  _etc[1] = 0.1234; // ty
298  _etc[2] = 0.1638; // tz
299 
300  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
301 
302  if (robotArms != NULL) {
303  while (get_displayBusy())
304  vpTime::wait(2);
305  free_Bound_scene(&(robotArms[5]));
306  char *name_arm = new char[full_length];
307  strcpy(name_arm, arm_dir.c_str());
308  strcat(name_arm, "/afma6_tool_gripper.bnd");
309  set_scene(name_arm, robotArms + 5, 1.0);
310  set_displayBusy(false);
311  delete[] name_arm;
312  }
313  break;
314  }
315  case vpAfma6::TOOL_VACUUM: {
316  _erc[0] = vpMath::rad(90.40); // rx
317  _erc[1] = vpMath::rad(75.11); // ry
318  _erc[2] = vpMath::rad(0.18); // rz
319  _etc[0] = 0.0038; // tx
320  _etc[1] = 0.1281; // ty
321  _etc[2] = 0.1658; // tz
322 
323  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
324 
325  if (robotArms != NULL) {
326  while (get_displayBusy())
327  vpTime::wait(2);
328  free_Bound_scene(&(robotArms[5]));
329 
330  char *name_arm = new char[full_length];
331 
332  strcpy(name_arm, arm_dir.c_str());
333  strcat(name_arm, "/afma6_tool_vacuum.bnd");
334  set_scene(name_arm, robotArms + 5, 1.0);
335  set_displayBusy(false);
336  delete[] name_arm;
337  }
338  break;
339  }
340  case vpAfma6::TOOL_CUSTOM: {
341  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
342  break;
343  }
345  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
346  break;
347  }
349  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
350  break;
351  }
352  }
353 
354  vpRotationMatrix eRc(_erc);
355 
356  m_mutex_eMc.lock();
357  this->_eMc.buildFrom(_etc, eRc);
359 
360  setToolType(tool);
361  return;
362 }
363 
374 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
375  const unsigned int &image_height)
376 {
377  if (toolCustom) {
378  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
379  }
380  // Set default parameters
381  switch (getToolType()) {
382  case vpAfma6::TOOL_CCMOP: {
383  // Set default intrinsic camera parameters for 640x480 images
384  if (image_width == 640 && image_height == 480) {
385  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
386  << std::endl;
387  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
388  } else {
389  vpTRACE("Cannot get default intrinsic camera parameters for this image "
390  "resolution");
391  }
392  break;
393  }
394  case vpAfma6::TOOL_GRIPPER: {
395  // Set default intrinsic camera parameters for 640x480 images
396  if (image_width == 640 && image_height == 480) {
397  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
398  << std::endl;
399  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
400  } else {
401  vpTRACE("Cannot get default intrinsic camera parameters for this image "
402  "resolution");
403  }
404  break;
405  }
406  case vpAfma6::TOOL_CUSTOM: {
407  std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
408  break;
409  }
411  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
412  break;
413  }
415  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
416  break;
417  }
418  case vpAfma6::TOOL_VACUUM: {
419  std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
420  break;
421  }
422  default:
423  vpERROR_TRACE("This error should not occur!");
424  break;
425  }
426  return;
427 }
428 
438 {
439  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
440 }
441 
451 {
452  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
453 }
454 
461 {
462  px_int = cam.get_px();
463  py_int = cam.get_py();
464  toolCustom = true;
465 }
466 
472 {
473  double tcur_1 = tcur; // temporary variable used to store the last time
474  // since the last command
475  bool stop = false;
476  bool setVelocityCalled_ = false;
477  while (!stop) {
478  // Get current time
479  tprev = tcur_1;
481 
483  setVelocityCalled_ = setVelocityCalled;
485 
486  if (setVelocityCalled_ || !constantSamplingTimeMode) {
488  setVelocityCalled = false;
490 
492 
493  double ellapsedTime = (tcur - tprev) * 1e-3;
494  if (constantSamplingTimeMode) { // if we want a constant velocity, we
495  // force the ellapsed time to the given
496  // samplingTime
497  ellapsedTime = getSamplingTime(); // in second
498  }
499 
500  vpColVector articularCoordinates = get_artCoord();
501  vpColVector articularVelocities = get_artVel();
502 
503  if (jointLimit) {
504  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
505  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
506  if (verbose_) {
507  std::cout << "Joint " << jointLimitArt - 1
508  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
509  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
510  }
511 
512  articularVelocities = 0.0;
513  } else
514  jointLimit = false;
515  }
516 
517  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
518  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
519  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
520  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
521  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
522  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
523 
524  int jl = isInJointLimit();
525 
526  if (jl != 0 && jointLimit == false) {
527  if (jl < 0)
528  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
529  (articularVelocities[(unsigned int)(-jl - 1)]);
530  else
531  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
532  (articularVelocities[(unsigned int)(jl - 1)]);
533 
534  for (unsigned int i = 0; i < 6; i++)
535  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
536 
537  jointLimit = true;
538  jointLimitArt = (unsigned int)fabs((double)jl);
539  }
540 
541  set_artCoord(articularCoordinates);
542  set_artVel(articularVelocities);
543 
544  compute_fMi();
545 
546  if (displayAllowed) {
550  }
551 
552  if (displayType == MODEL_3D && displayAllowed) {
553  while (get_displayBusy()) {
554  vpTime::wait(2);
555  }
557  set_displayBusy(false);
558  }
559 
560  if (displayType == MODEL_DH && displayAllowed) {
561  vpHomogeneousMatrix fMit[8];
562  get_fMi(fMit);
563 
564  // vpDisplay::displayFrame(I,getExternalCameraPosition
565  // ()*fMi[6],cameraParam,0.2,vpColor::none);
566 
567  vpImagePoint iP, iP_1;
568  vpPoint pt(0, 0, 0);
569 
572  pt.track(getExternalCameraPosition() * fMit[0]);
575  for (unsigned int k = 1; k < 7; k++) {
576  pt.track(getExternalCameraPosition() * fMit[k - 1]);
578 
579  pt.track(getExternalCameraPosition() * fMit[k]);
581 
583  }
585  thickness_);
586  }
587 
589 
590  vpTime::wait(tcur, 1000 * getSamplingTime());
591  tcur_1 = tcur;
592  } else {
594  }
595 
597  stop = robotStop;
599  }
600 }
601 
616 {
617  // vpColVector q = get_artCoord();
618  vpColVector q(6); //; = get_artCoord();
619  q = get_artCoord();
620 
621  vpHomogeneousMatrix fMit[8];
622 
623  double q1 = q[0];
624  double q2 = q[1];
625  double q3 = q[2];
626  double q4 = q[3];
627  double q5 = q[4];
628  double q6 = q[5];
629 
630  double c4 = cos(q4);
631  double s4 = sin(q4);
632  double c5 = cos(q5);
633  double s5 = sin(q5);
634  double c6 = cos(q6);
635  double s6 = sin(q6);
636 
637  fMit[0][0][0] = 1;
638  fMit[0][1][0] = 0;
639  fMit[0][2][0] = 0;
640  fMit[0][0][1] = 0;
641  fMit[0][1][1] = 1;
642  fMit[0][2][1] = 0;
643  fMit[0][0][2] = 0;
644  fMit[0][1][2] = 0;
645  fMit[0][2][2] = 1;
646  fMit[0][0][3] = q1;
647  fMit[0][1][3] = 0;
648  fMit[0][2][3] = 0;
649 
650  fMit[1][0][0] = 1;
651  fMit[1][1][0] = 0;
652  fMit[1][2][0] = 0;
653  fMit[1][0][1] = 0;
654  fMit[1][1][1] = 1;
655  fMit[1][2][1] = 0;
656  fMit[1][0][2] = 0;
657  fMit[1][1][2] = 0;
658  fMit[1][2][2] = 1;
659  fMit[1][0][3] = q1;
660  fMit[1][1][3] = q2;
661  fMit[1][2][3] = 0;
662 
663  fMit[2][0][0] = 1;
664  fMit[2][1][0] = 0;
665  fMit[2][2][0] = 0;
666  fMit[2][0][1] = 0;
667  fMit[2][1][1] = 1;
668  fMit[2][2][1] = 0;
669  fMit[2][0][2] = 0;
670  fMit[2][1][2] = 0;
671  fMit[2][2][2] = 1;
672  fMit[2][0][3] = q1;
673  fMit[2][1][3] = q2;
674  fMit[2][2][3] = q3;
675 
676  fMit[3][0][0] = s4;
677  fMit[3][1][0] = -c4;
678  fMit[3][2][0] = 0;
679  fMit[3][0][1] = c4;
680  fMit[3][1][1] = s4;
681  fMit[3][2][1] = 0;
682  fMit[3][0][2] = 0;
683  fMit[3][1][2] = 0;
684  fMit[3][2][2] = 1;
685  fMit[3][0][3] = q1;
686  fMit[3][1][3] = q2;
687  fMit[3][2][3] = q3;
688 
689  fMit[4][0][0] = s4 * s5;
690  fMit[4][1][0] = -c4 * s5;
691  fMit[4][2][0] = c5;
692  fMit[4][0][1] = s4 * c5;
693  fMit[4][1][1] = -c4 * c5;
694  fMit[4][2][1] = -s5;
695  fMit[4][0][2] = c4;
696  fMit[4][1][2] = s4;
697  fMit[4][2][2] = 0;
698  fMit[4][0][3] = c4 * this->_long_56 + q1;
699  fMit[4][1][3] = s4 * this->_long_56 + q2;
700  fMit[4][2][3] = q3;
701 
702  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
703  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
704  fMit[5][2][0] = c5 * c6;
705  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
706  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
707  fMit[5][2][1] = -c5 * s6;
708  fMit[5][0][2] = -s4 * c5;
709  fMit[5][1][2] = c4 * c5;
710  fMit[5][2][2] = s5;
711  fMit[5][0][3] = c4 * this->_long_56 + q1;
712  fMit[5][1][3] = s4 * this->_long_56 + q2;
713  fMit[5][2][3] = q3;
714 
715  fMit[6][0][0] = fMit[5][0][0];
716  fMit[6][1][0] = fMit[5][1][0];
717  fMit[6][2][0] = fMit[5][2][0];
718  fMit[6][0][1] = fMit[5][0][1];
719  fMit[6][1][1] = fMit[5][1][1];
720  fMit[6][2][1] = fMit[5][2][1];
721  fMit[6][0][2] = fMit[5][0][2];
722  fMit[6][1][2] = fMit[5][1][2];
723  fMit[6][2][2] = fMit[5][2][2];
724  fMit[6][0][3] = fMit[5][0][3];
725  fMit[6][1][3] = fMit[5][1][3];
726  fMit[6][2][3] = fMit[5][2][3];
727 
728  // vpHomogeneousMatrix cMe;
729  // get_cMe(cMe);
730  // cMe = cMe.inverse();
731  // fMit[7] = fMit[6] * cMe;
732  m_mutex_eMc.lock();
733  vpAfma6::get_fMc(q, fMit[7]);
735 
736  m_mutex_fMi.lock();
737  for (int i = 0; i < 8; i++) {
738  fMi[i] = fMit[i];
739  }
741 }
742 
749 {
750  switch (newState) {
751  case vpRobot::STATE_STOP: {
752  // Start primitive STOP only if the current state is Velocity
754  stopMotion();
755  }
756  break;
757  }
760  std::cout << "Change the control mode from velocity to position control.\n";
761  stopMotion();
762  } else {
763  // std::cout << "Change the control mode from stop to position
764  // control.\n";
765  }
766  break;
767  }
770  std::cout << "Change the control mode from stop to velocity control.\n";
771  }
772  break;
773  }
775  default:
776  break;
777  }
778 
779  return vpRobot::setRobotState(newState);
780 }
781 
856 {
858  vpERROR_TRACE("Cannot send a velocity to the robot "
859  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
861  "Cannot send a velocity to the robot "
862  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
863  }
864 
865  vpColVector vel_sat(6);
866 
867  double scale_sat = 1;
868  double vel_trans_max = getMaxTranslationVelocity();
869  double vel_rot_max = getMaxRotationVelocity();
870 
871  double vel_abs; // Absolute value
872 
873  // Velocity saturation
874  switch (frame) {
875  // saturation in cartesian space
878  if (vel.getRows() != 6) {
879  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
880  throw;
881  }
882 
883  for (unsigned int i = 0; i < 3; ++i) {
884  vel_abs = fabs(vel[i]);
885  if (vel_abs > vel_trans_max && !jointLimit) {
886  vel_trans_max = vel_abs;
887  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
888  "(axis nr. %d).",
889  vel[i], i + 1);
890  }
891 
892  vel_abs = fabs(vel[i + 3]);
893  if (vel_abs > vel_rot_max && !jointLimit) {
894  vel_rot_max = vel_abs;
895  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
896  "(axis nr. %d).",
897  vel[i + 3], i + 4);
898  }
899  }
900 
901  double scale_trans_sat = 1;
902  double scale_rot_sat = 1;
903  if (vel_trans_max > getMaxTranslationVelocity())
904  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
905 
906  if (vel_rot_max > getMaxRotationVelocity())
907  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
908 
909  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
910  if (scale_trans_sat < scale_rot_sat)
911  scale_sat = scale_trans_sat;
912  else
913  scale_sat = scale_rot_sat;
914  }
915  break;
916  }
917 
918  // saturation in joint space
920  if (vel.getRows() != 6) {
921  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
922  throw;
923  }
924  for (unsigned int i = 0; i < 6; ++i) {
925  vel_abs = fabs(vel[i]);
926  if (vel_abs > vel_rot_max && !jointLimit) {
927  vel_rot_max = vel_abs;
928  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
929  "(axis nr. %d).",
930  vel[i], i + 1);
931  }
932  }
933  double scale_rot_sat = 1;
934  if (vel_rot_max > getMaxRotationVelocity())
935  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
936  if (scale_rot_sat < 1)
937  scale_sat = scale_rot_sat;
938  break;
939  }
940  case vpRobot::MIXT_FRAME: {
941  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
942  "functionality not implemented");
943  }
945  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOR_FRAME frame:"
946  "functionality not implemented");
947  }
948  }
949 
950  set_velocity(vel * scale_sat);
951 
953  setRobotFrame(frame);
955 
957  setVelocityCalled = true;
959 }
960 
965 {
967 
969  frame = getRobotFrame();
971 
972  double vel_rot_max = getMaxRotationVelocity();
973 
974  vpColVector articularCoordinates = get_artCoord();
975  vpColVector velocityframe = get_velocity();
976  vpColVector articularVelocity;
977 
978  switch (frame) {
979  case vpRobot::CAMERA_FRAME: {
980  vpMatrix eJe_;
982  vpAfma6::get_eJe(articularCoordinates, eJe_);
983  eJe_ = eJe_.pseudoInverse();
985  singularityTest(articularCoordinates, eJe_);
986  articularVelocity = eJe_ * eVc * velocityframe;
987  set_artVel(articularVelocity);
988  break;
989  }
991  vpMatrix fJe_;
992  vpAfma6::get_fJe(articularCoordinates, fJe_);
993  fJe_ = fJe_.pseudoInverse();
995  singularityTest(articularCoordinates, fJe_);
996  articularVelocity = fJe_ * velocityframe;
997  set_artVel(articularVelocity);
998  break;
999  }
1000  case vpRobot::ARTICULAR_FRAME: {
1001  articularVelocity = velocityframe;
1002  set_artVel(articularVelocity);
1003  break;
1004  }
1006  case vpRobot::MIXT_FRAME: {
1007  break;
1008  }
1009  }
1010 
1011  switch (frame) {
1012  case vpRobot::CAMERA_FRAME:
1013  case vpRobot::REFERENCE_FRAME: {
1014  for (unsigned int i = 0; i < 6; ++i) {
1015  double vel_abs = fabs(articularVelocity[i]);
1016  if (vel_abs > vel_rot_max && !jointLimit) {
1017  vel_rot_max = vel_abs;
1018  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1019  "(axis nr. %d).",
1020  articularVelocity[i], i + 1);
1021  }
1022  }
1023  double scale_rot_sat = 1;
1024  double scale_sat = 1;
1025  if (vel_rot_max > getMaxRotationVelocity())
1026  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1027  if (scale_rot_sat < 1)
1028  scale_sat = scale_rot_sat;
1029 
1030  set_artVel(articularVelocity * scale_sat);
1031  break;
1032  }
1035  case vpRobot::MIXT_FRAME: {
1036  break;
1037  }
1038  }
1039 }
1040 
1087 {
1088  vel.resize(6);
1089 
1090  vpColVector articularCoordinates = get_artCoord();
1091  vpColVector articularVelocity = get_artVel();
1092 
1093  switch (frame) {
1094  case vpRobot::CAMERA_FRAME: {
1095  vpMatrix eJe_;
1097  vpAfma6::get_eJe(articularCoordinates, eJe_);
1098  vel = cVe * eJe_ * articularVelocity;
1099  break;
1100  }
1101  case vpRobot::ARTICULAR_FRAME: {
1102  vel = articularVelocity;
1103  break;
1104  }
1105  case vpRobot::REFERENCE_FRAME: {
1106  vpMatrix fJe_;
1107  vpAfma6::get_fJe(articularCoordinates, fJe_);
1108  vel = fJe_ * articularVelocity;
1109  break;
1110  }
1112  case vpRobot::MIXT_FRAME: {
1113  break;
1114  }
1115  default: {
1116  vpERROR_TRACE("Error in spec of vpRobot. "
1117  "Case not taken in account.");
1118  return;
1119  }
1120  }
1121 }
1122 
1140 {
1141  timestamp = vpTime::measureTimeSecond();
1142  getVelocity(frame, vel);
1143 }
1144 
1187 {
1188  vpColVector vel(6);
1189  getVelocity(frame, vel);
1190 
1191  return vel;
1192 }
1193 
1207 {
1208  timestamp = vpTime::measureTimeSecond();
1209  vpColVector vel(6);
1210  getVelocity(frame, vel);
1211 
1212  return vel;
1213 }
1214 
1216 {
1217  double vel_rot_max = getMaxRotationVelocity();
1218  double velmax = fabs(q[0]);
1219  for (unsigned int i = 1; i < 6; i++) {
1220  if (velmax < fabs(q[i]))
1221  velmax = fabs(q[i]);
1222  }
1223 
1224  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1225  q = q * alpha;
1226 }
1227 
1303 {
1305  vpERROR_TRACE("Robot was not in position-based control\n"
1306  "Modification of the robot state");
1307  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1308  }
1309 
1310  vpColVector articularCoordinates = get_artCoord();
1311 
1312  vpColVector error(6);
1313  double errsqr = 0;
1314  switch (frame) {
1315  case vpRobot::CAMERA_FRAME: {
1316  int nbSol;
1317  vpColVector qdes(6);
1318 
1319  vpTranslationVector txyz;
1320  vpRxyzVector rxyz;
1321  for (unsigned int i = 0; i < 3; i++) {
1322  txyz[i] = q[i];
1323  rxyz[i] = q[i + 3];
1324  }
1325 
1326  vpRotationMatrix cRc2(rxyz);
1327  vpHomogeneousMatrix cMc2(txyz, cRc2);
1328 
1329  vpHomogeneousMatrix fMc_;
1330  vpAfma6::get_fMc(articularCoordinates, fMc_);
1331 
1332  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1333 
1334  do {
1335  articularCoordinates = get_artCoord();
1336  qdes = articularCoordinates;
1337  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1338 
1340  setVelocityCalled = true;
1342 
1343  if (nbSol > 0) {
1344  error = qdes - articularCoordinates;
1345  errsqr = error.sumSquare();
1346  // findHighestPositioningSpeed(error);
1347  set_artVel(error);
1348  if (errsqr < 1e-4) {
1349  set_artCoord(qdes);
1350  error = 0;
1351  set_artVel(error);
1352  set_velocity(error);
1353  break;
1354  }
1355  } else {
1356  vpERROR_TRACE("Positioning error.");
1357  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1358  }
1359  } while (errsqr > 1e-8 && nbSol > 0);
1360 
1361  break;
1362  }
1363 
1364  case vpRobot::ARTICULAR_FRAME: {
1365  do {
1366  articularCoordinates = get_artCoord();
1367  error = q - articularCoordinates;
1368  errsqr = error.sumSquare();
1369  // findHighestPositioningSpeed(error);
1370  set_artVel(error);
1372  setVelocityCalled = true;
1374  if (errsqr < 1e-4) {
1375  set_artCoord(q);
1376  error = 0;
1377  set_artVel(error);
1378  set_velocity(error);
1379  break;
1380  }
1381  } while (errsqr > 1e-8);
1382  break;
1383  }
1384 
1385  case vpRobot::REFERENCE_FRAME: {
1386  int nbSol;
1387  vpColVector qdes(6);
1388 
1389  vpTranslationVector txyz;
1390  vpRxyzVector rxyz;
1391  for (unsigned int i = 0; i < 3; i++) {
1392  txyz[i] = q[i];
1393  rxyz[i] = q[i + 3];
1394  }
1395 
1396  vpRotationMatrix fRc(rxyz);
1397  vpHomogeneousMatrix fMc_(txyz, fRc);
1398 
1399  do {
1400  articularCoordinates = get_artCoord();
1401  qdes = articularCoordinates;
1402  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1404  setVelocityCalled = true;
1406  if (nbSol > 0) {
1407  error = qdes - articularCoordinates;
1408  errsqr = error.sumSquare();
1409  // findHighestPositioningSpeed(error);
1410  set_artVel(error);
1411  if (errsqr < 1e-4) {
1412  set_artCoord(qdes);
1413  error = 0;
1414  set_artVel(error);
1415  set_velocity(error);
1416  break;
1417  }
1418  } else
1419  vpERROR_TRACE("Positioning error. Position unreachable");
1420  } while (errsqr > 1e-8 && nbSol > 0);
1421  break;
1422  }
1423  case vpRobot::MIXT_FRAME: {
1424  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1425  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1426  "MIXT_FRAME not implemented.");
1427  }
1429  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1430  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1431  "END_EFFECTOR_FRAME not implemented.");
1432  }
1433  }
1434 }
1435 
1498 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1499  double pos4, double pos5, double pos6)
1500 {
1501  try {
1502  vpColVector position(6);
1503  position[0] = pos1;
1504  position[1] = pos2;
1505  position[2] = pos3;
1506  position[3] = pos4;
1507  position[4] = pos5;
1508  position[5] = pos6;
1509 
1510  setPosition(frame, position);
1511  } catch (...) {
1512  vpERROR_TRACE("Error caught");
1513  throw;
1514  }
1515 }
1516 
1551 void vpSimulatorAfma6::setPosition(const char *filename)
1552 {
1553  vpColVector q;
1554  bool ret;
1555 
1556  ret = this->readPosFile(filename, q);
1557 
1558  if (ret == false) {
1559  vpERROR_TRACE("Bad position in \"%s\"", filename);
1560  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1561  }
1564 }
1565 
1626 {
1627  q.resize(6);
1628 
1629  switch (frame) {
1630  case vpRobot::CAMERA_FRAME: {
1631  q = 0;
1632  break;
1633  }
1634 
1635  case vpRobot::ARTICULAR_FRAME: {
1636  q = get_artCoord();
1637  break;
1638  }
1639 
1640  case vpRobot::REFERENCE_FRAME: {
1641  vpHomogeneousMatrix fMc_;
1642  vpAfma6::get_fMc(get_artCoord(), fMc_);
1643 
1644  vpRotationMatrix fRc;
1645  fMc_.extract(fRc);
1646  vpRxyzVector rxyz(fRc);
1647 
1648  vpTranslationVector txyz;
1649  fMc_.extract(txyz);
1650 
1651  for (unsigned int i = 0; i < 3; i++) {
1652  q[i] = txyz[i];
1653  q[i + 3] = rxyz[i];
1654  }
1655  break;
1656  }
1657 
1658  case vpRobot::MIXT_FRAME: {
1659  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1660  "Mixt frame not implemented.");
1661  }
1663  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1664  "End-effector frame not implemented.");
1665  }
1666  }
1667 }
1668 
1696 {
1697  timestamp = vpTime::measureTimeSecond();
1698  getPosition(frame, q);
1699 }
1700 
1713 {
1714  vpColVector posRxyz;
1715  // recupere position en Rxyz
1716  this->getPosition(frame, posRxyz);
1717 
1718  // recupere le vecteur thetaU correspondant
1719  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1720 
1721  // remplit le vpPoseVector avec translation et rotation ThetaU
1722  for (unsigned int j = 0; j < 3; j++) {
1723  position[j] = posRxyz[j];
1724  position[j + 3] = RtuVect[j];
1725  }
1726 }
1727 
1739 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1740 {
1741  timestamp = vpTime::measureTimeSecond();
1742  getPosition(frame, position);
1743 }
1744 
1755 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1756 {
1757  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1758  vpTRACE("Joint limit vector has not a size of 6 !");
1759  return;
1760  }
1761 
1762  _joint_min[0] = limitMin[0];
1763  _joint_min[1] = limitMin[1];
1764  _joint_min[2] = limitMin[2];
1765  _joint_min[3] = limitMin[3];
1766  _joint_min[4] = limitMin[4];
1767  _joint_min[5] = limitMin[5];
1768 
1769  _joint_max[0] = limitMax[0];
1770  _joint_max[1] = limitMax[1];
1771  _joint_max[2] = limitMax[2];
1772  _joint_max[3] = limitMax[3];
1773  _joint_max[4] = limitMax[4];
1774  _joint_max[5] = limitMax[5];
1775 }
1776 
1783 {
1784  double q5 = q[4];
1785 
1786  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1787 
1788  if (cond) {
1789  J[0][3] = 0;
1790  J[0][4] = 0;
1791  J[1][3] = 0;
1792  J[1][4] = 0;
1793  J[3][3] = 0;
1794  J[3][4] = 0;
1795  J[5][3] = 0;
1796  J[5][4] = 0;
1797  return true;
1798  }
1799 
1800  return false;
1801 }
1802 
1807 {
1808  int artNumb = 0;
1809  double diff = 0;
1810  double difft = 0;
1811 
1812  vpColVector articularCoordinates = get_artCoord();
1813 
1814  for (unsigned int i = 0; i < 6; i++) {
1815  if (articularCoordinates[i] <= _joint_min[i]) {
1816  difft = _joint_min[i] - articularCoordinates[i];
1817  if (difft > diff) {
1818  diff = difft;
1819  artNumb = -(int)i - 1;
1820  }
1821  }
1822  }
1823 
1824  for (unsigned int i = 0; i < 6; i++) {
1825  if (articularCoordinates[i] >= _joint_max[i]) {
1826  difft = articularCoordinates[i] - _joint_max[i];
1827  if (difft > diff) {
1828  diff = difft;
1829  artNumb = (int)(i + 1);
1830  }
1831  }
1832  }
1833 
1834  if (artNumb != 0)
1835  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1836  << std::endl;
1837 
1838  return artNumb;
1839 }
1840 
1859 {
1860  displacement.resize(6);
1861  displacement = 0;
1862  vpColVector q_cur(6);
1863 
1864  q_cur = get_artCoord();
1865 
1866  if (!first_time_getdis) {
1867  switch (frame) {
1868  case vpRobot::CAMERA_FRAME: {
1869  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1870  return;
1871  }
1872  case vpRobot::ARTICULAR_FRAME: {
1873  displacement = q_cur - q_prev_getdis;
1874  break;
1875  }
1876  case vpRobot::REFERENCE_FRAME: {
1877  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1878  return;
1879  }
1880  case vpRobot::MIXT_FRAME: {
1881  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1882  return;
1883  }
1885  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1886  return;
1887  }
1888  }
1889  } else {
1890  first_time_getdis = false;
1891  }
1892 
1893  // Memorize the joint position for the next call
1894  q_prev_getdis = q_cur;
1895 }
1896 
1944 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1945 {
1946  std::ifstream fd(filename.c_str(), std::ios::in);
1947 
1948  if (!fd.is_open()) {
1949  return false;
1950  }
1951 
1952  std::string line;
1953  std::string key("R:");
1954  std::string id("#AFMA6 - Position");
1955  bool pos_found = false;
1956  int lineNum = 0;
1957 
1958  q.resize(njoint);
1959 
1960  while (std::getline(fd, line)) {
1961  lineNum++;
1962  if (lineNum == 1) {
1963  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1964  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1965  return false;
1966  }
1967  }
1968  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1969  continue;
1970  }
1971  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1972  // check if there are at least njoint values in the line
1973  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1974  if (chain.size() < njoint + 1) // try to split with tab separator
1975  chain = vpIoTools::splitChain(line, std::string("\t"));
1976  if (chain.size() < njoint + 1)
1977  continue;
1978 
1979  std::istringstream ss(line);
1980  std::string key_;
1981  ss >> key_;
1982  for (unsigned int i = 0; i < njoint; i++)
1983  ss >> q[i];
1984  pos_found = true;
1985  break;
1986  }
1987  }
1988 
1989  // converts rotations from degrees into radians
1990  q[3] = vpMath::rad(q[3]);
1991  q[4] = vpMath::rad(q[4]);
1992  q[5] = vpMath::rad(q[5]);
1993 
1994  fd.close();
1995 
1996  if (!pos_found) {
1997  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
1998  return false;
1999  }
2000 
2001  return true;
2002 }
2003 
2026 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2027 {
2028  FILE *fd;
2029  fd = fopen(filename.c_str(), "w");
2030  if (fd == NULL)
2031  return false;
2032 
2033  fprintf(fd, "\
2034 #AFMA6 - Position - Version 2.01\n\
2035 #\n\
2036 # R: X Y Z A B C\n\
2037 # Joint position: X, Y, Z: translations in meters\n\
2038 # A, B, C: rotations in degrees\n\
2039 #\n\
2040 #\n\n");
2041 
2042  // Save positions in mm and deg
2043  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2044  vpMath::deg(q[5]));
2045 
2046  fclose(fd);
2047  return (true);
2048 }
2049 
2057 void vpSimulatorAfma6::move(const char *filename)
2058 {
2059  vpColVector q;
2060 
2061  try {
2062  this->readPosFile(filename, q);
2065  } catch (...) {
2066  throw;
2067  }
2068 }
2069 
2080 
2089 {
2090  vpHomogeneousMatrix cMe;
2091  vpAfma6::get_cMe(cMe);
2092 
2093  cVe.buildFrom(cMe);
2094 }
2095 
2106 {
2107  try {
2108  vpAfma6::get_eJe(get_artCoord(), eJe_);
2109  } catch (...) {
2110  vpERROR_TRACE("catch exception ");
2111  throw;
2112  }
2113 }
2114 
2126 {
2127  try {
2128  vpColVector articularCoordinates = get_artCoord();
2129  vpAfma6::get_fJe(articularCoordinates, fJe_);
2130  } catch (...) {
2131  vpERROR_TRACE("Error caught");
2132  throw;
2133  }
2134 }
2135 
2140 {
2142  return;
2143 
2144  vpColVector stop(6);
2145  stop = 0;
2146  set_artVel(stop);
2147  set_velocity(stop);
2149 }
2150 
2151 /**********************************************************************************/
2152 /**********************************************************************************/
2153 /**********************************************************************************/
2154 /**********************************************************************************/
2155 
2165 {
2166  // set scene_dir from #define VISP_SCENE_DIR if it exists
2167  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2168  std::string scene_dir_;
2169  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2170  bool sceneDirExists = false;
2171  for (size_t i = 0; i < scene_dirs.size(); i++)
2172  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2173  scene_dir_ = scene_dirs[i];
2174  sceneDirExists = true;
2175  break;
2176  }
2177  if (!sceneDirExists) {
2178  try {
2179  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2180  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2181  } catch (...) {
2182  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2183  }
2184  }
2185 
2186  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2187  if (scene_dir_.size() > FILENAME_MAX)
2188  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2189  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2190  if (full_length > FILENAME_MAX)
2191  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2192 
2193  char *name_cam = new char[full_length];
2194 
2195  strcpy(name_cam, scene_dir_.c_str());
2196  strcat(name_cam, "/camera.bnd");
2197  set_scene(name_cam, &camera, cameraFactor);
2198 
2199  if (arm_dir.size() > FILENAME_MAX)
2200  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2201  full_length = (unsigned int)arm_dir.size() + name_length;
2202  if (full_length > FILENAME_MAX)
2203  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2204 
2205  char *name_arm = new char[full_length];
2206  strcpy(name_arm, arm_dir.c_str());
2207  strcat(name_arm, "/afma6_gate.bnd");
2208  std::cout << "name arm: " << name_arm << std::endl;
2209  set_scene(name_arm, robotArms, 1.0);
2210  strcpy(name_arm, arm_dir.c_str());
2211  strcat(name_arm, "/afma6_arm1.bnd");
2212  set_scene(name_arm, robotArms + 1, 1.0);
2213  strcpy(name_arm, arm_dir.c_str());
2214  strcat(name_arm, "/afma6_arm2.bnd");
2215  set_scene(name_arm, robotArms + 2, 1.0);
2216  strcpy(name_arm, arm_dir.c_str());
2217  strcat(name_arm, "/afma6_arm3.bnd");
2218  set_scene(name_arm, robotArms + 3, 1.0);
2219  strcpy(name_arm, arm_dir.c_str());
2220  strcat(name_arm, "/afma6_arm4.bnd");
2221  set_scene(name_arm, robotArms + 4, 1.0);
2222 
2224  tool = getToolType();
2225  strcpy(name_arm, arm_dir.c_str());
2226  switch (tool) {
2227  case vpAfma6::TOOL_CCMOP: {
2228  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2229  break;
2230  }
2231  case vpAfma6::TOOL_GRIPPER: {
2232  strcat(name_arm, "/afma6_tool_gripper.bnd");
2233  break;
2234  }
2235  case vpAfma6::TOOL_VACUUM: {
2236  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2237  break;
2238  }
2239  case vpAfma6::TOOL_CUSTOM: {
2240  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2241  break;
2242  }
2244  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2245  break;
2246  }
2248  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2249  break;
2250  }
2251  }
2252  set_scene(name_arm, robotArms + 5, 1.0);
2253 
2254  add_rfstack(IS_BACK);
2255 
2256  add_vwstack("start", "depth", 0.0, 100.0);
2257  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2258  add_vwstack("start", "type", PERSPECTIVE);
2259  //
2260  // sceneInitialized = true;
2261  // displayObject = true;
2262  displayCamera = true;
2263 
2264  delete[] name_cam;
2265  delete[] name_arm;
2266 }
2267 
2269 {
2270  m_mutex_scene.lock();
2271  bool changed = false;
2272  vpHomogeneousMatrix displacement = navigation(I_, changed);
2273 
2274  // if (displacement[2][3] != 0)
2275  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2276  camMf2 = camMf2 * displacement;
2277 
2278  f2Mf = camMf2.inverse() * camMf;
2279 
2280  camMf = camMf2 * displacement * f2Mf;
2281 
2282  double u;
2283  double v;
2284  // if(px_ext != 1 && py_ext != 1)
2285  // we assume px_ext and py_ext > 0
2286  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2287  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2288  u = (double)I_.getWidth() / (2 * px_ext);
2289  v = (double)I_.getHeight() / (2 * py_ext);
2290  } else {
2291  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2292  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2293  }
2294 
2295  float w44o[4][4], w44cext[4][4], x, y, z;
2296 
2297  vp2jlc_matrix(camMf.inverse(), w44cext);
2298 
2299  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2300  x = w44cext[2][0] + w44cext[3][0];
2301  y = w44cext[2][1] + w44cext[3][1];
2302  z = w44cext[2][2] + w44cext[3][2];
2303  add_vwstack("start", "vrp", x, y, z);
2304  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2305  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2306  add_vwstack("start", "window", -u, u, -v, v);
2307 
2308  vpHomogeneousMatrix fMit[8];
2309  get_fMi(fMit);
2310 
2311  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2312  display_scene(w44o, robotArms[0], I_, curColor);
2313 
2314  vp2jlc_matrix(fMit[0], w44o);
2315  display_scene(w44o, robotArms[1], I_, curColor);
2316 
2317  vp2jlc_matrix(fMit[2], w44o);
2318  display_scene(w44o, robotArms[2], I_, curColor);
2319 
2320  vp2jlc_matrix(fMit[3], w44o);
2321  display_scene(w44o, robotArms[3], I_, curColor);
2322 
2323  vp2jlc_matrix(fMit[4], w44o);
2324  display_scene(w44o, robotArms[4], I_, curColor);
2325 
2326  vp2jlc_matrix(fMit[5], w44o);
2327  display_scene(w44o, robotArms[5], I_, curColor);
2328 
2329  if (displayCamera) {
2330  vpHomogeneousMatrix cMe;
2331  get_cMe(cMe);
2332  cMe = cMe.inverse();
2333  cMe = fMit[6] * cMe;
2334  vp2jlc_matrix(cMe, w44o);
2335  display_scene(w44o, camera, I_, camColor);
2336  }
2337 
2338  if (displayObject) {
2339  vp2jlc_matrix(fMo, w44o);
2340  display_scene(w44o, scene, I_, curColor);
2341  }
2343 }
2344 
2363 {
2364  vpColVector stop(6);
2365  bool status = true;
2366  stop = 0;
2367  m_mutex_scene.lock();
2368  set_artVel(stop);
2369  set_velocity(stop);
2370  vpHomogeneousMatrix fMc_;
2371  fMc_ = fMo * cMo_.inverse();
2372 
2373  vpColVector articularCoordinates = get_artCoord();
2374  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2375 
2376  if (nbSol == 0) {
2377  status = false;
2378  vpERROR_TRACE("Positioning error. Position unreachable");
2379  }
2380 
2381  if (verbose_)
2382  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2383 
2384  set_artCoord(articularCoordinates);
2385 
2386  compute_fMi();
2388 
2389  return status;
2390 }
2391 
2406 {
2407  vpColVector stop(6);
2408  stop = 0;
2409  m_mutex_scene.lock();
2410  set_artVel(stop);
2411  set_velocity(stop);
2412  vpHomogeneousMatrix fMit[8];
2413  get_fMi(fMit);
2414  fMo = fMit[7] * cMo_;
2416 }
2417 
2429 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2430 {
2431  // get rid of max velocity
2432  double vMax = getMaxTranslationVelocity();
2433  double wMax = getMaxRotationVelocity();
2434  setMaxTranslationVelocity(10. * vMax);
2435  setMaxRotationVelocity(10. * wMax);
2436 
2437  vpColVector v(3), w(3), vel(6);
2438  vpHomogeneousMatrix cdMc;
2439  vpTranslationVector cdTc;
2440  vpRotationMatrix cdRc;
2441  vpThetaUVector cdTUc;
2442  vpColVector err(6);
2443  err = 1.;
2444  const double lambda = 5.;
2445 
2447 
2448  unsigned int i, iter = 0;
2449  while ((iter++ < 300) && (err.frobeniusNorm() > errMax)) {
2450  double t = vpTime::measureTimeMs();
2451 
2452  // update image
2453  if (Iint != NULL) {
2454  vpDisplay::display(*Iint);
2455  getInternalView(*Iint);
2456  vpDisplay::flush(*Iint);
2457  }
2458 
2459  // update pose error
2460  cdMc = cdMo_ * get_cMo().inverse();
2461  cdMc.extract(cdRc);
2462  cdMc.extract(cdTc);
2463  cdTUc.buildFrom(cdRc);
2464 
2465  // compute v,w and velocity
2466  v = -lambda * cdRc.t() * cdTc;
2467  w = -lambda * cdTUc;
2468  for (i = 0; i < 3; ++i) {
2469  vel[i] = v[i];
2470  vel[i + 3] = w[i];
2471  err[i] = cdTc[i];
2472  err[i + 3] = cdTUc[i];
2473  }
2474 
2475  // update feat
2477 
2478  // wait for it
2479  vpTime::wait(t, 10);
2480  }
2481  vel = 0.;
2482  set_velocity(vel);
2483  set_artVel(vel);
2485  setMaxRotationVelocity(wMax);
2486 
2487  // std::cout << "setPosition: final error " << err.t() << std::endl;
2488  return (err.frobeniusNorm() <= errMax);
2489 }
2490 
2491 #elif !defined(VISP_BUILD_SHARED_LIBS)
2492 // Work around to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2493 // no symbols
2494 void dummy_vpSimulatorAfma6(){};
2495 #endif
Modelisation 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:887
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:774
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:931
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1001
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:290
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:167
double sumSquare() const
vpRowVector t() const
double frobeniusNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:351
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:2012
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:449
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:379
static double rad(double deg)
Definition: vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:172
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:180
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2238
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void unlock()
Definition: vpMutex.h:106
void lock()
Definition: vpMutex.h:90
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:469
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:467
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:192
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)
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:142
vpControlFrameType
Definition: vpRobot.h:73
@ REFERENCE_FRAME
Definition: vpRobot.h:74
@ ARTICULAR_FRAME
Definition: vpRobot.h:76
@ MIXT_FRAME
Definition: vpRobot.h:84
@ CAMERA_FRAME
Definition: vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:79
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:170
vpRobotStateType
Definition: vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:64
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:63
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:178
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:411
#define vpERROR_TRACE
Definition: vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()