ViSP
vpSimulatorAfma6.cpp
1 /****************************************************************************
2  *
3  * $Id: vpSimulatorAfma6.cpp 2595 2010-06-02 08:50:59Z nmelchio $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Class which provides a simulator for the robot Afma6.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 
43 
44 #include <visp/vpConfig.h>
45 #if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
46 #include <visp/vpSimulatorAfma6.h>
47 #include <visp/vpTime.h>
48 #include <visp/vpImagePoint.h>
49 #include <visp/vpPoint.h>
50 #include <visp/vpMeterPixelConversion.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpIoTools.h>
53 #include <cmath> // std::fabs
54 #include <limits> // numeric_limits
55 #include <string>
57 
58 
64  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
65  zeroPos(), reposPos(), toolCustom(false), arm_dir()
66 {
67  init();
68  initDisplay();
69 
71 
72  #if defined(_WIN32)
73  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
74  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
75  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
76  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
77  mutex_display = CreateMutex(NULL,FALSE,NULL);
78 
79 
80  DWORD dwThreadIdArray;
81  hThread = CreateThread(
82  NULL, // default security attributes
83  0, // use default stack size
84  launcher, // thread function name
85  this, // argument to thread function
86  0, // use default creation flags
87  &dwThreadIdArray); // returns the thread identifier
88  #elif defined (VISP_HAVE_PTHREAD)
89  pthread_mutex_init(&mutex_fMi, NULL);
90  pthread_mutex_init(&mutex_artVel, NULL);
91  pthread_mutex_init(&mutex_artCoord, NULL);
92  pthread_mutex_init(&mutex_velocity, NULL);
93  pthread_mutex_init(&mutex_display, NULL);
94 
95  pthread_attr_init(&attr);
96  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
97 
98  pthread_create(&thread, NULL, launcher, (void *)this);
99  #endif
100 
101  compute_fMi();
102 }
103 
111  : vpRobotWireFrameSimulator(do_display),
112  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
113  zeroPos(), reposPos(), toolCustom(false), arm_dir()
114 {
115  init();
116  initDisplay();
117 
119 
120  #if defined(_WIN32)
121  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
122  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
123  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
124  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
125  mutex_display = CreateMutex(NULL,FALSE,NULL);
126 
127 
128  DWORD dwThreadIdArray;
129  hThread = CreateThread(
130  NULL, // default security attributes
131  0, // use default stack size
132  launcher, // thread function name
133  this, // argument to thread function
134  0, // use default creation flags
135  &dwThreadIdArray); // returns the thread identifier
136  #elif defined(VISP_HAVE_PTHREAD)
137  pthread_mutex_init(&mutex_fMi, NULL);
138  pthread_mutex_init(&mutex_artVel, NULL);
139  pthread_mutex_init(&mutex_artCoord, NULL);
140  pthread_mutex_init(&mutex_velocity, NULL);
141  pthread_mutex_init(&mutex_display, NULL);
142 
143  pthread_attr_init(&attr);
144  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
145 
146  pthread_create(&thread, NULL, launcher, (void *)this);
147  #endif
148 
149  compute_fMi();
150 }
151 
156 {
157  robotStop = true;
158 
159  #if defined(_WIN32)
160  WaitForSingleObject(hThread,INFINITE);
161  CloseHandle(hThread);
162  CloseHandle(mutex_fMi);
163  CloseHandle(mutex_artVel);
164  CloseHandle(mutex_artCoord);
165  CloseHandle(mutex_velocity);
166  CloseHandle(mutex_display);
167  #elif defined(VISP_HAVE_PTHREAD)
168  pthread_attr_destroy(&attr);
169  pthread_join(thread, NULL);
170  pthread_mutex_destroy(&mutex_fMi);
171  pthread_mutex_destroy(&mutex_artVel);
172  pthread_mutex_destroy(&mutex_artCoord);
173  pthread_mutex_destroy(&mutex_velocity);
174  pthread_mutex_destroy(&mutex_display);
175  #endif
176 
177  if (robotArms != NULL)
178  {
179  for(int i = 0; i < 6; i++)
180  free_Bound_scene (&(robotArms[i]));
181  }
182 
183  delete[] robotArms;
184  delete[] fMi;
185 }
186 
195 void
197 {
198  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
199  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
200  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
201  bool armDirExists = false;
202  for(size_t i=0; i < arm_dirs.size(); i++)
203  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
204  arm_dir = arm_dirs[i];
205  armDirExists = true;
206  break;
207  }
208  if (! armDirExists) {
209  try {
210  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
211  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
212  }
213  catch (...) {
214  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
215  }
216  }
217 
218  this->init(vpAfma6::TOOL_CCMOP);
219  toolCustom = false;
220 
221  size_fMi = 8;
222  fMi = new vpHomogeneousMatrix[8];
225 
226  zeroPos.resize(njoint);
227  zeroPos = 0;
228  reposPos.resize(njoint);
229  reposPos = 0;
230  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
231 
232  artCoord = zeroPos;
233  artVel = 0;
234 
235  q_prev_getdis.resize(njoint);
236  q_prev_getdis = 0;
237  first_time_getdis = true;
238 
239  positioningVelocity = defaultPositioningVelocity ;
240 
243 
244  // Software joint limits in radians
245  //_joint_min.resize(njoint);
246  _joint_min[0] = -0.6501;
247  _joint_min[1] = -0.6001;
248  _joint_min[2] = -0.5001;
249  _joint_min[3] = -2.7301;
250  _joint_min[4] = -0.1001;
251  _joint_min[5] = -1.5901;
252  //_joint_max.resize(njoint);
253  _joint_max[0] = 0.7001;
254  _joint_max[1] = 0.5201;
255  _joint_max[2] = 0.4601;
256  _joint_max[3] = 2.7301;
257  _joint_max[4] = 2.4801;
258  _joint_max[5] = 1.5901;
259 }
260 
264 void
266 {
267  robotArms = NULL;
268  robotArms = new Bound_scene[6];
269  initArms();
271  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
273  vpCameraParameters tmp;
274  getCameraParameters(tmp,640,480);
275  px_int = tmp.get_px();
276  py_int = tmp.get_py();
277  sceneInitialized = true;
278 }
279 
280 
296 void
299 {
300  this->projModel = proj_model;
301  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
302  if (arm_dir.size() > FILENAME_MAX)
303  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
304  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
305  if (full_length > FILENAME_MAX)
306  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
307 
308  // Use here default values of the robot constant parameters.
309  switch (tool) {
310  case vpAfma6::TOOL_CCMOP: {
311  _erc[0] = vpMath::rad(164.35); // rx
312  _erc[1] = vpMath::rad( 89.64); // ry
313  _erc[2] = vpMath::rad(-73.05); // rz
314  _etc[0] = 0.0117; // tx
315  _etc[1] = 0.0033; // ty
316  _etc[2] = 0.2272; // tz
317 
318  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
319 
320  if (robotArms != NULL)
321  {
322  while (get_displayBusy()) vpTime::wait(2);
323  free_Bound_scene (&(robotArms[5]));
324  char *name_arm = new char [full_length];
325  strcpy(name_arm, arm_dir.c_str());
326  strcat(name_arm,"/afma6_tool_ccmop.bnd");
327  set_scene(name_arm, robotArms+5, 1.0);
328  set_displayBusy(false);
329  delete [] name_arm;
330  }
331  break;
332  }
333  case vpAfma6::TOOL_GRIPPER: {
334  _erc[0] = vpMath::rad( 88.33); // rx
335  _erc[1] = vpMath::rad( 72.07); // ry
336  _erc[2] = vpMath::rad( 2.53); // rz
337  _etc[0] = 0.0783; // tx
338  _etc[1] = 0.1234; // ty
339  _etc[2] = 0.1638; // tz
340 
341  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
342 
343  if (robotArms != NULL)
344  {
345  while (get_displayBusy()) vpTime::wait(2);
346  free_Bound_scene (&(robotArms[5]));
347  char *name_arm = new char [full_length];
348  strcpy(name_arm, arm_dir.c_str());
349  strcat(name_arm,"/afma6_tool_gripper.bnd");
350  set_scene(name_arm, robotArms+5, 1.0);
351  set_displayBusy(false);
352  delete [] name_arm;
353  }
354  break;
355  }
356  case vpAfma6::TOOL_VACUUM: {
357  _erc[0] = vpMath::rad( 90.40); // rx
358  _erc[1] = vpMath::rad( 75.11); // ry
359  _erc[2] = vpMath::rad( 0.18); // rz
360  _etc[0] = 0.0038; // tx
361  _etc[1] = 0.1281; // ty
362  _etc[2] = 0.1658; // tz
363 
364  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
365 
366  if (robotArms != NULL)
367  {
368  while (get_displayBusy()) vpTime::wait(2);
369  free_Bound_scene (&(robotArms[5]));
370 
371  char *name_arm = new char [full_length];
372 
373  strcpy(name_arm, arm_dir.c_str());
374  strcat(name_arm,"/afma6_tool_vacuum.bnd");
375  set_scene(name_arm, robotArms+5, 1.0);
376  set_displayBusy(false);
377  delete [] name_arm;
378  }
379  break;
380  }
382  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
383  }
384  }
385 
386  vpRotationMatrix eRc(_erc);
387  this->_eMc.buildFrom(_etc, eRc);
388 
389  setToolType(tool);
390  return ;
391 }
392 
403 void
405  const unsigned int &image_width,
406  const unsigned int &image_height)
407 {
408  if (toolCustom)
409  {
410  vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
411  }
412  // Set default parameters
413  switch (getToolType()) {
414  case vpAfma6::TOOL_CCMOP: {
415  // Set default intrinsic camera parameters for 640x480 images
416  if (image_width == 640 && image_height == 480)
417  {
418  std::cout << "Get default camera parameters for camera \""
419  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
420  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
421  }
422  else {
423  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
424  }
425  break;
426  }
427  case vpAfma6::TOOL_GRIPPER: {
428  // Set default intrinsic camera parameters for 640x480 images
429  if (image_width == 640 && image_height == 480) {
430  std::cout << "Get default camera parameters for camera \""
431  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
432  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
433  }
434  else {
435  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
436  }
437  break;
438  }
440  case vpAfma6::TOOL_VACUUM: {
441  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
442  break;
443  }
444  default:
445  vpERROR_TRACE ("This error should not occur!");
446  break;
447  }
448  return;
449 }
450 
459 void
461  const vpImage<unsigned char> &I_)
462 {
463  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
464 }
465 
474 void
476  const vpImage<vpRGBa> &I_)
477 {
478  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
479 }
480 
481 
487 void
489 {
490  px_int = cam.get_px();
491  py_int = cam.get_py();
492  toolCustom = true;
493 }
494 
495 
499 void
501 {
502  double tcur_1 = tcur;// temporary variable used to store the last time since the last command
503 
504  while (!robotStop)
505  {
506  //Get current time
507  tprev = tcur_1;
509 
511  setVelocityCalled = false;
512 
514 
515  double ellapsedTime = (tcur - tprev) * 1e-3;
516  if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
517  ellapsedTime = getSamplingTime(); // in second
518  }
519 
520  vpColVector articularCoordinates = get_artCoord();
521  articularCoordinates = get_artCoord();
522  vpColVector articularVelocities = get_artVel();
523 
524  if (jointLimit)
525  {
526  double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
527  if (art <= _joint_min[jointLimitArt-1] || art >= _joint_max[jointLimitArt-1]) {
528  if (verbose_) {
529  std::cout << "Joint " << jointLimitArt-1
530  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt-1]) << " < "
531  << vpMath::deg(art) << " < " << vpMath::deg(_joint_max[jointLimitArt-1]) << std::endl;
532  }
533 
534  articularVelocities = 0.0;
535  }
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  {
551  if (jl < 0)
552  ellapsedTime = (_joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
553  else
554  ellapsedTime = (_joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
555 
556  for (unsigned int i = 0; i < 6; i++)
557  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
558 
559  jointLimit = true;
560  jointLimitArt = (unsigned int)fabs((double)jl);
561  }
562 
563  set_artCoord(articularCoordinates);
564  set_artVel(articularVelocities);
565 
566  compute_fMi();
567 
568  if (displayAllowed)
569  {
573  }
574 
576  {
577  while (get_displayBusy()) vpTime::wait(2);
579  set_displayBusy(false);
580  }
581 
582 
583  if (0/*displayType == MODEL_DH && displayAllowed*/)
584  {
585  vpHomogeneousMatrix fMit[8];
586  get_fMi(fMit);
587 
588  //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
589 
590  vpImagePoint iP, iP_1;
591  vpPoint pt;
592  pt.setWorldCoordinates (0,0,0);
593 
596  pt.track(getExternalCameraPosition ()*fMit[0]);
599  for (unsigned int k = 1; k < 7; k++)
600  {
601  pt.track(getExternalCameraPosition ()*fMit[k-1]);
603 
604  pt.track(getExternalCameraPosition ()*fMit[k]);
606 
608  }
610  }
611 
613 
614 
615  vpTime::wait( tcur, 1000*getSamplingTime() );
616  tcur_1 = tcur;
617  }else{
619  }
620  }
621 }
622 
630 void
632 {
633  //vpColVector q = get_artCoord();
634  vpColVector q(6);//; = get_artCoord();
635  q = get_artCoord();
636 
637  vpHomogeneousMatrix fMit[8];
638 
639  double q1 = q[0];
640  double q2 = q[1];
641  double q3 = q[2];
642  double q4 = q[3];
643  double q5 = q[4];
644  double q6 = q[5];
645 
646  double c4 = cos(q4);
647  double s4 = sin(q4);
648  double c5 = cos(q5);
649  double s5 = sin(q5);
650  double c6 = cos(q6);
651  double s6 = sin(q6);
652 
653  fMit[0][0][0] = 1;
654  fMit[0][1][0] = 0;
655  fMit[0][2][0] = 0;
656  fMit[0][0][1] = 0;
657  fMit[0][1][1] = 1;
658  fMit[0][2][1] = 0;
659  fMit[0][0][2] = 0;
660  fMit[0][1][2] = 0;
661  fMit[0][2][2] = 1;
662  fMit[0][0][3] = q1;
663  fMit[0][1][3] = 0;
664  fMit[0][2][3] = 0;
665 
666  fMit[1][0][0] = 1;
667  fMit[1][1][0] = 0;
668  fMit[1][2][0] = 0;
669  fMit[1][0][1] = 0;
670  fMit[1][1][1] = 1;
671  fMit[1][2][1] = 0;
672  fMit[1][0][2] = 0;
673  fMit[1][1][2] = 0;
674  fMit[1][2][2] = 1;
675  fMit[1][0][3] = q1;
676  fMit[1][1][3] = q2;
677  fMit[1][2][3] = 0;
678 
679  fMit[2][0][0] = 1;
680  fMit[2][1][0] = 0;
681  fMit[2][2][0] = 0;
682  fMit[2][0][1] = 0;
683  fMit[2][1][1] = 1;
684  fMit[2][2][1] = 0;
685  fMit[2][0][2] = 0;
686  fMit[2][1][2] = 0;
687  fMit[2][2][2] = 1;
688  fMit[2][0][3] = q1;
689  fMit[2][1][3] = q2;
690  fMit[2][2][3] = q3;
691 
692  fMit[3][0][0] = s4;
693  fMit[3][1][0] = -c4;
694  fMit[3][2][0] = 0;
695  fMit[3][0][1] = c4;
696  fMit[3][1][1] = s4;
697  fMit[3][2][1] = 0;
698  fMit[3][0][2] = 0;
699  fMit[3][1][2] = 0;
700  fMit[3][2][2] = 1;
701  fMit[3][0][3] = q1;
702  fMit[3][1][3] = q2;
703  fMit[3][2][3] = q3;
704 
705  fMit[4][0][0] = s4*s5;
706  fMit[4][1][0] = -c4*s5;
707  fMit[4][2][0] = c5;
708  fMit[4][0][1] = s4*c5;
709  fMit[4][1][1] = -c4*c5;
710  fMit[4][2][1] = -s5;
711  fMit[4][0][2] = c4;
712  fMit[4][1][2] = s4;
713  fMit[4][2][2] = 0;
714  fMit[4][0][3] = c4*this->_long_56+q1;
715  fMit[4][1][3] = s4*this->_long_56+q2;
716  fMit[4][2][3] = q3;
717 
718  fMit[5][0][0] = s4*s5*c6+c4*s6;
719  fMit[5][1][0] = -c4*s5*c6+s4*s6;
720  fMit[5][2][0] = c5*c6;
721  fMit[5][0][1] = -s4*s5*s6+c4*c6;
722  fMit[5][1][1] = c4*s5*s6+s4*c6;
723  fMit[5][2][1] = -c5*s6;
724  fMit[5][0][2] = -s4*c5;
725  fMit[5][1][2] = c4*c5;
726  fMit[5][2][2] = s5;
727  fMit[5][0][3] = c4*this->_long_56+q1;
728  fMit[5][1][3] = s4*this->_long_56+q2;
729  fMit[5][2][3] = q3;
730 
731  fMit[6][0][0] = fMit[5][0][0];
732  fMit[6][1][0] = fMit[5][1][0];
733  fMit[6][2][0] = fMit[5][2][0];
734  fMit[6][0][1] = fMit[5][0][1];
735  fMit[6][1][1] = fMit[5][1][1];
736  fMit[6][2][1] = fMit[5][2][1];
737  fMit[6][0][2] = fMit[5][0][2];
738  fMit[6][1][2] = fMit[5][1][2];
739  fMit[6][2][2] = fMit[5][2][2];
740  fMit[6][0][3] = fMit[5][0][3];
741  fMit[6][1][3] = fMit[5][1][3];
742  fMit[6][2][3] = fMit[5][2][3];
743 
744 // vpHomogeneousMatrix cMe;
745 // get_cMe(cMe);
746 // cMe = cMe.inverse();
747 // fMit[7] = fMit[6] * cMe;
748  vpAfma6::get_fMc(q,fMit[7]);
749 
750  #if defined(_WIN32)
751  WaitForSingleObject(mutex_fMi,INFINITE);
752  for (int i = 0; i < 8; i++)
753  fMi[i] = fMit[i];
754  ReleaseMutex(mutex_fMi);
755  #elif defined(VISP_HAVE_PTHREAD)
756  pthread_mutex_lock (&mutex_fMi);
757  for (int i = 0; i < 8; i++)
758  fMi[i] = fMit[i];
759  pthread_mutex_unlock (&mutex_fMi);
760  #endif
761 }
762 
763 
771 {
772  switch (newState) {
773  case vpRobot::STATE_STOP: {
774  // Start primitive STOP only if the current state is Velocity
776  stopMotion();
777  }
778  break;
779  }
782  std::cout << "Change the control mode from velocity to position control.\n";
783  stopMotion();
784  }
785  else {
786  //std::cout << "Change the control mode from stop to position control.\n";
787  }
788  break;
789  }
792  std::cout << "Change the control mode from stop to velocity control.\n";
793  }
794  break;
795  }
797  default:
798  break ;
799  }
800 
801  return vpRobot::setRobotState (newState);
802 }
803 
873 void
875 {
877  vpERROR_TRACE ("Cannot send a velocity to the robot "
878  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
880  "Cannot send a velocity to the robot "
881  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
882  }
883 
884  vpColVector vel_sat(6);
885 
886  double scale_trans_sat = 1;
887  double scale_rot_sat = 1;
888  double scale_sat = 1;
889  double vel_trans_max = getMaxTranslationVelocity();
890  double vel_rot_max = getMaxRotationVelocity();
891 
892  double vel_abs; // Absolute value
893 
894  // Velocity saturation
895  switch(frame)
896  {
897  // saturation in cartesian space
898  case vpRobot::CAMERA_FRAME :
900  {
901  if (vel.getRows() != 6)
902  {
903  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
904  throw;
905  }
906 
907  for (unsigned int i = 0 ; i < 3; ++ i)
908  {
909  vel_abs = fabs (vel[i]);
910  if (vel_abs > vel_trans_max && !jointLimit)
911  {
912  vel_trans_max = vel_abs;
913  vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
914  "(axis nr. %d).", vel[i], i+1);
915  }
916 
917  vel_abs = fabs (vel[i+3]);
918  if (vel_abs > vel_rot_max && !jointLimit) {
919  vel_rot_max = vel_abs;
920  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
921  "(axis nr. %d).", vel[i+3], i+4);
922  }
923  }
924 
925  if (vel_trans_max > getMaxTranslationVelocity())
926  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
927 
928  if (vel_rot_max > getMaxRotationVelocity())
929  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
930 
931  if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
932  {
933  if (scale_trans_sat < scale_rot_sat)
934  scale_sat = scale_trans_sat;
935  else
936  scale_sat = scale_rot_sat;
937  }
938  break;
939  }
940 
941  // saturation in joint space
943  {
944  if (vel.getRows() != 6)
945  {
946  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
947  throw;
948  }
949  for (unsigned int i = 0 ; i < 6; ++ i)
950  {
951  vel_abs = fabs (vel[i]);
952  if (vel_abs > vel_rot_max && !jointLimit)
953  {
954  vel_rot_max = vel_abs;
955  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
956  "(axis nr. %d).", vel[i], i+1);
957  }
958  }
959  if (vel_rot_max > getMaxRotationVelocity())
960  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
961  if ( scale_rot_sat < 1 )
962  scale_sat = scale_rot_sat;
963  break;
964  }
965  case vpRobot::MIXT_FRAME :
966  {
967  break;
968  }
969  }
970 
971  set_velocity (vel * scale_sat);
972  setRobotFrame (frame);
973  setVelocityCalled = true;
974 }
975 
976 
980 void
982 {
984 
985  double scale_rot_sat = 1;
986  double scale_sat = 1;
987  double vel_rot_max = getMaxRotationVelocity();
988  double vel_abs;
989 
990  vpColVector articularCoordinates = get_artCoord();
991  vpColVector velocityframe = get_velocity();
992  vpColVector articularVelocity;
993 
994  switch(frame)
995  {
996  case vpRobot::CAMERA_FRAME :
997  {
998  vpMatrix eJe_;
1000  vpAfma6::get_eJe(articularCoordinates,eJe_);
1001  eJe_ = eJe_.pseudoInverse();
1003  singularityTest(articularCoordinates,eJe_);
1004  articularVelocity = eJe_*eVc*velocityframe;
1005  set_artVel (articularVelocity);
1006  break;
1007  }
1009  {
1010  vpMatrix fJe_;
1011  vpAfma6::get_fJe(articularCoordinates,fJe_);
1012  fJe_ = fJe_.pseudoInverse();
1014  singularityTest(articularCoordinates,fJe_);
1015  articularVelocity = fJe_*velocityframe;
1016  set_artVel (articularVelocity);
1017  break;
1018  }
1020  {
1021  articularVelocity = velocityframe;
1022  set_artVel (articularVelocity);
1023  break;
1024  }
1025  case vpRobot::MIXT_FRAME :
1026  {
1027  break;
1028  }
1029  }
1030 
1031 
1032 
1033  switch(frame)
1034  {
1035  case vpRobot::CAMERA_FRAME :
1037  {
1038  for (unsigned int i = 0 ; i < 6; ++ i)
1039  {
1040  vel_abs = fabs (articularVelocity[i]);
1041  if (vel_abs > vel_rot_max && !jointLimit)
1042  {
1043  vel_rot_max = vel_abs;
1044  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
1045  "(axis nr. %d).", articularVelocity[i], i+1);
1046  }
1047  }
1048  if (vel_rot_max > getMaxRotationVelocity())
1049  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1050  if ( scale_rot_sat < 1 )
1051  scale_sat = scale_rot_sat;
1052 
1053  set_artVel(articularVelocity * scale_sat);
1054  break;
1055  }
1057  case vpRobot::MIXT_FRAME :
1058  {
1059  break;
1060  }
1061  }
1062 }
1063 
1064 
1111 void
1113 {
1114  vel.resize(6);
1115 
1116  vpColVector articularCoordinates = get_artCoord();
1117  vpColVector articularVelocity = get_artVel();
1118 
1119  switch(frame)
1120  {
1121  case vpRobot::CAMERA_FRAME :
1122  {
1123  vpMatrix eJe_;
1125  vpAfma6::get_eJe(articularCoordinates,eJe_);
1126  vel = cVe*eJe_*articularVelocity;
1127  break ;
1128  }
1129  case vpRobot::ARTICULAR_FRAME :
1130  {
1131  vel = articularVelocity;
1132  break ;
1133  }
1134  case vpRobot::REFERENCE_FRAME :
1135  {
1136  vpMatrix fJe_;
1137  vpAfma6::get_fJe(articularCoordinates,fJe_);
1138  vel = fJe_*articularVelocity;
1139  break ;
1140  }
1141  case vpRobot::MIXT_FRAME :
1142  {
1143  break ;
1144  }
1145  default:
1146  {
1147  vpERROR_TRACE ("Error in spec of vpRobot. "
1148  "Case not taken in account.");
1149  return;
1150  }
1151  }
1152 }
1153 
1170 void
1172 {
1173  timestamp = vpTime::measureTimeSecond();
1174  getVelocity(frame, vel);
1175 }
1176 
1221 {
1222  vpColVector vel(6);
1223  getVelocity (frame, vel);
1224 
1225  return vel;
1226 }
1227 
1242 {
1243  timestamp = vpTime::measureTimeSecond();
1244  vpColVector vel(6);
1245  getVelocity (frame, vel);
1246 
1247  return vel;
1248 }
1249 
1250 void
1252 {
1253  double vel_rot_max = getMaxRotationVelocity();
1254  double velmax = fabs(q[0]);
1255  for (unsigned int i = 1; i < 6; i++)
1256  {
1257  if (velmax < fabs(q[i]))
1258  velmax = fabs(q[i]);
1259  }
1260 
1261  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
1262  q = q * alpha;
1263 }
1264 
1339 void
1341 {
1343  {
1344  vpERROR_TRACE ("Robot was not in position-based control\n"
1345  "Modification of the robot state");
1346  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1347  }
1348 
1349  vpColVector articularCoordinates = get_artCoord();
1350 
1351  vpColVector error(6);
1352  double errsqr = 0;
1353  switch(frame)
1354  {
1355  case vpRobot::CAMERA_FRAME :
1356  {
1357  int nbSol;
1358  vpColVector qdes(6);
1359 
1360  vpTranslationVector txyz;
1361  vpRxyzVector rxyz;
1362  for (unsigned int i=0; i < 3; i++)
1363  {
1364  txyz[i] = q[i];
1365  rxyz[i] = q[i+3];
1366  }
1367 
1368  vpRotationMatrix cRc2(rxyz);
1369  vpHomogeneousMatrix cMc2(txyz, cRc2);
1370 
1371  vpHomogeneousMatrix fMc_;
1372  vpAfma6::get_fMc(articularCoordinates, fMc_);
1373 
1374  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1375 
1376  do
1377  {
1378  articularCoordinates = get_artCoord();
1379  qdes = articularCoordinates;
1380  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1381  setVelocityCalled = true;
1382  if (nbSol > 0)
1383  {
1384  error = qdes - articularCoordinates;
1385  errsqr = error.sumSquare();
1386  //findHighestPositioningSpeed(error);
1387  set_artVel(error);
1388  if (errsqr < 1e-4)
1389  {
1390  set_artCoord (qdes);
1391  error = 0;
1392  set_artVel(error);
1393  set_velocity(error);
1394  break;
1395  }
1396  }
1397  else
1398  {
1399  vpERROR_TRACE ("Positionning error.");
1401  "Position out of range.");
1402  }
1403  }while (errsqr > 1e-8 && nbSol > 0);
1404 
1405  break ;
1406  }
1407 
1409  {
1410  do
1411  {
1412  articularCoordinates = get_artCoord();
1413  error = q - articularCoordinates;
1414  errsqr = error.sumSquare();
1415  //findHighestPositioningSpeed(error);
1416  set_artVel(error);
1417  setVelocityCalled = true;
1418  if (errsqr < 1e-4)
1419  {
1420  set_artCoord (q);
1421  error = 0;
1422  set_artVel(error);
1423  set_velocity(error);
1424  break;
1425  }
1426  }while (errsqr > 1e-8);
1427  break ;
1428  }
1429 
1431  {
1432  int nbSol;
1433  vpColVector qdes(6);
1434 
1435  vpTranslationVector txyz;
1436  vpRxyzVector rxyz;
1437  for (unsigned int i=0; i < 3; i++)
1438  {
1439  txyz[i] = q[i];
1440  rxyz[i] = q[i+3];
1441  }
1442 
1443  vpRotationMatrix fRc(rxyz);
1444  vpHomogeneousMatrix fMc_(txyz, fRc);
1445 
1446  do
1447  {
1448  articularCoordinates = get_artCoord();
1449  qdes = articularCoordinates;
1450  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1451  setVelocityCalled = true;
1452  if (nbSol > 0)
1453  {
1454  error = qdes - articularCoordinates;
1455  errsqr = error.sumSquare();
1456  //findHighestPositioningSpeed(error);
1457  set_artVel(error);
1458  if (errsqr < 1e-4)
1459  {
1460  set_artCoord (qdes);
1461  error = 0;
1462  set_artVel(error);
1463  set_velocity(error);
1464  break;
1465  }
1466  }
1467  else
1468  vpERROR_TRACE ("Positionning error. Position unreachable");
1469  }while (errsqr > 1e-8 && nbSol > 0);
1470  break ;
1471  }
1472  case vpRobot::MIXT_FRAME:
1473  {
1474  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1476  "Positionning error: "
1477  "Mixt frame not implemented.");
1478  break ;
1479  }
1480  }
1481 }
1482 
1547  const double pos1,
1548  const double pos2,
1549  const double pos3,
1550  const double pos4,
1551  const double pos5,
1552  const double pos6)
1553 {
1554  try{
1555  vpColVector position(6) ;
1556  position[0] = pos1 ;
1557  position[1] = pos2 ;
1558  position[2] = pos3 ;
1559  position[3] = pos4 ;
1560  position[4] = pos5 ;
1561  position[5] = pos6 ;
1562 
1563  setPosition(frame, position) ;
1564  }
1565  catch(...)
1566  {
1567  vpERROR_TRACE("Error caught");
1568  throw ;
1569  }
1570 }
1571 
1607 void vpSimulatorAfma6::setPosition(const char *filename)
1608 {
1609  vpColVector q;
1610  bool ret;
1611 
1612  ret = this->readPosFile(filename, q);
1613 
1614  if (ret == false) {
1615  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1617  "Bad position in filename.");
1618  }
1621 }
1622 
1682 void
1684 {
1685  q.resize(6);
1686 
1687  switch(frame)
1688  {
1689  case vpRobot::CAMERA_FRAME :
1690  {
1691  q = 0;
1692  break ;
1693  }
1694 
1696  {
1697  q = get_artCoord();
1698  break ;
1699  }
1700 
1702  {
1703  vpHomogeneousMatrix fMc_;
1704  vpAfma6::get_fMc (get_artCoord(), fMc_);
1705 
1706  vpRotationMatrix fRc;
1707  fMc_.extract(fRc);
1708  vpRxyzVector rxyz(fRc);
1709 
1710  vpTranslationVector txyz;
1711  fMc_.extract(txyz);
1712 
1713  for (unsigned int i=0; i <3; i++)
1714  {
1715  q[i] = txyz[i];
1716  q[i+3] = rxyz[i];
1717  }
1718  break ;
1719  }
1720 
1721  case vpRobot::MIXT_FRAME:
1722  {
1723  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1725  "Positionning error: "
1726  "Mixt frame not implemented.");
1727  break ;
1728  }
1729  }
1730 }
1731 
1758 void
1760 {
1761  timestamp = vpTime::measureTimeSecond();
1762  getPosition(frame, q);
1763 }
1764 
1765 
1776 void
1778 {
1779  vpColVector posRxyz;
1780  //recupere position en Rxyz
1781  this->getPosition(frame,posRxyz);
1782 
1783  //recupere le vecteur thetaU correspondant
1784  vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
1785 
1786  //remplit le vpPoseVector avec translation et rotation ThetaU
1787  for(unsigned int j=0;j<3;j++)
1788  {
1789  position[j]=posRxyz[j];
1790  position[j+3]=RtuVect[j];
1791  }
1792 }
1793 
1804 void
1806  vpPoseVector &position, double &timestamp)
1807 {
1808  timestamp = vpTime::measureTimeSecond();
1809  getPosition(frame, position);
1810 }
1811 
1818 void
1820 {
1821  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
1822  {
1823  vpTRACE("Joint limit vector has not a size of 6 !");
1824  return;
1825  }
1826 
1827  _joint_min[0] = limitMin[0];
1828  _joint_min[1] = limitMin[1];
1829  _joint_min[2] = limitMin[2];
1830  _joint_min[3] = limitMin[3];
1831  _joint_min[4] = limitMin[4];
1832  _joint_min[5] = limitMin[5];
1833 
1834  _joint_max[0] = limitMax[0];
1835  _joint_max[1] = limitMax[1];
1836  _joint_max[2] = limitMax[2];
1837  _joint_max[3] = limitMax[3];
1838  _joint_max[4] = limitMax[4];
1839  _joint_max[5] = limitMax[5];
1840 
1841 }
1842 
1848 bool
1850 {
1851  double q5 = q[4];
1852 
1853  bool cond = fabs(q5-M_PI/2) < 1e-1;
1854 
1855  if(cond)
1856  {
1857  J[0][3] = 0;
1858  J[0][4] = 0;
1859  J[1][3] = 0;
1860  J[1][4] = 0;
1861  J[3][3] = 0;
1862  J[3][4] = 0;
1863  J[5][3] = 0;
1864  J[5][4] = 0;
1865  return true;
1866  }
1867 
1868  return false;
1869 }
1870 
1874 int
1876 {
1877  int artNumb = 0;
1878  double diff = 0;
1879  double difft = 0;
1880 
1881  vpColVector articularCoordinates = get_artCoord();
1882 
1883  for (unsigned int i = 0; i < 6; i++)
1884  {
1885  if (articularCoordinates[i] <= _joint_min[i])
1886  {
1887  difft = _joint_min[i] - articularCoordinates[i];
1888  if (difft > diff)
1889  {
1890  diff = difft;
1891  artNumb = -(int)i-1;
1892  }
1893  }
1894  }
1895 
1896  for (unsigned int i = 0; i < 6; i++)
1897  {
1898  if (articularCoordinates[i] >= _joint_max[i])
1899  {
1900  difft = articularCoordinates[i] - _joint_max[i];
1901  if (difft > diff)
1902  {
1903  diff = difft;
1904  artNumb = (int)(i+1);
1905  }
1906  }
1907  }
1908 
1909  if (artNumb != 0)
1910  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
1911 
1912  return artNumb;
1913 }
1914 
1926 void
1927 vpSimulatorAfma6::getCameraDisplacement(vpColVector &displacement)
1928 {
1929  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1930 }
1931 
1941 void
1942 vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
1943 {
1945 }
1946 
1965 void
1967  vpColVector &displacement)
1968 {
1969  displacement.resize (6);
1970  displacement = 0;
1971  vpColVector q_cur(6);
1972 
1973  q_cur = get_artCoord();
1974 
1975  if ( ! first_time_getdis )
1976  {
1977  switch (frame)
1978  {
1979  case vpRobot::CAMERA_FRAME:
1980  {
1981  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1982  return;
1983  break ;
1984  }
1985 
1987  {
1988  displacement = q_cur - q_prev_getdis;
1989  break ;
1990  }
1991 
1993  {
1994  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1995  return;
1996  break ;
1997  }
1998 
1999  case vpRobot::MIXT_FRAME:
2000  {
2001  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
2002  return;
2003  break ;
2004  }
2005  }
2006  }
2007  else
2008  {
2009  first_time_getdis = false;
2010  }
2011 
2012  // Memorize the joint position for the next call
2013  q_prev_getdis = q_cur;
2014 }
2015 
2062 bool
2064 {
2065  FILE * fd ;
2066  fd = fopen(filename, "r") ;
2067  if (fd == NULL)
2068  return false;
2069 
2070  char line[FILENAME_MAX];
2071  char dummy[FILENAME_MAX];
2072  char head[] = "R:";
2073  bool sortie = false;
2074 
2075  do {
2076  // Saut des lignes commencant par #
2077  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2078  if ( strncmp (line, "#", 1) != 0) {
2079  // La ligne n'est pas un commentaire
2080  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2081  sortie = true; // Position robot trouvee.
2082  }
2083  // else
2084  // return (false); // fin fichier sans position robot.
2085  }
2086  }
2087  else {
2088  fclose(fd) ;
2089  return (false); /* fin fichier */
2090  }
2091 
2092  }
2093  while ( sortie != true );
2094 
2095  // Lecture des positions
2096  q.resize(njoint);
2097  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2098  dummy,
2099  &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2100 
2101  if (ret != 7) {
2102  fclose(fd) ;
2103  return false;
2104  }
2105 
2106  // converts rotations from degrees into radians
2107  //q.deg2rad();
2108 
2109  q[3] = vpMath::rad(q[3]);
2110  q[4] = vpMath::rad(q[4]);
2111  q[5] = vpMath::rad(q[5]);
2112 
2113  fclose(fd) ;
2114  return (true);
2115 }
2116 
2139 bool
2140 vpSimulatorAfma6::savePosFile(const char *filename, const vpColVector &q)
2141 {
2142 
2143  FILE * fd ;
2144  fd = fopen(filename, "w") ;
2145  if (fd == NULL)
2146  return false;
2147 
2148  fprintf(fd, "\
2149 #AFMA6 - Position - Version 2.01\n\
2150 #\n\
2151 # R: X Y Z A B C\n\
2152 # Joint position: X, Y, Z: translations in meters\n\
2153 # A, B, C: rotations in degrees\n\
2154 #\n\
2155 #\n\n");
2156 
2157  // Save positions in mm and deg
2158  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
2159  q[0],
2160  q[1],
2161  q[2],
2162  vpMath::deg(q[3]),
2163  vpMath::deg(q[4]),
2164  vpMath::deg(q[5]));
2165 
2166  fclose(fd) ;
2167  return (true);
2168 }
2169 
2177 void
2178 vpSimulatorAfma6::move(const char *filename)
2179 {
2180  vpColVector q;
2181 
2182  try
2183  {
2184  this->readPosFile(filename, q);
2187  }
2188  catch(...)
2189  {
2190  throw;
2191  }
2192 }
2193 
2203 void
2205 {
2206  vpAfma6::get_cMe(cMe) ;
2207 }
2208 
2216 void
2218 {
2219  vpHomogeneousMatrix cMe ;
2220  vpAfma6::get_cMe(cMe) ;
2221 
2222  cVe.buildFrom(cMe) ;
2223 }
2224 
2234 void
2236 {
2237  try
2238  {
2239  vpAfma6::get_eJe(get_artCoord(), eJe_) ;
2240  }
2241  catch(...)
2242  {
2243  vpERROR_TRACE("catch exception ") ;
2244  throw ;
2245  }
2246 }
2247 
2258 void
2260 {
2261  try
2262  {
2263  vpColVector articularCoordinates = get_artCoord();
2264  vpAfma6::get_fJe(articularCoordinates, fJe_) ;
2265  }
2266  catch(...)
2267  {
2268  vpERROR_TRACE("Error caught");
2269  throw ;
2270  }
2271 }
2272 
2276 void
2278 {
2280  return;
2281 
2282  vpColVector stop(6);
2283  stop = 0;
2284  set_artVel(stop);
2285  set_velocity(stop);
2287 }
2288 
2289 
2290 
2291 /**********************************************************************************/
2292 /**********************************************************************************/
2293 /**********************************************************************************/
2294 /**********************************************************************************/
2295 
2304 void
2306 {
2307  // set scene_dir from #define VISP_SCENE_DIR if it exists
2308  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2309  std::string scene_dir_;
2310  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2311  bool sceneDirExists = false;
2312  for(size_t i=0; i < scene_dirs.size(); i++)
2313  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2314  scene_dir_ = scene_dirs[i];
2315  sceneDirExists = true;
2316  break;
2317  }
2318  if (! sceneDirExists) {
2319  try {
2320  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2321  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2322  }
2323  catch (...) {
2324  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2325  }
2326  }
2327 
2328 
2329  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2330  if (scene_dir_.size() > FILENAME_MAX)
2331  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2332  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2333  if (full_length > FILENAME_MAX)
2334  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2335 
2336  char *name_cam = new char [full_length];
2337 
2338  strcpy(name_cam, scene_dir_.c_str());
2339  strcat(name_cam,"/camera.bnd");
2340  set_scene(name_cam,&camera,cameraFactor);
2341 
2342  if (arm_dir.size() > FILENAME_MAX)
2343  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2344  full_length = (unsigned int)arm_dir.size() + name_length;
2345  if (full_length > FILENAME_MAX)
2346  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2347 
2348  char *name_arm = new char [full_length];
2349  strcpy(name_arm, arm_dir.c_str());
2350  strcat(name_arm,"/afma6_gate.bnd");
2351  std::cout <<"name arm: " << name_arm << std::endl;
2352  set_scene(name_arm, robotArms, 1.0);
2353  strcpy(name_arm, arm_dir.c_str());
2354  strcat(name_arm,"/afma6_arm1.bnd");
2355  set_scene(name_arm, robotArms+1, 1.0);
2356  strcpy(name_arm, arm_dir.c_str());
2357  strcat(name_arm,"/afma6_arm2.bnd");
2358  set_scene(name_arm, robotArms+2, 1.0);
2359  strcpy(name_arm, arm_dir.c_str());
2360  strcat(name_arm,"/afma6_arm3.bnd");
2361  set_scene(name_arm, robotArms+3, 1.0);
2362  strcpy(name_arm, arm_dir.c_str());
2363  strcat(name_arm,"/afma6_arm4.bnd");
2364  set_scene(name_arm, robotArms+4, 1.0);
2365 
2367  tool = getToolType();
2368  strcpy(name_arm, arm_dir.c_str());
2369  switch (tool) {
2370  case vpAfma6::TOOL_CCMOP: {
2371  strcat(name_arm,"/afma6_tool_ccmop.bnd");
2372  break;
2373  }
2374  case vpAfma6::TOOL_GRIPPER: {
2375  strcat(name_arm,"/afma6_tool_gripper.bnd");
2376  break;
2377  }
2378  case vpAfma6::TOOL_VACUUM: {
2379  strcat(name_arm,"/afma6_tool_vacuum.bnd");
2380  break;
2381  }
2383  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2384  break;
2385  }
2386  }
2387  set_scene(name_arm, robotArms+5, 1.0);
2388 
2389  add_rfstack(IS_BACK);
2390 
2391  add_vwstack ("start","depth", 0.0, 100.0);
2392  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
2393  add_vwstack ("start","type", PERSPECTIVE);
2394 //
2395 // sceneInitialized = true;
2396 // displayObject = true;
2397  displayCamera = true;
2398 
2399  delete [] name_cam;
2400  delete [] name_arm;
2401 }
2402 
2403 
2404 void
2406 {
2407  bool changed = false;
2408  vpHomogeneousMatrix displacement = navigation(I_,changed);
2409 
2410  //if (displacement[2][3] != 0)
2411  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2412  camMf2 = camMf2*displacement;
2413 
2414  f2Mf = camMf2.inverse()*camMf;
2415 
2416  camMf = camMf2* displacement * f2Mf;
2417 
2418  double u;
2419  double v;
2420  //if(px_ext != 1 && py_ext != 1)
2421  // we assume px_ext and py_ext > 0
2422  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
2423  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
2424  {
2425  u = (double)I_.getWidth()/(2*px_ext);
2426  v = (double)I_.getHeight()/(2*py_ext);
2427  }
2428  else
2429  {
2430  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2431  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2432  }
2433 
2434  float w44o[4][4],w44cext[4][4],x,y,z;
2435 
2436  vp2jlc_matrix(camMf.inverse(),w44cext);
2437 
2438  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2439  x = w44cext[2][0] + w44cext[3][0];
2440  y = w44cext[2][1] + w44cext[3][1];
2441  z = w44cext[2][2] + w44cext[3][2];
2442  add_vwstack ("start","vrp", x,y,z);
2443  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2444  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2445  add_vwstack ("start","window", -u, u, -v, v);
2446 
2447  vpHomogeneousMatrix fMit[8];
2448  get_fMi(fMit);
2449 
2450  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
2451  display_scene(w44o,robotArms[0],I_, curColor);
2452 
2453  vp2jlc_matrix(fMit[0],w44o);
2454  display_scene(w44o,robotArms[1],I_, curColor);
2455 
2456  vp2jlc_matrix(fMit[2],w44o);
2457  display_scene(w44o,robotArms[2],I_, curColor);
2458 
2459  vp2jlc_matrix(fMit[3],w44o);
2460  display_scene(w44o,robotArms[3],I_, curColor);
2461 
2462  vp2jlc_matrix(fMit[4],w44o);
2463  display_scene(w44o,robotArms[4],I_, curColor);
2464 
2465  vp2jlc_matrix(fMit[5],w44o);
2466  display_scene(w44o,robotArms[5],I_, curColor);
2467 
2468  if (displayCamera)
2469  {
2470  vpHomogeneousMatrix cMe;
2471  get_cMe(cMe);
2472  cMe = cMe.inverse();
2473  cMe = fMit[6] * cMe;
2474  vp2jlc_matrix(cMe,w44o);
2475  display_scene(w44o,camera, I_, camColor);
2476  }
2477 
2478  if (displayObject)
2479  {
2480  vp2jlc_matrix(fMo,w44o);
2481  display_scene(w44o,scene,I_, curColor);
2482  }
2483 }
2484 
2501 bool
2503 {
2504  vpColVector stop(6);
2505  bool status = true;
2506  stop = 0;
2507  set_artVel(stop);
2508  set_velocity(stop);
2509  vpHomogeneousMatrix fMc_;
2510  fMc_ = fMo * cMo_.inverse();
2511 
2512  vpColVector articularCoordinates = get_artCoord();
2513  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2514 
2515  if (nbSol == 0) {
2516  status = false;
2517  vpERROR_TRACE ("Positionning error. Position unreachable");
2518  }
2519 
2520  if (verbose_)
2521  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2522 
2523  set_artCoord(articularCoordinates);
2524 
2525  compute_fMi();
2526 
2527  return status;
2528 }
2529 
2542 void
2544 {
2545  vpColVector stop(6);
2546  stop = 0;
2547  set_artVel(stop);
2548  set_velocity(stop);
2549  vpHomogeneousMatrix fMit[8];
2550  get_fMi(fMit);
2551  fMo = fMit[7] * cMo_;
2552 }
2553 
2564 bool
2566 {
2567  // get rid of max velocity
2568  double vMax = getMaxTranslationVelocity();
2569  double wMax = getMaxRotationVelocity();
2570  setMaxTranslationVelocity(10.*vMax);
2571  setMaxRotationVelocity(10.*wMax);
2572 
2573  vpColVector v(3),w(3),vel(6);
2574  vpHomogeneousMatrix cdMc;
2576  vpColVector err(6);err=1.;
2577  const double lambda = 5.;
2578  double t;
2579 
2581 
2582  unsigned int i,iter=0;
2583  while((iter++<300) & (err.euclideanNorm()>errMax))
2584  {
2585  t = vpTime::measureTimeMs();
2586 
2587  // update image
2588  if(Iint != NULL)
2589  {
2590  vpDisplay::display(*Iint);
2591  getInternalView(*Iint);
2592  vpDisplay::flush(*Iint);
2593  }
2594 
2595  // update pose error
2596  cdMc = cdMo_*get_cMo().inverse();
2597  cdMc.extract(cdRc);
2598  cdMc.extract(cdTc);
2599  cdTUc.buildFrom(cdRc);
2600 
2601  // compute v,w and velocity
2602  v = -lambda*cdRc.t()*cdTc;
2603  w = -lambda*cdTUc;
2604  for(i=0;i<3;++i)
2605  {
2606  vel[i] = v[i];
2607  vel[i+3] = w[i];
2608  err[i] = cdTc[i];
2609  err[i+3] = cdTUc[i];
2610  }
2611 
2612  // update feat
2614 
2615  // wait for it
2616  vpTime::wait(t,10);
2617  }
2618  vel=0.;
2619  set_velocity(vel);
2620  set_artVel(vel);
2622  setMaxRotationVelocity(wMax);
2623 
2624  //std::cout << "setPosition: final error " << err.t() << std::endl;
2625  return(err.euclideanNorm()<= errMax);
2626 }
2627 
2628 #endif
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Definition: vpDisplay.cpp:513
Modelisation of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:69
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
vpRxyzVector _erc
Definition: vpAfma6.h:190
vpTranslationVector _etc
Definition: vpAfma6.h:189
static const unsigned int njoint
Number of joint.
Definition: vpAfma6.h:176
Error that can be emited by the vpRobot class and its derivates.
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:315
unsigned int getWidth() const
Definition: vpImage.h:161
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:242
void get_eJe(vpMatrix &eJe)
int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest=true, const bool &verbose=false) const
Definition: vpAfma6.cpp:564
double getSamplingTime() const
vpAfma6ToolType getToolType() const
Get the current tool type.
Definition: vpAfma6.h:150
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
#define vpTRACE
Definition: vpDebug.h:418
void get_cVe(vpVelocityTwistMatrix &cVe)
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:745
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)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
bool constantSamplingTimeMode
Flag used to force the sampling time in the thread computing the robot's displacement to a constant v...
static std::string getenv(const char *env)
Definition: vpIoTools.cpp:204
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:254
static const vpColor none
Definition: vpColor.h:179
Initialize the position controller.
Definition: vpRobot.h:71
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:899
error that can be emited by ViSP classes.
Definition: vpException.h:76
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:108
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void track(const vpHomogeneousMatrix &cMo)
static const double defaultPositioningVelocity
vpRotationMatrix t() const
transpose
vpControlFrameType
Definition: vpRobot.h:78
double get_py() const
void getExternalImage(vpImage< vpRGBa > &I)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpAfma6.h:198
static double measureTimeMs()
Definition: vpTime.cpp:86
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.h:138
static int wait(double t0, double t)
Definition: vpTime.cpp:149
double sumSquare() const
Definition: vpMatrix.cpp:868
static const vpColor green
Definition: vpColor.h:170
vpHomogeneousMatrix fMo
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:279
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static bool readPosFile(const char *filename, vpColVector &q)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
Class that defines what is a point.
Definition: vpPoint.h:65
void setToolType(vpAfma6::vpAfma6ToolType tool)
Set the current tool type.
Definition: vpAfma6.h:174
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:95
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
The vpRotationMatrix considers the particular case of a rotation matrix.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
bool singularityTest(const vpColVector q, vpMatrix &J)
Initialize the velocity controller.
Definition: vpRobot.h:70
vpRobotStateType
Definition: vpRobot.h:66
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
Initialize the acceleration controller.
Definition: vpRobot.h:72
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix f2Mf
void set_displayBusy(const bool &status)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:861
vpRowVector t() const
Transpose of a vector.
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)
void extract(vpRotationMatrix &R) const
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1457
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)
Definition: vpDisplay.cpp:375
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:148
void setCameraParameters(const vpCameraParameters &cam)
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void set_velocity(const vpColVector &vel)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Construction from translation vector and rotation matrix.
double get_px() const
static double rad(double deg)
Definition: vpMath.h:100
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:162
This class aims to be a basis used to create all the simulators of robots.
void setMaxRotationVelocity(const double maxVr)
Definition: vpRobot.cpp:266
bool setVelocityCalled
Flag used to specify to the thread managing the robot displacements that the setVelocity() method has...
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
double getPositioningVelocity(void)
double _long_56
Definition: vpAfma6.h:185
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
Definition: vpMath.h:93
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double euclideanNorm() const
Definition: vpMatrix.cpp:3168
void get_fJe(vpMatrix &fJe)
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
vpHomogeneousMatrix _eMc
Definition: vpAfma6.h:192
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:140
unsigned int getHeight() const
Definition: vpImage.h:152
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:212
void get_cMe(vpHomogeneousMatrix &cMe)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1932
static double measureTimeSecond()
Definition: vpTime.cpp:225
double _joint_max[6]
Definition: vpAfma6.h:186
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
void set_artCoord(const vpColVector &coord)
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void set_artVel(const vpColVector &vel)
void get_fMi(vpHomogeneousMatrix *fMit)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:90
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:968
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
static double minTimeForUsleepCall
Definition: vpTime.h:82
vpHomogeneousMatrix camMf2
static bool savePosFile(const char *filename, const vpColVector &q)
double _joint_min[6]
Definition: vpAfma6.h:187
void findHighestPositioningSpeed(vpColVector &q)
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98