Visual Servoing Platform  version 3.2.1 under development (2019-07-18)
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  }
393  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
394  }
395  }
396 
397  vpRotationMatrix eRc(_erc);
398  this->_eMc.buildFrom(_etc, eRc);
399 
400  setToolType(tool);
401  return;
402 }
403 
414 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
415  const unsigned int &image_height)
416 {
417  if (toolCustom) {
418  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
419  }
420  // Set default parameters
421  switch (getToolType()) {
422  case vpAfma6::TOOL_CCMOP: {
423  // Set default intrinsic camera parameters for 640x480 images
424  if (image_width == 640 && image_height == 480) {
425  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
426  << std::endl;
427  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
428  } else {
429  vpTRACE("Cannot get default intrinsic camera parameters for this image "
430  "resolution");
431  }
432  break;
433  }
434  case vpAfma6::TOOL_GRIPPER: {
435  // Set default intrinsic camera parameters for 640x480 images
436  if (image_width == 640 && image_height == 480) {
437  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
438  << std::endl;
439  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
440  } else {
441  vpTRACE("Cannot get default intrinsic camera parameters for this image "
442  "resolution");
443  }
444  break;
445  }
448  case vpAfma6::TOOL_VACUUM: {
449  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
450  break;
451  }
452  default:
453  vpERROR_TRACE("This error should not occur!");
454  break;
455  }
456  return;
457 }
458 
468 {
469  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
470 }
471 
481 {
482  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
483 }
484 
491 {
492  px_int = cam.get_px();
493  py_int = cam.get_py();
494  toolCustom = true;
495 }
496 
502 {
503  double tcur_1 = tcur; // temporary variable used to store the last time
504  // since the last command
505 
506  while (!robotStop) {
507  // Get current time
508  tprev = tcur_1;
510 
512  setVelocityCalled = false;
513 
515 
516  double ellapsedTime = (tcur - tprev) * 1e-3;
517  if (constantSamplingTimeMode) { // if we want a constant velocity, we
518  // force the ellapsed time to the given
519  // samplingTime
520  ellapsedTime = getSamplingTime(); // in second
521  }
522 
523  vpColVector articularCoordinates = get_artCoord();
524  vpColVector articularVelocities = get_artVel();
525 
526  if (jointLimit) {
527  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
528  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
529  if (verbose_) {
530  std::cout << "Joint " << jointLimitArt - 1
531  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
532  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
533  }
534 
535  articularVelocities = 0.0;
536  } else
537  jointLimit = false;
538  }
539 
540  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
541  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
542  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
543  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
544  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
545  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
546 
547  int jl = isInJointLimit();
548 
549  if (jl != 0 && jointLimit == false) {
550  if (jl < 0)
551  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
552  (articularVelocities[(unsigned int)(-jl - 1)]);
553  else
554  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
555  (articularVelocities[(unsigned int)(jl - 1)]);
556 
557  for (unsigned int i = 0; i < 6; i++)
558  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
559 
560  jointLimit = true;
561  jointLimitArt = (unsigned int)fabs((double)jl);
562  }
563 
564  set_artCoord(articularCoordinates);
565  set_artVel(articularVelocities);
566 
567  compute_fMi();
568 
569  if (displayAllowed) {
573  }
574 
575  if (displayType == MODEL_3D && displayAllowed) {
576  while (get_displayBusy())
577  vpTime::wait(2);
579  set_displayBusy(false);
580  }
581 
582  if (0 /*displayType == MODEL_DH && displayAllowed*/) {
583  vpHomogeneousMatrix fMit[8];
584  get_fMi(fMit);
585 
586  // vpDisplay::displayFrame(I,getExternalCameraPosition
587  // ()*fMi[6],cameraParam,0.2,vpColor::none);
588 
589  vpImagePoint iP, iP_1;
590  vpPoint pt(0, 0, 0);
591 
594  pt.track(getExternalCameraPosition() * fMit[0]);
597  for (unsigned int k = 1; k < 7; k++) {
598  pt.track(getExternalCameraPosition() * fMit[k - 1]);
600 
601  pt.track(getExternalCameraPosition() * fMit[k]);
603 
605  }
607  thickness_);
608  }
609 
611 
612  vpTime::wait(tcur, 1000 * getSamplingTime());
613  tcur_1 = tcur;
614  } else {
616  }
617  }
618 }
619 
634 {
635  // vpColVector q = get_artCoord();
636  vpColVector q(6); //; = get_artCoord();
637  q = get_artCoord();
638 
639  vpHomogeneousMatrix fMit[8];
640 
641  double q1 = q[0];
642  double q2 = q[1];
643  double q3 = q[2];
644  double q4 = q[3];
645  double q5 = q[4];
646  double q6 = q[5];
647 
648  double c4 = cos(q4);
649  double s4 = sin(q4);
650  double c5 = cos(q5);
651  double s5 = sin(q5);
652  double c6 = cos(q6);
653  double s6 = sin(q6);
654 
655  fMit[0][0][0] = 1;
656  fMit[0][1][0] = 0;
657  fMit[0][2][0] = 0;
658  fMit[0][0][1] = 0;
659  fMit[0][1][1] = 1;
660  fMit[0][2][1] = 0;
661  fMit[0][0][2] = 0;
662  fMit[0][1][2] = 0;
663  fMit[0][2][2] = 1;
664  fMit[0][0][3] = q1;
665  fMit[0][1][3] = 0;
666  fMit[0][2][3] = 0;
667 
668  fMit[1][0][0] = 1;
669  fMit[1][1][0] = 0;
670  fMit[1][2][0] = 0;
671  fMit[1][0][1] = 0;
672  fMit[1][1][1] = 1;
673  fMit[1][2][1] = 0;
674  fMit[1][0][2] = 0;
675  fMit[1][1][2] = 0;
676  fMit[1][2][2] = 1;
677  fMit[1][0][3] = q1;
678  fMit[1][1][3] = q2;
679  fMit[1][2][3] = 0;
680 
681  fMit[2][0][0] = 1;
682  fMit[2][1][0] = 0;
683  fMit[2][2][0] = 0;
684  fMit[2][0][1] = 0;
685  fMit[2][1][1] = 1;
686  fMit[2][2][1] = 0;
687  fMit[2][0][2] = 0;
688  fMit[2][1][2] = 0;
689  fMit[2][2][2] = 1;
690  fMit[2][0][3] = q1;
691  fMit[2][1][3] = q2;
692  fMit[2][2][3] = q3;
693 
694  fMit[3][0][0] = s4;
695  fMit[3][1][0] = -c4;
696  fMit[3][2][0] = 0;
697  fMit[3][0][1] = c4;
698  fMit[3][1][1] = s4;
699  fMit[3][2][1] = 0;
700  fMit[3][0][2] = 0;
701  fMit[3][1][2] = 0;
702  fMit[3][2][2] = 1;
703  fMit[3][0][3] = q1;
704  fMit[3][1][3] = q2;
705  fMit[3][2][3] = q3;
706 
707  fMit[4][0][0] = s4 * s5;
708  fMit[4][1][0] = -c4 * s5;
709  fMit[4][2][0] = c5;
710  fMit[4][0][1] = s4 * c5;
711  fMit[4][1][1] = -c4 * c5;
712  fMit[4][2][1] = -s5;
713  fMit[4][0][2] = c4;
714  fMit[4][1][2] = s4;
715  fMit[4][2][2] = 0;
716  fMit[4][0][3] = c4 * this->_long_56 + q1;
717  fMit[4][1][3] = s4 * this->_long_56 + q2;
718  fMit[4][2][3] = q3;
719 
720  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
721  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
722  fMit[5][2][0] = c5 * c6;
723  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
724  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
725  fMit[5][2][1] = -c5 * s6;
726  fMit[5][0][2] = -s4 * c5;
727  fMit[5][1][2] = c4 * c5;
728  fMit[5][2][2] = s5;
729  fMit[5][0][3] = c4 * this->_long_56 + q1;
730  fMit[5][1][3] = s4 * this->_long_56 + q2;
731  fMit[5][2][3] = q3;
732 
733  fMit[6][0][0] = fMit[5][0][0];
734  fMit[6][1][0] = fMit[5][1][0];
735  fMit[6][2][0] = fMit[5][2][0];
736  fMit[6][0][1] = fMit[5][0][1];
737  fMit[6][1][1] = fMit[5][1][1];
738  fMit[6][2][1] = fMit[5][2][1];
739  fMit[6][0][2] = fMit[5][0][2];
740  fMit[6][1][2] = fMit[5][1][2];
741  fMit[6][2][2] = fMit[5][2][2];
742  fMit[6][0][3] = fMit[5][0][3];
743  fMit[6][1][3] = fMit[5][1][3];
744  fMit[6][2][3] = fMit[5][2][3];
745 
746  // vpHomogeneousMatrix cMe;
747  // get_cMe(cMe);
748  // cMe = cMe.inverse();
749  // fMit[7] = fMit[6] * cMe;
750  vpAfma6::get_fMc(q, fMit[7]);
751 
752 #if defined(_WIN32)
753 #if defined(WINRT_8_1)
754  WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
755 #else // pure win32
756  WaitForSingleObject(mutex_fMi, INFINITE);
757 #endif
758  for (int i = 0; i < 8; i++)
759  fMi[i] = fMit[i];
760  ReleaseMutex(mutex_fMi);
761 #elif defined(VISP_HAVE_PTHREAD)
762  pthread_mutex_lock(&mutex_fMi);
763  for (int i = 0; i < 8; i++)
764  fMi[i] = fMit[i];
765  pthread_mutex_unlock(&mutex_fMi);
766 #endif
767 }
768 
775 {
776  switch (newState) {
777  case vpRobot::STATE_STOP: {
778  // Start primitive STOP only if the current state is Velocity
780  stopMotion();
781  }
782  break;
783  }
786  std::cout << "Change the control mode from velocity to position control.\n";
787  stopMotion();
788  } else {
789  // std::cout << "Change the control mode from stop to position
790  // control.\n";
791  }
792  break;
793  }
796  std::cout << "Change the control mode from stop to velocity control.\n";
797  }
798  break;
799  }
801  default:
802  break;
803  }
804 
805  return vpRobot::setRobotState(newState);
806 }
807 
883 {
885  vpERROR_TRACE("Cannot send a velocity to the robot "
886  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
888  "Cannot send a velocity to the robot "
889  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
890  }
891 
892  vpColVector vel_sat(6);
893 
894  double scale_sat = 1;
895  double vel_trans_max = getMaxTranslationVelocity();
896  double vel_rot_max = getMaxRotationVelocity();
897 
898  double vel_abs; // Absolute value
899 
900  // Velocity saturation
901  switch (frame) {
902  // saturation in cartesian space
905  if (vel.getRows() != 6) {
906  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
907  throw;
908  }
909 
910  for (unsigned int i = 0; i < 3; ++i) {
911  vel_abs = fabs(vel[i]);
912  if (vel_abs > vel_trans_max && !jointLimit) {
913  vel_trans_max = vel_abs;
914  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
915  "(axis nr. %d).",
916  vel[i], i + 1);
917  }
918 
919  vel_abs = fabs(vel[i + 3]);
920  if (vel_abs > vel_rot_max && !jointLimit) {
921  vel_rot_max = vel_abs;
922  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
923  "(axis nr. %d).",
924  vel[i + 3], i + 4);
925  }
926  }
927 
928  double scale_trans_sat = 1;
929  double scale_rot_sat = 1;
930  if (vel_trans_max > getMaxTranslationVelocity())
931  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
932 
933  if (vel_rot_max > getMaxRotationVelocity())
934  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
935 
936  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
937  if (scale_trans_sat < scale_rot_sat)
938  scale_sat = scale_trans_sat;
939  else
940  scale_sat = scale_rot_sat;
941  }
942  break;
943  }
944 
945  // saturation in joint space
947  if (vel.getRows() != 6) {
948  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
949  throw;
950  }
951  for (unsigned int i = 0; i < 6; ++i) {
952  vel_abs = fabs(vel[i]);
953  if (vel_abs > vel_rot_max && !jointLimit) {
954  vel_rot_max = vel_abs;
955  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
956  "(axis nr. %d).",
957  vel[i], i + 1);
958  }
959  }
960  double scale_rot_sat = 1;
961  if (vel_rot_max > getMaxRotationVelocity())
962  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
963  if (scale_rot_sat < 1)
964  scale_sat = scale_rot_sat;
965  break;
966  }
967  case vpRobot::MIXT_FRAME: {
968  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
969  "functionality not implemented");
970  }
972  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOT_FRAME frame:"
973  "functionality not implemented");
974  }
975  }
976 
977  set_velocity(vel * scale_sat);
978  setRobotFrame(frame);
979  setVelocityCalled = true;
980 }
981 
986 {
988 
989  double vel_rot_max = getMaxRotationVelocity();
990 
991  vpColVector articularCoordinates = get_artCoord();
992  vpColVector velocityframe = get_velocity();
993  vpColVector articularVelocity;
994 
995  switch (frame) {
996  case vpRobot::CAMERA_FRAME: {
997  vpMatrix eJe_;
999  vpAfma6::get_eJe(articularCoordinates, eJe_);
1000  eJe_ = eJe_.pseudoInverse();
1002  singularityTest(articularCoordinates, eJe_);
1003  articularVelocity = eJe_ * eVc * velocityframe;
1004  set_artVel(articularVelocity);
1005  break;
1006  }
1007  case vpRobot::REFERENCE_FRAME: {
1008  vpMatrix fJe_;
1009  vpAfma6::get_fJe(articularCoordinates, fJe_);
1010  fJe_ = fJe_.pseudoInverse();
1012  singularityTest(articularCoordinates, fJe_);
1013  articularVelocity = fJe_ * velocityframe;
1014  set_artVel(articularVelocity);
1015  break;
1016  }
1017  case vpRobot::ARTICULAR_FRAME: {
1018  articularVelocity = velocityframe;
1019  set_artVel(articularVelocity);
1020  break;
1021  }
1023  case vpRobot::MIXT_FRAME: {
1024  break;
1025  }
1026  }
1027 
1028  switch (frame) {
1029  case vpRobot::CAMERA_FRAME:
1030  case vpRobot::REFERENCE_FRAME: {
1031  for (unsigned int i = 0; i < 6; ++i) {
1032  double vel_abs = fabs(articularVelocity[i]);
1033  if (vel_abs > vel_rot_max && !jointLimit) {
1034  vel_rot_max = vel_abs;
1035  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
1036  "(axis nr. %d).",
1037  articularVelocity[i], i + 1);
1038  }
1039  }
1040  double scale_rot_sat = 1;
1041  double scale_sat = 1;
1042  if (vel_rot_max > getMaxRotationVelocity())
1043  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1044  if (scale_rot_sat < 1)
1045  scale_sat = scale_rot_sat;
1046 
1047  set_artVel(articularVelocity * scale_sat);
1048  break;
1049  }
1052  case vpRobot::MIXT_FRAME: {
1053  break;
1054  }
1055  }
1056 }
1057 
1105 {
1106  vel.resize(6);
1107 
1108  vpColVector articularCoordinates = get_artCoord();
1109  vpColVector articularVelocity = get_artVel();
1110 
1111  switch (frame) {
1112  case vpRobot::CAMERA_FRAME: {
1113  vpMatrix eJe_;
1115  vpAfma6::get_eJe(articularCoordinates, eJe_);
1116  vel = cVe * eJe_ * articularVelocity;
1117  break;
1118  }
1119  case vpRobot::ARTICULAR_FRAME: {
1120  vel = articularVelocity;
1121  break;
1122  }
1123  case vpRobot::REFERENCE_FRAME: {
1124  vpMatrix fJe_;
1125  vpAfma6::get_fJe(articularCoordinates, fJe_);
1126  vel = fJe_ * articularVelocity;
1127  break;
1128  }
1130  case vpRobot::MIXT_FRAME: {
1131  break;
1132  }
1133  default: {
1134  vpERROR_TRACE("Error in spec of vpRobot. "
1135  "Case not taken in account.");
1136  return;
1137  }
1138  }
1139 }
1140 
1158 {
1159  timestamp = vpTime::measureTimeSecond();
1160  getVelocity(frame, vel);
1161 }
1162 
1206 {
1207  vpColVector vel(6);
1208  getVelocity(frame, vel);
1209 
1210  return vel;
1211 }
1212 
1226 {
1227  timestamp = vpTime::measureTimeSecond();
1228  vpColVector vel(6);
1229  getVelocity(frame, vel);
1230 
1231  return vel;
1232 }
1233 
1235 {
1236  double vel_rot_max = getMaxRotationVelocity();
1237  double velmax = fabs(q[0]);
1238  for (unsigned int i = 1; i < 6; i++) {
1239  if (velmax < fabs(q[i]))
1240  velmax = fabs(q[i]);
1241  }
1242 
1243  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1244  q = q * alpha;
1245 }
1246 
1323 {
1325  vpERROR_TRACE("Robot was not in position-based control\n"
1326  "Modification of the robot state");
1327  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1328  }
1329 
1330  vpColVector articularCoordinates = get_artCoord();
1331 
1332  vpColVector error(6);
1333  double errsqr = 0;
1334  switch (frame) {
1335  case vpRobot::CAMERA_FRAME: {
1336  int nbSol;
1337  vpColVector qdes(6);
1338 
1339  vpTranslationVector txyz;
1340  vpRxyzVector rxyz;
1341  for (unsigned int i = 0; i < 3; i++) {
1342  txyz[i] = q[i];
1343  rxyz[i] = q[i + 3];
1344  }
1345 
1346  vpRotationMatrix cRc2(rxyz);
1347  vpHomogeneousMatrix cMc2(txyz, cRc2);
1348 
1349  vpHomogeneousMatrix fMc_;
1350  vpAfma6::get_fMc(articularCoordinates, fMc_);
1351 
1352  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1353 
1354  do {
1355  articularCoordinates = get_artCoord();
1356  qdes = articularCoordinates;
1357  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1358  setVelocityCalled = true;
1359  if (nbSol > 0) {
1360  error = qdes - articularCoordinates;
1361  errsqr = error.sumSquare();
1362  // findHighestPositioningSpeed(error);
1363  set_artVel(error);
1364  if (errsqr < 1e-4) {
1365  set_artCoord(qdes);
1366  error = 0;
1367  set_artVel(error);
1368  set_velocity(error);
1369  break;
1370  }
1371  } else {
1372  vpERROR_TRACE("Positionning error.");
1373  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1374  }
1375  } while (errsqr > 1e-8 && nbSol > 0);
1376 
1377  break;
1378  }
1379 
1380  case vpRobot::ARTICULAR_FRAME: {
1381  do {
1382  articularCoordinates = get_artCoord();
1383  error = q - articularCoordinates;
1384  errsqr = error.sumSquare();
1385  // findHighestPositioningSpeed(error);
1386  set_artVel(error);
1387  setVelocityCalled = true;
1388  if (errsqr < 1e-4) {
1389  set_artCoord(q);
1390  error = 0;
1391  set_artVel(error);
1392  set_velocity(error);
1393  break;
1394  }
1395  } while (errsqr > 1e-8);
1396  break;
1397  }
1398 
1399  case vpRobot::REFERENCE_FRAME: {
1400  int nbSol;
1401  vpColVector qdes(6);
1402 
1403  vpTranslationVector txyz;
1404  vpRxyzVector rxyz;
1405  for (unsigned int i = 0; i < 3; i++) {
1406  txyz[i] = q[i];
1407  rxyz[i] = q[i + 3];
1408  }
1409 
1410  vpRotationMatrix fRc(rxyz);
1411  vpHomogeneousMatrix fMc_(txyz, fRc);
1412 
1413  do {
1414  articularCoordinates = get_artCoord();
1415  qdes = articularCoordinates;
1416  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1417  setVelocityCalled = true;
1418  if (nbSol > 0) {
1419  error = qdes - articularCoordinates;
1420  errsqr = error.sumSquare();
1421  // findHighestPositioningSpeed(error);
1422  set_artVel(error);
1423  if (errsqr < 1e-4) {
1424  set_artCoord(qdes);
1425  error = 0;
1426  set_artVel(error);
1427  set_velocity(error);
1428  break;
1429  }
1430  } else
1431  vpERROR_TRACE("Positionning error. Position unreachable");
1432  } while (errsqr > 1e-8 && nbSol > 0);
1433  break;
1434  }
1435  case vpRobot::MIXT_FRAME: {
1436  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1437  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1438  "MIXT_FRAME not implemented.");
1439  }
1441  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1442  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1443  "END_EFFECTOR_FRAME not implemented.");
1444  }
1445  }
1446 }
1447 
1511 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, const double pos1, const double pos2,
1512  const double pos3, const double pos4, const double pos5, const double pos6)
1513 {
1514  try {
1515  vpColVector position(6);
1516  position[0] = pos1;
1517  position[1] = pos2;
1518  position[2] = pos3;
1519  position[3] = pos4;
1520  position[4] = pos5;
1521  position[5] = pos6;
1522 
1523  setPosition(frame, position);
1524  } catch (...) {
1525  vpERROR_TRACE("Error caught");
1526  throw;
1527  }
1528 }
1529 
1565 void vpSimulatorAfma6::setPosition(const char *filename)
1566 {
1567  vpColVector q;
1568  bool ret;
1569 
1570  ret = this->readPosFile(filename, q);
1571 
1572  if (ret == false) {
1573  vpERROR_TRACE("Bad position in \"%s\"", filename);
1574  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1575  }
1578 }
1579 
1641 {
1642  q.resize(6);
1643 
1644  switch (frame) {
1645  case vpRobot::CAMERA_FRAME: {
1646  q = 0;
1647  break;
1648  }
1649 
1650  case vpRobot::ARTICULAR_FRAME: {
1651  q = get_artCoord();
1652  break;
1653  }
1654 
1655  case vpRobot::REFERENCE_FRAME: {
1656  vpHomogeneousMatrix fMc_;
1657  vpAfma6::get_fMc(get_artCoord(), fMc_);
1658 
1659  vpRotationMatrix fRc;
1660  fMc_.extract(fRc);
1661  vpRxyzVector rxyz(fRc);
1662 
1663  vpTranslationVector txyz;
1664  fMc_.extract(txyz);
1665 
1666  for (unsigned int i = 0; i < 3; i++) {
1667  q[i] = txyz[i];
1668  q[i + 3] = rxyz[i];
1669  }
1670  break;
1671  }
1672 
1673  case vpRobot::MIXT_FRAME: {
1674  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1675  "Mixt frame not implemented.");
1676  }
1678  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1679  "End-effector frame not implemented.");
1680  }
1681  }
1682 }
1683 
1711 {
1712  timestamp = vpTime::measureTimeSecond();
1713  getPosition(frame, q);
1714 }
1715 
1728 {
1729  vpColVector posRxyz;
1730  // recupere position en Rxyz
1731  this->getPosition(frame, posRxyz);
1732 
1733  // recupere le vecteur thetaU correspondant
1734  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1735 
1736  // remplit le vpPoseVector avec translation et rotation ThetaU
1737  for (unsigned int j = 0; j < 3; j++) {
1738  position[j] = posRxyz[j];
1739  position[j + 3] = RtuVect[j];
1740  }
1741 }
1742 
1754 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1755 {
1756  timestamp = vpTime::measureTimeSecond();
1757  getPosition(frame, position);
1758 }
1759 
1770 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1771 {
1772  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1773  vpTRACE("Joint limit vector has not a size of 6 !");
1774  return;
1775  }
1776 
1777  _joint_min[0] = limitMin[0];
1778  _joint_min[1] = limitMin[1];
1779  _joint_min[2] = limitMin[2];
1780  _joint_min[3] = limitMin[3];
1781  _joint_min[4] = limitMin[4];
1782  _joint_min[5] = limitMin[5];
1783 
1784  _joint_max[0] = limitMax[0];
1785  _joint_max[1] = limitMax[1];
1786  _joint_max[2] = limitMax[2];
1787  _joint_max[3] = limitMax[3];
1788  _joint_max[4] = limitMax[4];
1789  _joint_max[5] = limitMax[5];
1790 }
1791 
1798 {
1799  double q5 = q[4];
1800 
1801  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1802 
1803  if (cond) {
1804  J[0][3] = 0;
1805  J[0][4] = 0;
1806  J[1][3] = 0;
1807  J[1][4] = 0;
1808  J[3][3] = 0;
1809  J[3][4] = 0;
1810  J[5][3] = 0;
1811  J[5][4] = 0;
1812  return true;
1813  }
1814 
1815  return false;
1816 }
1817 
1822 {
1823  int artNumb = 0;
1824  double diff = 0;
1825  double difft = 0;
1826 
1827  vpColVector articularCoordinates = get_artCoord();
1828 
1829  for (unsigned int i = 0; i < 6; i++) {
1830  if (articularCoordinates[i] <= _joint_min[i]) {
1831  difft = _joint_min[i] - articularCoordinates[i];
1832  if (difft > diff) {
1833  diff = difft;
1834  artNumb = -(int)i - 1;
1835  }
1836  }
1837  }
1838 
1839  for (unsigned int i = 0; i < 6; i++) {
1840  if (articularCoordinates[i] >= _joint_max[i]) {
1841  difft = articularCoordinates[i] - _joint_max[i];
1842  if (difft > diff) {
1843  diff = difft;
1844  artNumb = (int)(i + 1);
1845  }
1846  }
1847  }
1848 
1849  if (artNumb != 0)
1850  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1851  << std::endl;
1852 
1853  return artNumb;
1854 }
1855 
1874 {
1875  displacement.resize(6);
1876  displacement = 0;
1877  vpColVector q_cur(6);
1878 
1879  q_cur = get_artCoord();
1880 
1881  if (!first_time_getdis) {
1882  switch (frame) {
1883  case vpRobot::CAMERA_FRAME: {
1884  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1885  return;
1886  }
1887  case vpRobot::ARTICULAR_FRAME: {
1888  displacement = q_cur - q_prev_getdis;
1889  break;
1890  }
1891  case vpRobot::REFERENCE_FRAME: {
1892  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1893  return;
1894  }
1895  case vpRobot::MIXT_FRAME: {
1896  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1897  return;
1898  }
1900  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1901  return;
1902  }
1903  }
1904  } else {
1905  first_time_getdis = false;
1906  }
1907 
1908  // Memorize the joint position for the next call
1909  q_prev_getdis = q_cur;
1910 }
1911 
1959 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1960 {
1961  std::ifstream fd(filename.c_str(), std::ios::in);
1962 
1963  if (!fd.is_open()) {
1964  return false;
1965  }
1966 
1967  std::string line;
1968  std::string key("R:");
1969  std::string id("#AFMA6 - Position");
1970  bool pos_found = false;
1971  int lineNum = 0;
1972 
1973  q.resize(njoint);
1974 
1975  while (std::getline(fd, line)) {
1976  lineNum++;
1977  if (lineNum == 1) {
1978  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1979  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1980  return false;
1981  }
1982  }
1983  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1984  continue;
1985  }
1986  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1987  // check if there are at least njoint values in the line
1988  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1989  if (chain.size() < njoint + 1) // try to split with tab separator
1990  chain = vpIoTools::splitChain(line, std::string("\t"));
1991  if (chain.size() < njoint + 1)
1992  continue;
1993 
1994  std::istringstream ss(line);
1995  std::string key_;
1996  ss >> key_;
1997  for (unsigned int i = 0; i < njoint; i++)
1998  ss >> q[i];
1999  pos_found = true;
2000  break;
2001  }
2002  }
2003 
2004  // converts rotations from degrees into radians
2005  q[3] = vpMath::rad(q[3]);
2006  q[4] = vpMath::rad(q[4]);
2007  q[5] = vpMath::rad(q[5]);
2008 
2009  fd.close();
2010 
2011  if (!pos_found) {
2012  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2013  return false;
2014  }
2015 
2016  return true;
2017 }
2018 
2041 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2042 {
2043  FILE *fd;
2044  fd = fopen(filename.c_str(), "w");
2045  if (fd == NULL)
2046  return false;
2047 
2048  fprintf(fd, "\
2049 #AFMA6 - Position - Version 2.01\n\
2050 #\n\
2051 # R: X Y Z A B C\n\
2052 # Joint position: X, Y, Z: translations in meters\n\
2053 # A, B, C: rotations in degrees\n\
2054 #\n\
2055 #\n\n");
2056 
2057  // Save positions in mm and deg
2058  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2059  vpMath::deg(q[5]));
2060 
2061  fclose(fd);
2062  return (true);
2063 }
2064 
2072 void vpSimulatorAfma6::move(const char *filename)
2073 {
2074  vpColVector q;
2075 
2076  try {
2077  this->readPosFile(filename, q);
2080  } catch (...) {
2081  throw;
2082  }
2083 }
2084 
2095 
2104 {
2105  vpHomogeneousMatrix cMe;
2106  vpAfma6::get_cMe(cMe);
2107 
2108  cVe.buildFrom(cMe);
2109 }
2110 
2121 {
2122  try {
2123  vpAfma6::get_eJe(get_artCoord(), eJe_);
2124  } catch (...) {
2125  vpERROR_TRACE("catch exception ");
2126  throw;
2127  }
2128 }
2129 
2141 {
2142  try {
2143  vpColVector articularCoordinates = get_artCoord();
2144  vpAfma6::get_fJe(articularCoordinates, fJe_);
2145  } catch (...) {
2146  vpERROR_TRACE("Error caught");
2147  throw;
2148  }
2149 }
2150 
2155 {
2157  return;
2158 
2159  vpColVector stop(6);
2160  stop = 0;
2161  set_artVel(stop);
2162  set_velocity(stop);
2164 }
2165 
2166 /**********************************************************************************/
2167 /**********************************************************************************/
2168 /**********************************************************************************/
2169 /**********************************************************************************/
2170 
2180 {
2181  // set scene_dir from #define VISP_SCENE_DIR if it exists
2182  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2183  std::string scene_dir_;
2184  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2185  bool sceneDirExists = false;
2186  for (size_t i = 0; i < scene_dirs.size(); i++)
2187  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2188  scene_dir_ = scene_dirs[i];
2189  sceneDirExists = true;
2190  break;
2191  }
2192  if (!sceneDirExists) {
2193  try {
2194  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2195  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2196  } catch (...) {
2197  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2198  }
2199  }
2200 
2201  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2202  if (scene_dir_.size() > FILENAME_MAX)
2203  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2204  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2205  if (full_length > FILENAME_MAX)
2206  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2207 
2208  char *name_cam = new char[full_length];
2209 
2210  strcpy(name_cam, scene_dir_.c_str());
2211  strcat(name_cam, "/camera.bnd");
2212  set_scene(name_cam, &camera, cameraFactor);
2213 
2214  if (arm_dir.size() > FILENAME_MAX)
2215  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2216  full_length = (unsigned int)arm_dir.size() + name_length;
2217  if (full_length > FILENAME_MAX)
2218  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2219 
2220  char *name_arm = new char[full_length];
2221  strcpy(name_arm, arm_dir.c_str());
2222  strcat(name_arm, "/afma6_gate.bnd");
2223  std::cout << "name arm: " << name_arm << std::endl;
2224  set_scene(name_arm, robotArms, 1.0);
2225  strcpy(name_arm, arm_dir.c_str());
2226  strcat(name_arm, "/afma6_arm1.bnd");
2227  set_scene(name_arm, robotArms + 1, 1.0);
2228  strcpy(name_arm, arm_dir.c_str());
2229  strcat(name_arm, "/afma6_arm2.bnd");
2230  set_scene(name_arm, robotArms + 2, 1.0);
2231  strcpy(name_arm, arm_dir.c_str());
2232  strcat(name_arm, "/afma6_arm3.bnd");
2233  set_scene(name_arm, robotArms + 3, 1.0);
2234  strcpy(name_arm, arm_dir.c_str());
2235  strcat(name_arm, "/afma6_arm4.bnd");
2236  set_scene(name_arm, robotArms + 4, 1.0);
2237 
2239  tool = getToolType();
2240  strcpy(name_arm, arm_dir.c_str());
2241  switch (tool) {
2242  case vpAfma6::TOOL_CCMOP: {
2243  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2244  break;
2245  }
2246  case vpAfma6::TOOL_GRIPPER: {
2247  strcat(name_arm, "/afma6_tool_gripper.bnd");
2248  break;
2249  }
2250  case vpAfma6::TOOL_VACUUM: {
2251  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2252  break;
2253  }
2254  case vpAfma6::TOOL_CUSTOM: {
2255  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2256  break;
2257  }
2259  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2260  break;
2261  }
2262  }
2263  set_scene(name_arm, robotArms + 5, 1.0);
2264 
2265  add_rfstack(IS_BACK);
2266 
2267  add_vwstack("start", "depth", 0.0, 100.0);
2268  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2269  add_vwstack("start", "type", PERSPECTIVE);
2270  //
2271  // sceneInitialized = true;
2272  // displayObject = true;
2273  displayCamera = true;
2274 
2275  delete[] name_cam;
2276  delete[] name_arm;
2277 }
2278 
2280 {
2281  bool changed = false;
2282  vpHomogeneousMatrix displacement = navigation(I_, changed);
2283 
2284  // if (displacement[2][3] != 0)
2285  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2286  camMf2 = camMf2 * displacement;
2287 
2288  f2Mf = camMf2.inverse() * camMf;
2289 
2290  camMf = camMf2 * displacement * f2Mf;
2291 
2292  double u;
2293  double v;
2294  // if(px_ext != 1 && py_ext != 1)
2295  // we assume px_ext and py_ext > 0
2296  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2297  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2298  u = (double)I_.getWidth() / (2 * px_ext);
2299  v = (double)I_.getHeight() / (2 * py_ext);
2300  } else {
2301  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2302  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2303  }
2304 
2305  float w44o[4][4], w44cext[4][4], x, y, z;
2306 
2307  vp2jlc_matrix(camMf.inverse(), w44cext);
2308 
2309  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2310  x = w44cext[2][0] + w44cext[3][0];
2311  y = w44cext[2][1] + w44cext[3][1];
2312  z = w44cext[2][2] + w44cext[3][2];
2313  add_vwstack("start", "vrp", x, y, z);
2314  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2315  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2316  add_vwstack("start", "window", -u, u, -v, v);
2317 
2318  vpHomogeneousMatrix fMit[8];
2319  get_fMi(fMit);
2320 
2321  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2322  display_scene(w44o, robotArms[0], I_, curColor);
2323 
2324  vp2jlc_matrix(fMit[0], w44o);
2325  display_scene(w44o, robotArms[1], I_, curColor);
2326 
2327  vp2jlc_matrix(fMit[2], w44o);
2328  display_scene(w44o, robotArms[2], I_, curColor);
2329 
2330  vp2jlc_matrix(fMit[3], w44o);
2331  display_scene(w44o, robotArms[3], I_, curColor);
2332 
2333  vp2jlc_matrix(fMit[4], w44o);
2334  display_scene(w44o, robotArms[4], I_, curColor);
2335 
2336  vp2jlc_matrix(fMit[5], w44o);
2337  display_scene(w44o, robotArms[5], I_, curColor);
2338 
2339  if (displayCamera) {
2340  vpHomogeneousMatrix cMe;
2341  get_cMe(cMe);
2342  cMe = cMe.inverse();
2343  cMe = fMit[6] * cMe;
2344  vp2jlc_matrix(cMe, w44o);
2345  display_scene(w44o, camera, I_, camColor);
2346  }
2347 
2348  if (displayObject) {
2349  vp2jlc_matrix(fMo, w44o);
2350  display_scene(w44o, scene, I_, curColor);
2351  }
2352 }
2353 
2372 {
2373  vpColVector stop(6);
2374  bool status = true;
2375  stop = 0;
2376  set_artVel(stop);
2377  set_velocity(stop);
2378  vpHomogeneousMatrix fMc_;
2379  fMc_ = fMo * cMo_.inverse();
2380 
2381  vpColVector articularCoordinates = get_artCoord();
2382  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2383 
2384  if (nbSol == 0) {
2385  status = false;
2386  vpERROR_TRACE("Positionning error. Position unreachable");
2387  }
2388 
2389  if (verbose_)
2390  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2391 
2392  set_artCoord(articularCoordinates);
2393 
2394  compute_fMi();
2395 
2396  return status;
2397 }
2398 
2413 {
2414  vpColVector stop(6);
2415  stop = 0;
2416  set_artVel(stop);
2417  set_velocity(stop);
2418  vpHomogeneousMatrix fMit[8];
2419  get_fMi(fMit);
2420  fMo = fMit[7] * cMo_;
2421 }
2422 
2434 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2435 {
2436  // get rid of max velocity
2437  double vMax = getMaxTranslationVelocity();
2438  double wMax = getMaxRotationVelocity();
2439  setMaxTranslationVelocity(10. * vMax);
2440  setMaxRotationVelocity(10. * wMax);
2441 
2442  vpColVector v(3), w(3), vel(6);
2443  vpHomogeneousMatrix cdMc;
2444  vpTranslationVector cdTc;
2445  vpRotationMatrix cdRc;
2446  vpThetaUVector cdTUc;
2447  vpColVector err(6);
2448  err = 1.;
2449  const double lambda = 5.;
2450 
2452 
2453  unsigned int i, iter = 0;
2454  while ((iter++ < 300) & (err.frobeniusNorm() > errMax)) {
2455  double t = vpTime::measureTimeMs();
2456 
2457  // update image
2458  if (Iint != NULL) {
2459  vpDisplay::display(*Iint);
2460  getInternalView(*Iint);
2461  vpDisplay::flush(*Iint);
2462  }
2463 
2464  // update pose error
2465  cdMc = cdMo_ * get_cMo().inverse();
2466  cdMc.extract(cdRc);
2467  cdMc.extract(cdTc);
2468  cdTUc.buildFrom(cdRc);
2469 
2470  // compute v,w and velocity
2471  v = -lambda * cdRc.t() * cdTc;
2472  w = -lambda * cdTUc;
2473  for (i = 0; i < 3; ++i) {
2474  vel[i] = v[i];
2475  vel[i + 3] = w[i];
2476  err[i] = cdTc[i];
2477  err[i + 3] = cdTUc[i];
2478  }
2479 
2480  // update feat
2482 
2483  // wait for it
2484  vpTime::wait(t, 10);
2485  }
2486  vel = 0.;
2487  set_velocity(vel);
2488  set_artVel(vel);
2490  setMaxRotationVelocity(wMax);
2491 
2492  // std::cout << "setPosition: final error " << err.t() << std::endl;
2493  return (err.frobeniusNorm() <= errMax);
2494 }
2495 
2496 #elif !defined(VISP_BUILD_SHARED_LIBS)
2497 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2498 // no symbols
2499 void dummy_vpSimulatorAfma6(){};
2500 #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:164
double frobeniusNorm() const
vpRxyzVector _erc
Definition: vpAfma6.h:198
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
vpTranslationVector _etc
Definition: vpAfma6.h:197
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:185
Error that can be emited by the vpRobot class and its derivates.
unsigned int getWidth() const
Definition: vpImage.h:244
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:239
void get_eJe(vpMatrix &eJe)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:532
double getSamplingTime() const
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:160
Implementation of an homogeneous matrix and operations on such kind of matrices.
void get_cVe(vpVelocityTwistMatrix &cVe)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:707
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:250
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
static const vpColor none
Definition: vpColor.h:192
Initialize the position controller.
Definition: vpRobot.h:67
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:864
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:118
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void track(const vpHomogeneousMatrix &cMo)
static const double defaultPositioningVelocity
vpRotationMatrix t() const
vpControlFrameType
Definition: vpRobot.h:75
double get_py() const
static bool savePosFile(const std::string &filename, const vpColVector &q)
void getExternalImage(vpImage< vpRGBa > &I)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:206
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
static const vpColor green
Definition: vpColor.h:183
vpHomogeneousMatrix fMo
static void flush(const vpImage< unsigned char > &I)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Class that defines what is a point.
Definition: vpPoint.h:58
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:185
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:105
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
Implementation of a rotation matrix and operations on such kind of matrices.
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:441
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
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)
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 get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:820
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
vpRowVector t() const
void getInternalView(vpImage< vpRGBa > &I)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix getExternalCameraPosition() const
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
bool singularityTest(const vpColVector &q, vpMatrix &J)
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1786
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
void setCameraParameters(const vpCameraParameters &cam)
unsigned int getRows() const
Definition: vpArray2D.h:289
void set_velocity(const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double get_px() const
static double rad(double deg)
Definition: vpMath.h:108
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:172
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 setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:260
double sumSquare() const
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
double getPositioningVelocity(void)
double _long_56
Definition: vpAfma6.h:193
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
Definition: vpMath.h:101
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))
void get_fJe(vpMatrix &fJe)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:200
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
unsigned int getHeight() const
Definition: vpImage.h:186
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
void get_cMe(vpHomogeneousMatrix &cMe)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
double _joint_max[6]
Definition: vpAfma6.h:194
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_artCoord(const vpColVector &coord)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:80
void set_artVel(const vpColVector &vel)
void get_fMi(vpHomogeneousMatrix *fMit)
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:100
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:934
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:195
void findHighestPositioningSpeed(vpColVector &q)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:374