Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 #ifdef WINRT_8_1
73  mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
74  mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
75  mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
76  mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
77  mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
78 #else
79  mutex_fMi = CreateMutex(NULL, FALSE, NULL);
80  mutex_artVel = CreateMutex(NULL, FALSE, NULL);
81  mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
82  mutex_velocity = CreateMutex(NULL, FALSE, NULL);
83  mutex_display = CreateMutex(NULL, FALSE, NULL);
84 #endif
85 
86  DWORD dwThreadIdArray;
87  hThread = CreateThread(NULL, // default security attributes
88  0, // use default stack size
89  launcher, // thread function name
90  this, // argument to thread function
91  0, // use default creation flags
92  &dwThreadIdArray); // returns the thread identifier
93 #elif defined(VISP_HAVE_PTHREAD)
94  pthread_mutex_init(&mutex_fMi, NULL);
95  pthread_mutex_init(&mutex_artVel, NULL);
96  pthread_mutex_init(&mutex_artCoord, NULL);
97  pthread_mutex_init(&mutex_velocity, NULL);
98  pthread_mutex_init(&mutex_display, NULL);
99 
100  pthread_attr_init(&attr);
101  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
102 
103  pthread_create(&thread, NULL, launcher, (void *)this);
104 #endif
105 
106  compute_fMi();
107 }
108 
116  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
117  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
118 {
119  init();
120  initDisplay();
121 
123 
124 #if defined(_WIN32)
125 #ifdef WINRT_8_1
126  mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
127  mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
128  mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
129  mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
130  mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
131 #else
132  mutex_fMi = CreateMutex(NULL, FALSE, NULL);
133  mutex_artVel = CreateMutex(NULL, FALSE, NULL);
134  mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
135  mutex_velocity = CreateMutex(NULL, FALSE, NULL);
136  mutex_display = CreateMutex(NULL, FALSE, NULL);
137 #endif
138 
139  DWORD dwThreadIdArray;
140  hThread = CreateThread(NULL, // default security attributes
141  0, // use default stack size
142  launcher, // thread function name
143  this, // argument to thread function
144  0, // use default creation flags
145  &dwThreadIdArray); // returns the thread identifier
146 #elif defined(VISP_HAVE_PTHREAD)
147  pthread_mutex_init(&mutex_fMi, NULL);
148  pthread_mutex_init(&mutex_artVel, NULL);
149  pthread_mutex_init(&mutex_artCoord, NULL);
150  pthread_mutex_init(&mutex_velocity, NULL);
151  pthread_mutex_init(&mutex_display, NULL);
152 
153  pthread_attr_init(&attr);
154  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
155 
156  pthread_create(&thread, NULL, launcher, (void *)this);
157 #endif
158 
159  compute_fMi();
160 }
161 
166 {
167  robotStop = true;
168 
169 #if defined(_WIN32)
170 #if defined(WINRT_8_1)
171  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
172 #else // pure win32
173  WaitForSingleObject(hThread, INFINITE);
174 #endif
175  CloseHandle(hThread);
176  CloseHandle(mutex_fMi);
177  CloseHandle(mutex_artVel);
178  CloseHandle(mutex_artCoord);
179  CloseHandle(mutex_velocity);
180  CloseHandle(mutex_display);
181 #elif defined(VISP_HAVE_PTHREAD)
182  pthread_attr_destroy(&attr);
183  pthread_join(thread, NULL);
184  pthread_mutex_destroy(&mutex_fMi);
185  pthread_mutex_destroy(&mutex_artVel);
186  pthread_mutex_destroy(&mutex_artCoord);
187  pthread_mutex_destroy(&mutex_velocity);
188  pthread_mutex_destroy(&mutex_display);
189 #endif
190 
191  if (robotArms != NULL) {
192  for (int i = 0; i < 6; i++)
193  free_Bound_scene(&(robotArms[i]));
194  }
195 
196  delete[] robotArms;
197  delete[] fMi;
198 }
199 
209 {
210  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
211  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
212  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
213  bool armDirExists = false;
214  for (size_t i = 0; i < arm_dirs.size(); i++)
215  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
216  arm_dir = arm_dirs[i];
217  armDirExists = true;
218  break;
219  }
220  if (!armDirExists) {
221  try {
222  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
223  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
224  } catch (...) {
225  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
226  }
227  }
228 
229  this->init(vpAfma6::TOOL_CCMOP);
230  toolCustom = false;
231 
232  size_fMi = 8;
233  fMi = new vpHomogeneousMatrix[8];
236 
237  zeroPos.resize(njoint);
238  zeroPos = 0;
239  reposPos.resize(njoint);
240  reposPos = 0;
241  reposPos[1] = -M_PI / 2;
242  reposPos[2] = M_PI;
243  reposPos[4] = M_PI / 2;
244 
245  artCoord = zeroPos;
246  artVel = 0;
247 
248  q_prev_getdis.resize(njoint);
249  q_prev_getdis = 0;
250  first_time_getdis = true;
251 
252  positioningVelocity = defaultPositioningVelocity;
253 
256 
257  // Software joint limits in radians
258  //_joint_min.resize(njoint);
259  _joint_min[0] = -0.6501;
260  _joint_min[1] = -0.6001;
261  _joint_min[2] = -0.5001;
262  _joint_min[3] = -2.7301;
263  _joint_min[4] = -0.1001;
264  _joint_min[5] = -1.5901;
265  //_joint_max.resize(njoint);
266  _joint_max[0] = 0.7001;
267  _joint_max[1] = 0.5201;
268  _joint_max[2] = 0.4601;
269  _joint_max[3] = 2.7301;
270  _joint_max[4] = 2.4801;
271  _joint_max[5] = 1.5901;
272 }
273 
278 {
279  robotArms = NULL;
280  robotArms = new Bound_scene[6];
281  initArms();
283  vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
284  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
286  vpCameraParameters tmp;
287  getCameraParameters(tmp, 640, 480);
288  px_int = tmp.get_px();
289  py_int = tmp.get_py();
290  sceneInitialized = true;
291 }
292 
309 {
310  this->projModel = proj_model;
311  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
312  if (arm_dir.size() > FILENAME_MAX)
313  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
314  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
315  if (full_length > FILENAME_MAX)
316  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
317 
318  // Use here default values of the robot constant parameters.
319  switch (tool) {
320  case vpAfma6::TOOL_CCMOP: {
321  _erc[0] = vpMath::rad(164.35); // rx
322  _erc[1] = vpMath::rad(89.64); // ry
323  _erc[2] = vpMath::rad(-73.05); // rz
324  _etc[0] = 0.0117; // tx
325  _etc[1] = 0.0033; // ty
326  _etc[2] = 0.2272; // tz
327 
328  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
329 
330  if (robotArms != NULL) {
331  while (get_displayBusy())
332  vpTime::wait(2);
333  free_Bound_scene(&(robotArms[5]));
334  char *name_arm = new char[full_length];
335  strcpy(name_arm, arm_dir.c_str());
336  strcat(name_arm, "/afma6_tool_ccmop.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_GRIPPER: {
344  _erc[0] = vpMath::rad(88.33); // rx
345  _erc[1] = vpMath::rad(72.07); // ry
346  _erc[2] = vpMath::rad(2.53); // rz
347  _etc[0] = 0.0783; // tx
348  _etc[1] = 0.1234; // ty
349  _etc[2] = 0.1638; // tz
350 
351  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
352 
353  if (robotArms != NULL) {
354  while (get_displayBusy())
355  vpTime::wait(2);
356  free_Bound_scene(&(robotArms[5]));
357  char *name_arm = new char[full_length];
358  strcpy(name_arm, arm_dir.c_str());
359  strcat(name_arm, "/afma6_tool_gripper.bnd");
360  set_scene(name_arm, robotArms + 5, 1.0);
361  set_displayBusy(false);
362  delete[] name_arm;
363  }
364  break;
365  }
366  case vpAfma6::TOOL_VACUUM: {
367  _erc[0] = vpMath::rad(90.40); // rx
368  _erc[1] = vpMath::rad(75.11); // ry
369  _erc[2] = vpMath::rad(0.18); // rz
370  _etc[0] = 0.0038; // tx
371  _etc[1] = 0.1281; // ty
372  _etc[2] = 0.1658; // tz
373 
374  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
375 
376  if (robotArms != NULL) {
377  while (get_displayBusy())
378  vpTime::wait(2);
379  free_Bound_scene(&(robotArms[5]));
380 
381  char *name_arm = new char[full_length];
382 
383  strcpy(name_arm, arm_dir.c_str());
384  strcat(name_arm, "/afma6_tool_vacuum.bnd");
385  set_scene(name_arm, robotArms + 5, 1.0);
386  set_displayBusy(false);
387  delete[] name_arm;
388  }
389  break;
390  }
391  case vpAfma6::TOOL_CUSTOM: {
392  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
393  break;
394  }
396  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
397  break;
398  }
400  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
401  break;
402  }
403  }
404 
405  vpRotationMatrix eRc(_erc);
406  this->_eMc.buildFrom(_etc, eRc);
407 
408  setToolType(tool);
409  return;
410 }
411 
422 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
423  const unsigned int &image_height)
424 {
425  if (toolCustom) {
426  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
427  }
428  // Set default parameters
429  switch (getToolType()) {
430  case vpAfma6::TOOL_CCMOP: {
431  // Set default intrinsic camera parameters for 640x480 images
432  if (image_width == 640 && image_height == 480) {
433  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
434  << std::endl;
435  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
436  } else {
437  vpTRACE("Cannot get default intrinsic camera parameters for this image "
438  "resolution");
439  }
440  break;
441  }
442  case vpAfma6::TOOL_GRIPPER: {
443  // Set default intrinsic camera parameters for 640x480 images
444  if (image_width == 640 && image_height == 480) {
445  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
446  << std::endl;
447  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
448  } else {
449  vpTRACE("Cannot get default intrinsic camera parameters for this image "
450  "resolution");
451  }
452  break;
453  }
454  case vpAfma6::TOOL_CUSTOM: {
455  std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
456  break;
457  }
459  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
460  break;
461  }
463  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
464  break;
465  }
466  case vpAfma6::TOOL_VACUUM: {
467  std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
468  break;
469  }
470  default:
471  vpERROR_TRACE("This error should not occur!");
472  break;
473  }
474  return;
475 }
476 
486 {
487  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
488 }
489 
499 {
500  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
501 }
502 
509 {
510  px_int = cam.get_px();
511  py_int = cam.get_py();
512  toolCustom = true;
513 }
514 
520 {
521  double tcur_1 = tcur; // temporary variable used to store the last time
522  // since the last command
523 
524  while (!robotStop) {
525  // Get current time
526  tprev = tcur_1;
528 
530  setVelocityCalled = false;
531 
533 
534  double ellapsedTime = (tcur - tprev) * 1e-3;
535  if (constantSamplingTimeMode) { // if we want a constant velocity, we
536  // force the ellapsed time to the given
537  // samplingTime
538  ellapsedTime = getSamplingTime(); // in second
539  }
540 
541  vpColVector articularCoordinates = get_artCoord();
542  vpColVector articularVelocities = get_artVel();
543 
544  if (jointLimit) {
545  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
546  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
547  if (verbose_) {
548  std::cout << "Joint " << jointLimitArt - 1
549  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
550  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
551  }
552 
553  articularVelocities = 0.0;
554  } else
555  jointLimit = false;
556  }
557 
558  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
559  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
560  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
561  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
562  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
563  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
564 
565  int jl = isInJointLimit();
566 
567  if (jl != 0 && jointLimit == false) {
568  if (jl < 0)
569  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
570  (articularVelocities[(unsigned int)(-jl - 1)]);
571  else
572  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
573  (articularVelocities[(unsigned int)(jl - 1)]);
574 
575  for (unsigned int i = 0; i < 6; i++)
576  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
577 
578  jointLimit = true;
579  jointLimitArt = (unsigned int)fabs((double)jl);
580  }
581 
582  set_artCoord(articularCoordinates);
583  set_artVel(articularVelocities);
584 
585  compute_fMi();
586 
587  if (displayAllowed) {
591  }
592 
593  if (displayType == MODEL_3D && displayAllowed) {
594  while (get_displayBusy())
595  vpTime::wait(2);
597  set_displayBusy(false);
598  }
599 
600  if (0 /*displayType == MODEL_DH && displayAllowed*/) {
601  vpHomogeneousMatrix fMit[8];
602  get_fMi(fMit);
603 
604  // vpDisplay::displayFrame(I,getExternalCameraPosition
605  // ()*fMi[6],cameraParam,0.2,vpColor::none);
606 
607  vpImagePoint iP, iP_1;
608  vpPoint pt(0, 0, 0);
609 
612  pt.track(getExternalCameraPosition() * fMit[0]);
615  for (unsigned int k = 1; k < 7; k++) {
616  pt.track(getExternalCameraPosition() * fMit[k - 1]);
618 
619  pt.track(getExternalCameraPosition() * fMit[k]);
621 
623  }
625  thickness_);
626  }
627 
629 
630  vpTime::wait(tcur, 1000 * getSamplingTime());
631  tcur_1 = tcur;
632  } else {
634  }
635  }
636 }
637 
652 {
653  // vpColVector q = get_artCoord();
654  vpColVector q(6); //; = get_artCoord();
655  q = get_artCoord();
656 
657  vpHomogeneousMatrix fMit[8];
658 
659  double q1 = q[0];
660  double q2 = q[1];
661  double q3 = q[2];
662  double q4 = q[3];
663  double q5 = q[4];
664  double q6 = q[5];
665 
666  double c4 = cos(q4);
667  double s4 = sin(q4);
668  double c5 = cos(q5);
669  double s5 = sin(q5);
670  double c6 = cos(q6);
671  double s6 = sin(q6);
672 
673  fMit[0][0][0] = 1;
674  fMit[0][1][0] = 0;
675  fMit[0][2][0] = 0;
676  fMit[0][0][1] = 0;
677  fMit[0][1][1] = 1;
678  fMit[0][2][1] = 0;
679  fMit[0][0][2] = 0;
680  fMit[0][1][2] = 0;
681  fMit[0][2][2] = 1;
682  fMit[0][0][3] = q1;
683  fMit[0][1][3] = 0;
684  fMit[0][2][3] = 0;
685 
686  fMit[1][0][0] = 1;
687  fMit[1][1][0] = 0;
688  fMit[1][2][0] = 0;
689  fMit[1][0][1] = 0;
690  fMit[1][1][1] = 1;
691  fMit[1][2][1] = 0;
692  fMit[1][0][2] = 0;
693  fMit[1][1][2] = 0;
694  fMit[1][2][2] = 1;
695  fMit[1][0][3] = q1;
696  fMit[1][1][3] = q2;
697  fMit[1][2][3] = 0;
698 
699  fMit[2][0][0] = 1;
700  fMit[2][1][0] = 0;
701  fMit[2][2][0] = 0;
702  fMit[2][0][1] = 0;
703  fMit[2][1][1] = 1;
704  fMit[2][2][1] = 0;
705  fMit[2][0][2] = 0;
706  fMit[2][1][2] = 0;
707  fMit[2][2][2] = 1;
708  fMit[2][0][3] = q1;
709  fMit[2][1][3] = q2;
710  fMit[2][2][3] = q3;
711 
712  fMit[3][0][0] = s4;
713  fMit[3][1][0] = -c4;
714  fMit[3][2][0] = 0;
715  fMit[3][0][1] = c4;
716  fMit[3][1][1] = s4;
717  fMit[3][2][1] = 0;
718  fMit[3][0][2] = 0;
719  fMit[3][1][2] = 0;
720  fMit[3][2][2] = 1;
721  fMit[3][0][3] = q1;
722  fMit[3][1][3] = q2;
723  fMit[3][2][3] = q3;
724 
725  fMit[4][0][0] = s4 * s5;
726  fMit[4][1][0] = -c4 * s5;
727  fMit[4][2][0] = c5;
728  fMit[4][0][1] = s4 * c5;
729  fMit[4][1][1] = -c4 * c5;
730  fMit[4][2][1] = -s5;
731  fMit[4][0][2] = c4;
732  fMit[4][1][2] = s4;
733  fMit[4][2][2] = 0;
734  fMit[4][0][3] = c4 * this->_long_56 + q1;
735  fMit[4][1][3] = s4 * this->_long_56 + q2;
736  fMit[4][2][3] = q3;
737 
738  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
739  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
740  fMit[5][2][0] = c5 * c6;
741  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
742  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
743  fMit[5][2][1] = -c5 * s6;
744  fMit[5][0][2] = -s4 * c5;
745  fMit[5][1][2] = c4 * c5;
746  fMit[5][2][2] = s5;
747  fMit[5][0][3] = c4 * this->_long_56 + q1;
748  fMit[5][1][3] = s4 * this->_long_56 + q2;
749  fMit[5][2][3] = q3;
750 
751  fMit[6][0][0] = fMit[5][0][0];
752  fMit[6][1][0] = fMit[5][1][0];
753  fMit[6][2][0] = fMit[5][2][0];
754  fMit[6][0][1] = fMit[5][0][1];
755  fMit[6][1][1] = fMit[5][1][1];
756  fMit[6][2][1] = fMit[5][2][1];
757  fMit[6][0][2] = fMit[5][0][2];
758  fMit[6][1][2] = fMit[5][1][2];
759  fMit[6][2][2] = fMit[5][2][2];
760  fMit[6][0][3] = fMit[5][0][3];
761  fMit[6][1][3] = fMit[5][1][3];
762  fMit[6][2][3] = fMit[5][2][3];
763 
764  // vpHomogeneousMatrix cMe;
765  // get_cMe(cMe);
766  // cMe = cMe.inverse();
767  // fMit[7] = fMit[6] * cMe;
768  vpAfma6::get_fMc(q, fMit[7]);
769 
770 #if defined(_WIN32)
771 #if defined(WINRT_8_1)
772  WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
773 #else // pure win32
774  WaitForSingleObject(mutex_fMi, INFINITE);
775 #endif
776  for (int i = 0; i < 8; i++)
777  fMi[i] = fMit[i];
778  ReleaseMutex(mutex_fMi);
779 #elif defined(VISP_HAVE_PTHREAD)
780  pthread_mutex_lock(&mutex_fMi);
781  for (int i = 0; i < 8; i++)
782  fMi[i] = fMit[i];
783  pthread_mutex_unlock(&mutex_fMi);
784 #endif
785 }
786 
793 {
794  switch (newState) {
795  case vpRobot::STATE_STOP: {
796  // Start primitive STOP only if the current state is Velocity
798  stopMotion();
799  }
800  break;
801  }
804  std::cout << "Change the control mode from velocity to position control.\n";
805  stopMotion();
806  } else {
807  // std::cout << "Change the control mode from stop to position
808  // control.\n";
809  }
810  break;
811  }
814  std::cout << "Change the control mode from stop to velocity control.\n";
815  }
816  break;
817  }
819  default:
820  break;
821  }
822 
823  return vpRobot::setRobotState(newState);
824 }
825 
900 {
902  vpERROR_TRACE("Cannot send a velocity to the robot "
903  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
905  "Cannot send a velocity to the robot "
906  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
907  }
908 
909  vpColVector vel_sat(6);
910 
911  double scale_sat = 1;
912  double vel_trans_max = getMaxTranslationVelocity();
913  double vel_rot_max = getMaxRotationVelocity();
914 
915  double vel_abs; // Absolute value
916 
917  // Velocity saturation
918  switch (frame) {
919  // saturation in cartesian space
922  if (vel.getRows() != 6) {
923  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
924  throw;
925  }
926 
927  for (unsigned int i = 0; i < 3; ++i) {
928  vel_abs = fabs(vel[i]);
929  if (vel_abs > vel_trans_max && !jointLimit) {
930  vel_trans_max = vel_abs;
931  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
932  "(axis nr. %d).",
933  vel[i], i + 1);
934  }
935 
936  vel_abs = fabs(vel[i + 3]);
937  if (vel_abs > vel_rot_max && !jointLimit) {
938  vel_rot_max = vel_abs;
939  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
940  "(axis nr. %d).",
941  vel[i + 3], i + 4);
942  }
943  }
944 
945  double scale_trans_sat = 1;
946  double scale_rot_sat = 1;
947  if (vel_trans_max > getMaxTranslationVelocity())
948  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
949 
950  if (vel_rot_max > getMaxRotationVelocity())
951  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
952 
953  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
954  if (scale_trans_sat < scale_rot_sat)
955  scale_sat = scale_trans_sat;
956  else
957  scale_sat = scale_rot_sat;
958  }
959  break;
960  }
961 
962  // saturation in joint space
964  if (vel.getRows() != 6) {
965  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
966  throw;
967  }
968  for (unsigned int i = 0; i < 6; ++i) {
969  vel_abs = fabs(vel[i]);
970  if (vel_abs > vel_rot_max && !jointLimit) {
971  vel_rot_max = vel_abs;
972  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
973  "(axis nr. %d).",
974  vel[i], i + 1);
975  }
976  }
977  double scale_rot_sat = 1;
978  if (vel_rot_max > getMaxRotationVelocity())
979  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
980  if (scale_rot_sat < 1)
981  scale_sat = scale_rot_sat;
982  break;
983  }
984  case vpRobot::MIXT_FRAME: {
985  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
986  "functionality not implemented");
987  }
989  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOT_FRAME frame:"
990  "functionality not implemented");
991  }
992  }
993 
994  set_velocity(vel * scale_sat);
995  setRobotFrame(frame);
996  setVelocityCalled = true;
997 }
998 
1003 {
1005 
1006  double vel_rot_max = getMaxRotationVelocity();
1007 
1008  vpColVector articularCoordinates = get_artCoord();
1009  vpColVector velocityframe = get_velocity();
1010  vpColVector articularVelocity;
1011 
1012  switch (frame) {
1013  case vpRobot::CAMERA_FRAME: {
1014  vpMatrix eJe_;
1016  vpAfma6::get_eJe(articularCoordinates, eJe_);
1017  eJe_ = eJe_.pseudoInverse();
1019  singularityTest(articularCoordinates, eJe_);
1020  articularVelocity = eJe_ * eVc * velocityframe;
1021  set_artVel(articularVelocity);
1022  break;
1023  }
1024  case vpRobot::REFERENCE_FRAME: {
1025  vpMatrix fJe_;
1026  vpAfma6::get_fJe(articularCoordinates, fJe_);
1027  fJe_ = fJe_.pseudoInverse();
1029  singularityTest(articularCoordinates, fJe_);
1030  articularVelocity = fJe_ * velocityframe;
1031  set_artVel(articularVelocity);
1032  break;
1033  }
1034  case vpRobot::ARTICULAR_FRAME: {
1035  articularVelocity = velocityframe;
1036  set_artVel(articularVelocity);
1037  break;
1038  }
1040  case vpRobot::MIXT_FRAME: {
1041  break;
1042  }
1043  }
1044 
1045  switch (frame) {
1046  case vpRobot::CAMERA_FRAME:
1047  case vpRobot::REFERENCE_FRAME: {
1048  for (unsigned int i = 0; i < 6; ++i) {
1049  double vel_abs = fabs(articularVelocity[i]);
1050  if (vel_abs > vel_rot_max && !jointLimit) {
1051  vel_rot_max = vel_abs;
1052  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1053  "(axis nr. %d).",
1054  articularVelocity[i], i + 1);
1055  }
1056  }
1057  double scale_rot_sat = 1;
1058  double scale_sat = 1;
1059  if (vel_rot_max > getMaxRotationVelocity())
1060  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1061  if (scale_rot_sat < 1)
1062  scale_sat = scale_rot_sat;
1063 
1064  set_artVel(articularVelocity * scale_sat);
1065  break;
1066  }
1069  case vpRobot::MIXT_FRAME: {
1070  break;
1071  }
1072  }
1073 }
1074 
1121 {
1122  vel.resize(6);
1123 
1124  vpColVector articularCoordinates = get_artCoord();
1125  vpColVector articularVelocity = get_artVel();
1126 
1127  switch (frame) {
1128  case vpRobot::CAMERA_FRAME: {
1129  vpMatrix eJe_;
1131  vpAfma6::get_eJe(articularCoordinates, eJe_);
1132  vel = cVe * eJe_ * articularVelocity;
1133  break;
1134  }
1135  case vpRobot::ARTICULAR_FRAME: {
1136  vel = articularVelocity;
1137  break;
1138  }
1139  case vpRobot::REFERENCE_FRAME: {
1140  vpMatrix fJe_;
1141  vpAfma6::get_fJe(articularCoordinates, fJe_);
1142  vel = fJe_ * articularVelocity;
1143  break;
1144  }
1146  case vpRobot::MIXT_FRAME: {
1147  break;
1148  }
1149  default: {
1150  vpERROR_TRACE("Error in spec of vpRobot. "
1151  "Case not taken in account.");
1152  return;
1153  }
1154  }
1155 }
1156 
1174 {
1175  timestamp = vpTime::measureTimeSecond();
1176  getVelocity(frame, vel);
1177 }
1178 
1221 {
1222  vpColVector vel(6);
1223  getVelocity(frame, vel);
1224 
1225  return vel;
1226 }
1227 
1241 {
1242  timestamp = vpTime::measureTimeSecond();
1243  vpColVector vel(6);
1244  getVelocity(frame, vel);
1245 
1246  return vel;
1247 }
1248 
1250 {
1251  double vel_rot_max = getMaxRotationVelocity();
1252  double velmax = fabs(q[0]);
1253  for (unsigned int i = 1; i < 6; i++) {
1254  if (velmax < fabs(q[i]))
1255  velmax = fabs(q[i]);
1256  }
1257 
1258  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1259  q = q * alpha;
1260 }
1261 
1337 {
1339  vpERROR_TRACE("Robot was not in position-based control\n"
1340  "Modification of the robot state");
1341  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1342  }
1343 
1344  vpColVector articularCoordinates = get_artCoord();
1345 
1346  vpColVector error(6);
1347  double errsqr = 0;
1348  switch (frame) {
1349  case vpRobot::CAMERA_FRAME: {
1350  int nbSol;
1351  vpColVector qdes(6);
1352 
1353  vpTranslationVector txyz;
1354  vpRxyzVector rxyz;
1355  for (unsigned int i = 0; i < 3; i++) {
1356  txyz[i] = q[i];
1357  rxyz[i] = q[i + 3];
1358  }
1359 
1360  vpRotationMatrix cRc2(rxyz);
1361  vpHomogeneousMatrix cMc2(txyz, cRc2);
1362 
1363  vpHomogeneousMatrix fMc_;
1364  vpAfma6::get_fMc(articularCoordinates, fMc_);
1365 
1366  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1367 
1368  do {
1369  articularCoordinates = get_artCoord();
1370  qdes = articularCoordinates;
1371  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1372  setVelocityCalled = true;
1373  if (nbSol > 0) {
1374  error = qdes - articularCoordinates;
1375  errsqr = error.sumSquare();
1376  // findHighestPositioningSpeed(error);
1377  set_artVel(error);
1378  if (errsqr < 1e-4) {
1379  set_artCoord(qdes);
1380  error = 0;
1381  set_artVel(error);
1382  set_velocity(error);
1383  break;
1384  }
1385  } else {
1386  vpERROR_TRACE("Positionning error.");
1387  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1388  }
1389  } while (errsqr > 1e-8 && nbSol > 0);
1390 
1391  break;
1392  }
1393 
1394  case vpRobot::ARTICULAR_FRAME: {
1395  do {
1396  articularCoordinates = get_artCoord();
1397  error = q - articularCoordinates;
1398  errsqr = error.sumSquare();
1399  // findHighestPositioningSpeed(error);
1400  set_artVel(error);
1401  setVelocityCalled = true;
1402  if (errsqr < 1e-4) {
1403  set_artCoord(q);
1404  error = 0;
1405  set_artVel(error);
1406  set_velocity(error);
1407  break;
1408  }
1409  } while (errsqr > 1e-8);
1410  break;
1411  }
1412 
1413  case vpRobot::REFERENCE_FRAME: {
1414  int nbSol;
1415  vpColVector qdes(6);
1416 
1417  vpTranslationVector txyz;
1418  vpRxyzVector rxyz;
1419  for (unsigned int i = 0; i < 3; i++) {
1420  txyz[i] = q[i];
1421  rxyz[i] = q[i + 3];
1422  }
1423 
1424  vpRotationMatrix fRc(rxyz);
1425  vpHomogeneousMatrix fMc_(txyz, fRc);
1426 
1427  do {
1428  articularCoordinates = get_artCoord();
1429  qdes = articularCoordinates;
1430  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1431  setVelocityCalled = true;
1432  if (nbSol > 0) {
1433  error = qdes - articularCoordinates;
1434  errsqr = error.sumSquare();
1435  // findHighestPositioningSpeed(error);
1436  set_artVel(error);
1437  if (errsqr < 1e-4) {
1438  set_artCoord(qdes);
1439  error = 0;
1440  set_artVel(error);
1441  set_velocity(error);
1442  break;
1443  }
1444  } else
1445  vpERROR_TRACE("Positionning error. Position unreachable");
1446  } while (errsqr > 1e-8 && nbSol > 0);
1447  break;
1448  }
1449  case vpRobot::MIXT_FRAME: {
1450  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1451  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1452  "MIXT_FRAME not implemented.");
1453  }
1455  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1456  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1457  "END_EFFECTOR_FRAME not implemented.");
1458  }
1459  }
1460 }
1461 
1524 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1525  double pos3, double pos4, double pos5, double pos6)
1526 {
1527  try {
1528  vpColVector position(6);
1529  position[0] = pos1;
1530  position[1] = pos2;
1531  position[2] = pos3;
1532  position[3] = pos4;
1533  position[4] = pos5;
1534  position[5] = pos6;
1535 
1536  setPosition(frame, position);
1537  } catch (...) {
1538  vpERROR_TRACE("Error caught");
1539  throw;
1540  }
1541 }
1542 
1577 void vpSimulatorAfma6::setPosition(const char *filename)
1578 {
1579  vpColVector q;
1580  bool ret;
1581 
1582  ret = this->readPosFile(filename, q);
1583 
1584  if (ret == false) {
1585  vpERROR_TRACE("Bad position in \"%s\"", filename);
1586  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1587  }
1590 }
1591 
1652 {
1653  q.resize(6);
1654 
1655  switch (frame) {
1656  case vpRobot::CAMERA_FRAME: {
1657  q = 0;
1658  break;
1659  }
1660 
1661  case vpRobot::ARTICULAR_FRAME: {
1662  q = get_artCoord();
1663  break;
1664  }
1665 
1666  case vpRobot::REFERENCE_FRAME: {
1667  vpHomogeneousMatrix fMc_;
1668  vpAfma6::get_fMc(get_artCoord(), fMc_);
1669 
1670  vpRotationMatrix fRc;
1671  fMc_.extract(fRc);
1672  vpRxyzVector rxyz(fRc);
1673 
1674  vpTranslationVector txyz;
1675  fMc_.extract(txyz);
1676 
1677  for (unsigned int i = 0; i < 3; i++) {
1678  q[i] = txyz[i];
1679  q[i + 3] = rxyz[i];
1680  }
1681  break;
1682  }
1683 
1684  case vpRobot::MIXT_FRAME: {
1685  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1686  "Mixt frame not implemented.");
1687  }
1689  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1690  "End-effector frame not implemented.");
1691  }
1692  }
1693 }
1694 
1722 {
1723  timestamp = vpTime::measureTimeSecond();
1724  getPosition(frame, q);
1725 }
1726 
1739 {
1740  vpColVector posRxyz;
1741  // recupere position en Rxyz
1742  this->getPosition(frame, posRxyz);
1743 
1744  // recupere le vecteur thetaU correspondant
1745  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1746 
1747  // remplit le vpPoseVector avec translation et rotation ThetaU
1748  for (unsigned int j = 0; j < 3; j++) {
1749  position[j] = posRxyz[j];
1750  position[j + 3] = RtuVect[j];
1751  }
1752 }
1753 
1765 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1766 {
1767  timestamp = vpTime::measureTimeSecond();
1768  getPosition(frame, position);
1769 }
1770 
1781 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1782 {
1783  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1784  vpTRACE("Joint limit vector has not a size of 6 !");
1785  return;
1786  }
1787 
1788  _joint_min[0] = limitMin[0];
1789  _joint_min[1] = limitMin[1];
1790  _joint_min[2] = limitMin[2];
1791  _joint_min[3] = limitMin[3];
1792  _joint_min[4] = limitMin[4];
1793  _joint_min[5] = limitMin[5];
1794 
1795  _joint_max[0] = limitMax[0];
1796  _joint_max[1] = limitMax[1];
1797  _joint_max[2] = limitMax[2];
1798  _joint_max[3] = limitMax[3];
1799  _joint_max[4] = limitMax[4];
1800  _joint_max[5] = limitMax[5];
1801 }
1802 
1809 {
1810  double q5 = q[4];
1811 
1812  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1813 
1814  if (cond) {
1815  J[0][3] = 0;
1816  J[0][4] = 0;
1817  J[1][3] = 0;
1818  J[1][4] = 0;
1819  J[3][3] = 0;
1820  J[3][4] = 0;
1821  J[5][3] = 0;
1822  J[5][4] = 0;
1823  return true;
1824  }
1825 
1826  return false;
1827 }
1828 
1833 {
1834  int artNumb = 0;
1835  double diff = 0;
1836  double difft = 0;
1837 
1838  vpColVector articularCoordinates = get_artCoord();
1839 
1840  for (unsigned int i = 0; i < 6; i++) {
1841  if (articularCoordinates[i] <= _joint_min[i]) {
1842  difft = _joint_min[i] - articularCoordinates[i];
1843  if (difft > diff) {
1844  diff = difft;
1845  artNumb = -(int)i - 1;
1846  }
1847  }
1848  }
1849 
1850  for (unsigned int i = 0; i < 6; i++) {
1851  if (articularCoordinates[i] >= _joint_max[i]) {
1852  difft = articularCoordinates[i] - _joint_max[i];
1853  if (difft > diff) {
1854  diff = difft;
1855  artNumb = (int)(i + 1);
1856  }
1857  }
1858  }
1859 
1860  if (artNumb != 0)
1861  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1862  << std::endl;
1863 
1864  return artNumb;
1865 }
1866 
1885 {
1886  displacement.resize(6);
1887  displacement = 0;
1888  vpColVector q_cur(6);
1889 
1890  q_cur = get_artCoord();
1891 
1892  if (!first_time_getdis) {
1893  switch (frame) {
1894  case vpRobot::CAMERA_FRAME: {
1895  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1896  return;
1897  }
1898  case vpRobot::ARTICULAR_FRAME: {
1899  displacement = q_cur - q_prev_getdis;
1900  break;
1901  }
1902  case vpRobot::REFERENCE_FRAME: {
1903  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1904  return;
1905  }
1906  case vpRobot::MIXT_FRAME: {
1907  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1908  return;
1909  }
1911  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1912  return;
1913  }
1914  }
1915  } else {
1916  first_time_getdis = false;
1917  }
1918 
1919  // Memorize the joint position for the next call
1920  q_prev_getdis = q_cur;
1921 }
1922 
1970 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1971 {
1972  std::ifstream fd(filename.c_str(), std::ios::in);
1973 
1974  if (!fd.is_open()) {
1975  return false;
1976  }
1977 
1978  std::string line;
1979  std::string key("R:");
1980  std::string id("#AFMA6 - Position");
1981  bool pos_found = false;
1982  int lineNum = 0;
1983 
1984  q.resize(njoint);
1985 
1986  while (std::getline(fd, line)) {
1987  lineNum++;
1988  if (lineNum == 1) {
1989  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1990  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1991  return false;
1992  }
1993  }
1994  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1995  continue;
1996  }
1997  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1998  // check if there are at least njoint values in the line
1999  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2000  if (chain.size() < njoint + 1) // try to split with tab separator
2001  chain = vpIoTools::splitChain(line, std::string("\t"));
2002  if (chain.size() < njoint + 1)
2003  continue;
2004 
2005  std::istringstream ss(line);
2006  std::string key_;
2007  ss >> key_;
2008  for (unsigned int i = 0; i < njoint; i++)
2009  ss >> q[i];
2010  pos_found = true;
2011  break;
2012  }
2013  }
2014 
2015  // converts rotations from degrees into radians
2016  q[3] = vpMath::rad(q[3]);
2017  q[4] = vpMath::rad(q[4]);
2018  q[5] = vpMath::rad(q[5]);
2019 
2020  fd.close();
2021 
2022  if (!pos_found) {
2023  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2024  return false;
2025  }
2026 
2027  return true;
2028 }
2029 
2052 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2053 {
2054  FILE *fd;
2055  fd = fopen(filename.c_str(), "w");
2056  if (fd == NULL)
2057  return false;
2058 
2059  fprintf(fd, "\
2060 #AFMA6 - Position - Version 2.01\n\
2061 #\n\
2062 # R: X Y Z A B C\n\
2063 # Joint position: X, Y, Z: translations in meters\n\
2064 # A, B, C: rotations in degrees\n\
2065 #\n\
2066 #\n\n");
2067 
2068  // Save positions in mm and deg
2069  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2070  vpMath::deg(q[5]));
2071 
2072  fclose(fd);
2073  return (true);
2074 }
2075 
2083 void vpSimulatorAfma6::move(const char *filename)
2084 {
2085  vpColVector q;
2086 
2087  try {
2088  this->readPosFile(filename, q);
2091  } catch (...) {
2092  throw;
2093  }
2094 }
2095 
2106 
2115 {
2116  vpHomogeneousMatrix cMe;
2117  vpAfma6::get_cMe(cMe);
2118 
2119  cVe.buildFrom(cMe);
2120 }
2121 
2132 {
2133  try {
2134  vpAfma6::get_eJe(get_artCoord(), eJe_);
2135  } catch (...) {
2136  vpERROR_TRACE("catch exception ");
2137  throw;
2138  }
2139 }
2140 
2152 {
2153  try {
2154  vpColVector articularCoordinates = get_artCoord();
2155  vpAfma6::get_fJe(articularCoordinates, fJe_);
2156  } catch (...) {
2157  vpERROR_TRACE("Error caught");
2158  throw;
2159  }
2160 }
2161 
2166 {
2168  return;
2169 
2170  vpColVector stop(6);
2171  stop = 0;
2172  set_artVel(stop);
2173  set_velocity(stop);
2175 }
2176 
2177 /**********************************************************************************/
2178 /**********************************************************************************/
2179 /**********************************************************************************/
2180 /**********************************************************************************/
2181 
2191 {
2192  // set scene_dir from #define VISP_SCENE_DIR if it exists
2193  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2194  std::string scene_dir_;
2195  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2196  bool sceneDirExists = false;
2197  for (size_t i = 0; i < scene_dirs.size(); i++)
2198  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2199  scene_dir_ = scene_dirs[i];
2200  sceneDirExists = true;
2201  break;
2202  }
2203  if (!sceneDirExists) {
2204  try {
2205  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2206  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2207  } catch (...) {
2208  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2209  }
2210  }
2211 
2212  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2213  if (scene_dir_.size() > FILENAME_MAX)
2214  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2215  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2216  if (full_length > FILENAME_MAX)
2217  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2218 
2219  char *name_cam = new char[full_length];
2220 
2221  strcpy(name_cam, scene_dir_.c_str());
2222  strcat(name_cam, "/camera.bnd");
2223  set_scene(name_cam, &camera, cameraFactor);
2224 
2225  if (arm_dir.size() > FILENAME_MAX)
2226  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2227  full_length = (unsigned int)arm_dir.size() + name_length;
2228  if (full_length > FILENAME_MAX)
2229  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2230 
2231  char *name_arm = new char[full_length];
2232  strcpy(name_arm, arm_dir.c_str());
2233  strcat(name_arm, "/afma6_gate.bnd");
2234  std::cout << "name arm: " << name_arm << std::endl;
2235  set_scene(name_arm, robotArms, 1.0);
2236  strcpy(name_arm, arm_dir.c_str());
2237  strcat(name_arm, "/afma6_arm1.bnd");
2238  set_scene(name_arm, robotArms + 1, 1.0);
2239  strcpy(name_arm, arm_dir.c_str());
2240  strcat(name_arm, "/afma6_arm2.bnd");
2241  set_scene(name_arm, robotArms + 2, 1.0);
2242  strcpy(name_arm, arm_dir.c_str());
2243  strcat(name_arm, "/afma6_arm3.bnd");
2244  set_scene(name_arm, robotArms + 3, 1.0);
2245  strcpy(name_arm, arm_dir.c_str());
2246  strcat(name_arm, "/afma6_arm4.bnd");
2247  set_scene(name_arm, robotArms + 4, 1.0);
2248 
2250  tool = getToolType();
2251  strcpy(name_arm, arm_dir.c_str());
2252  switch (tool) {
2253  case vpAfma6::TOOL_CCMOP: {
2254  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2255  break;
2256  }
2257  case vpAfma6::TOOL_GRIPPER: {
2258  strcat(name_arm, "/afma6_tool_gripper.bnd");
2259  break;
2260  }
2261  case vpAfma6::TOOL_VACUUM: {
2262  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2263  break;
2264  }
2265  case vpAfma6::TOOL_CUSTOM: {
2266  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2267  break;
2268  }
2270  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2271  break;
2272  }
2274  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2275  break;
2276  }
2277  }
2278  set_scene(name_arm, robotArms + 5, 1.0);
2279 
2280  add_rfstack(IS_BACK);
2281 
2282  add_vwstack("start", "depth", 0.0, 100.0);
2283  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2284  add_vwstack("start", "type", PERSPECTIVE);
2285  //
2286  // sceneInitialized = true;
2287  // displayObject = true;
2288  displayCamera = true;
2289 
2290  delete[] name_cam;
2291  delete[] name_arm;
2292 }
2293 
2295 {
2296  bool changed = false;
2297  vpHomogeneousMatrix displacement = navigation(I_, changed);
2298 
2299  // if (displacement[2][3] != 0)
2300  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2301  camMf2 = camMf2 * displacement;
2302 
2303  f2Mf = camMf2.inverse() * camMf;
2304 
2305  camMf = camMf2 * displacement * f2Mf;
2306 
2307  double u;
2308  double v;
2309  // if(px_ext != 1 && py_ext != 1)
2310  // we assume px_ext and py_ext > 0
2311  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2312  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2313  u = (double)I_.getWidth() / (2 * px_ext);
2314  v = (double)I_.getHeight() / (2 * py_ext);
2315  } else {
2316  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2317  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2318  }
2319 
2320  float w44o[4][4], w44cext[4][4], x, y, z;
2321 
2322  vp2jlc_matrix(camMf.inverse(), w44cext);
2323 
2324  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2325  x = w44cext[2][0] + w44cext[3][0];
2326  y = w44cext[2][1] + w44cext[3][1];
2327  z = w44cext[2][2] + w44cext[3][2];
2328  add_vwstack("start", "vrp", x, y, z);
2329  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2330  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2331  add_vwstack("start", "window", -u, u, -v, v);
2332 
2333  vpHomogeneousMatrix fMit[8];
2334  get_fMi(fMit);
2335 
2336  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2337  display_scene(w44o, robotArms[0], I_, curColor);
2338 
2339  vp2jlc_matrix(fMit[0], w44o);
2340  display_scene(w44o, robotArms[1], I_, curColor);
2341 
2342  vp2jlc_matrix(fMit[2], w44o);
2343  display_scene(w44o, robotArms[2], I_, curColor);
2344 
2345  vp2jlc_matrix(fMit[3], w44o);
2346  display_scene(w44o, robotArms[3], I_, curColor);
2347 
2348  vp2jlc_matrix(fMit[4], w44o);
2349  display_scene(w44o, robotArms[4], I_, curColor);
2350 
2351  vp2jlc_matrix(fMit[5], w44o);
2352  display_scene(w44o, robotArms[5], I_, curColor);
2353 
2354  if (displayCamera) {
2355  vpHomogeneousMatrix cMe;
2356  get_cMe(cMe);
2357  cMe = cMe.inverse();
2358  cMe = fMit[6] * cMe;
2359  vp2jlc_matrix(cMe, w44o);
2360  display_scene(w44o, camera, I_, camColor);
2361  }
2362 
2363  if (displayObject) {
2364  vp2jlc_matrix(fMo, w44o);
2365  display_scene(w44o, scene, I_, curColor);
2366  }
2367 }
2368 
2387 {
2388  vpColVector stop(6);
2389  bool status = true;
2390  stop = 0;
2391  set_artVel(stop);
2392  set_velocity(stop);
2393  vpHomogeneousMatrix fMc_;
2394  fMc_ = fMo * cMo_.inverse();
2395 
2396  vpColVector articularCoordinates = get_artCoord();
2397  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2398 
2399  if (nbSol == 0) {
2400  status = false;
2401  vpERROR_TRACE("Positionning error. Position unreachable");
2402  }
2403 
2404  if (verbose_)
2405  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2406 
2407  set_artCoord(articularCoordinates);
2408 
2409  compute_fMi();
2410 
2411  return status;
2412 }
2413 
2428 {
2429  vpColVector stop(6);
2430  stop = 0;
2431  set_artVel(stop);
2432  set_velocity(stop);
2433  vpHomogeneousMatrix fMit[8];
2434  get_fMi(fMit);
2435  fMo = fMit[7] * cMo_;
2436 }
2437 
2449 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2450 {
2451  // get rid of max velocity
2452  double vMax = getMaxTranslationVelocity();
2453  double wMax = getMaxRotationVelocity();
2454  setMaxTranslationVelocity(10. * vMax);
2455  setMaxRotationVelocity(10. * wMax);
2456 
2457  vpColVector v(3), w(3), vel(6);
2458  vpHomogeneousMatrix cdMc;
2459  vpTranslationVector cdTc;
2460  vpRotationMatrix cdRc;
2461  vpThetaUVector cdTUc;
2462  vpColVector err(6);
2463  err = 1.;
2464  const double lambda = 5.;
2465 
2467 
2468  unsigned int i, iter = 0;
2469  while ((iter++ < 300) & (err.frobeniusNorm() > errMax)) {
2470  double t = vpTime::measureTimeMs();
2471 
2472  // update image
2473  if (Iint != NULL) {
2474  vpDisplay::display(*Iint);
2475  getInternalView(*Iint);
2476  vpDisplay::flush(*Iint);
2477  }
2478 
2479  // update pose error
2480  cdMc = cdMo_ * get_cMo().inverse();
2481  cdMc.extract(cdRc);
2482  cdMc.extract(cdTc);
2483  cdTUc.buildFrom(cdRc);
2484 
2485  // compute v,w and velocity
2486  v = -lambda * cdRc.t() * cdTc;
2487  w = -lambda * cdTUc;
2488  for (i = 0; i < 3; ++i) {
2489  vel[i] = v[i];
2490  vel[i + 3] = w[i];
2491  err[i] = cdTc[i];
2492  err[i + 3] = cdTUc[i];
2493  }
2494 
2495  // update feat
2497 
2498  // wait for it
2499  vpTime::wait(t, 10);
2500  }
2501  vel = 0.;
2502  set_velocity(vel);
2503  set_artVel(vel);
2505  setMaxRotationVelocity(wMax);
2506 
2507  // std::cout << "setPosition: final error " << err.t() << std::endl;
2508  return (err.frobeniusNorm() <= errMax);
2509 }
2510 
2511 #elif !defined(VISP_BUILD_SHARED_LIBS)
2512 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2513 // no symbols
2514 void dummy_vpSimulatorAfma6(){};
2515 #endif
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Modelisation of Irisa&#39;s gantry robot named Afma6.
Definition: vpAfma6.h:78
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpRxyzVector _erc
Definition: vpAfma6.h:207
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
vpTranslationVector _etc
Definition: vpAfma6.h:206
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:194
Error that can be emited by the vpRobot class and its derivates.
double frobeniusNorm() const
vpHomogeneousMatrix getExternalCameraPosition() const
void get_eJe(vpMatrix &eJe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void get_cVe(vpVelocityTwistMatrix &cVe)
void move(const char *filename)
static void * launcher(void *arg)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT double measureTimeSecond()
Definition: vpTime.cpp:158
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:600
static const vpColor none
Definition: vpColor.h:229
Initialize the position controller.
Definition: vpRobot.h:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
unsigned int getRows() const
Definition: vpArray2D.h:289
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
static const double defaultPositioningVelocity
vpRowVector t() const
vpControlFrameType
Definition: vpRobot.h:75
static bool savePosFile(const std::string &filename, const vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:215
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:169
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
static const vpColor green
Definition: vpColor.h:220
vpHomogeneousMatrix fMo
static void flush(const vpImage< unsigned char > &I)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:775
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpRotationMatrix t() const
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void setMaxRotationVelocity(double maxVr)
Definition: vpRobot.cpp:260
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:194
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:107
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
Implementation of a rotation matrix and operations on such kind of matrices.
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Initialize the velocity controller.
Definition: vpRobot.h:66
static bool readPosFile(const std::string &filename, vpColVector &q)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
Initialize the acceleration controller.
Definition: vpRobot.h:68
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
vpHomogeneousMatrix f2Mf
void set_displayBusy(const bool &status)
static void display(const vpImage< unsigned char > &I)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void getInternalView(vpImage< vpRGBa > &I)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:932
Generic class defining intrinsic camera parameters.
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
bool singularityTest(const vpColVector &q, vpMatrix &J)
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1900
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:153
void setCameraParameters(const vpCameraParameters &cam)
double getSamplingTime() const
void set_velocity(const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1002
static double rad(double deg)
Definition: vpMath.h:110
void setExternalCameraParameters(const vpCameraParameters &cam)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
This class aims to be a basis used to create all the simulators of robots.
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
double getPositioningVelocity(void)
double _long_56
Definition: vpAfma6.h:202
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
Definition: vpMath.h:103
unsigned int getHeight() const
Definition: vpImage.h:188
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
void get_fJe(vpMatrix &fJe)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:888
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:209
double sumSquare() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:172
void get_cMe(vpHomogeneousMatrix &cMe)
double _joint_max[6]
Definition: vpAfma6.h:203
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
void set_artCoord(const vpColVector &coord)
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:86
void set_artVel(const vpColVector &vel)
void get_fMi(vpHomogeneousMatrix *fMit)
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:102
unsigned int getWidth() const
Definition: vpImage.h:246
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
void setMaxTranslationVelocity(double maxVt)
Definition: vpRobot.cpp:239
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
vpHomogeneousMatrix camMf2
double _joint_min[6]
Definition: vpAfma6.h:204
void findHighestPositioningSpeed(vpColVector &q)
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:353