ViSP  2.9.0
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  if (vpIoTools::checkDirectory(VISP_ROBOT_ARMS_DIR) == true) // directory exists
200  arm_dir = VISP_ROBOT_ARMS_DIR;
201  else {
202  try {
203  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
204  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
205  }
206  catch (...) {
207  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
208  }
209  }
210 
211  this->init(vpAfma6::TOOL_CCMOP);
212  toolCustom = false;
213 
214  size_fMi = 8;
215  fMi = new vpHomogeneousMatrix[8];
218 
219  zeroPos.resize(njoint);
220  zeroPos = 0;
221  reposPos.resize(njoint);
222  reposPos = 0;
223  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
224 
225  artCoord = zeroPos;
226  artVel = 0;
227 
228  q_prev_getdis.resize(njoint);
229  q_prev_getdis = 0;
230  first_time_getdis = true;
231 
232  positioningVelocity = defaultPositioningVelocity ;
233 
236 
237  // Software joint limits in radians
238  //_joint_min.resize(njoint);
239  _joint_min[0] = -0.6501;
240  _joint_min[1] = -0.6001;
241  _joint_min[2] = -0.5001;
242  _joint_min[3] = -2.7301;
243  _joint_min[4] = -0.1001;
244  _joint_min[5] = -1.5901;
245  //_joint_max.resize(njoint);
246  _joint_max[0] = 0.7001;
247  _joint_max[1] = 0.5201;
248  _joint_max[2] = 0.4601;
249  _joint_max[3] = 2.7301;
250  _joint_max[4] = 2.4801;
251  _joint_max[5] = 1.5901;
252 }
253 
257 void
259 {
260  robotArms = NULL;
261  robotArms = new Bound_scene[6];
262  initArms();
264  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
266  vpCameraParameters tmp;
267  getCameraParameters(tmp,640,480);
268  px_int = tmp.get_px();
269  py_int = tmp.get_py();
270  sceneInitialized = true;
271 }
272 
273 
289 void
292 {
293  this->projModel = proj_model;
294  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
295  if (arm_dir.size() > FILENAME_MAX)
296  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
297  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
298  if (full_length > FILENAME_MAX)
299  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
300 
301  // Use here default values of the robot constant parameters.
302  switch (tool) {
303  case vpAfma6::TOOL_CCMOP: {
304  _erc[0] = vpMath::rad(164.35); // rx
305  _erc[1] = vpMath::rad( 89.64); // ry
306  _erc[2] = vpMath::rad(-73.05); // rz
307  _etc[0] = 0.0117; // tx
308  _etc[1] = 0.0033; // ty
309  _etc[2] = 0.2272; // tz
310 
311  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
312 
313  if (robotArms != NULL)
314  {
315  while (get_displayBusy()) vpTime::wait(2);
316  free_Bound_scene (&(robotArms[5]));
317  char *name_arm = new char [full_length];
318  strcpy(name_arm, arm_dir.c_str());
319  strcat(name_arm,"/afma6_tool_ccmop.bnd");
320  set_scene(name_arm, robotArms+5, 1.0);
321  set_displayBusy(false);
322  delete [] name_arm;
323  }
324  break;
325  }
326  case vpAfma6::TOOL_GRIPPER: {
327  _erc[0] = vpMath::rad( 88.33); // rx
328  _erc[1] = vpMath::rad( 72.07); // ry
329  _erc[2] = vpMath::rad( 2.53); // rz
330  _etc[0] = 0.0783; // tx
331  _etc[1] = 0.1234; // ty
332  _etc[2] = 0.1638; // tz
333 
334  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
335 
336  if (robotArms != NULL)
337  {
338  while (get_displayBusy()) vpTime::wait(2);
339  free_Bound_scene (&(robotArms[5]));
340  char *name_arm = new char [full_length];
341  strcpy(name_arm, arm_dir.c_str());
342  strcat(name_arm,"/afma6_tool_gripper.bnd");
343  set_scene(name_arm, robotArms+5, 1.0);
344  set_displayBusy(false);
345  delete [] name_arm;
346  }
347  break;
348  }
349  case vpAfma6::TOOL_VACUUM: {
350  _erc[0] = vpMath::rad( 90.40); // rx
351  _erc[1] = vpMath::rad( 75.11); // ry
352  _erc[2] = vpMath::rad( 0.18); // rz
353  _etc[0] = 0.0038; // tx
354  _etc[1] = 0.1281; // ty
355  _etc[2] = 0.1658; // tz
356 
357  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
358 
359  if (robotArms != NULL)
360  {
361  while (get_displayBusy()) vpTime::wait(2);
362  free_Bound_scene (&(robotArms[5]));
363 
364  char *name_arm = new char [full_length];
365 
366  strcpy(name_arm, arm_dir.c_str());
367  strcat(name_arm,"/afma6_tool_vacuum.bnd");
368  set_scene(name_arm, robotArms+5, 1.0);
369  set_displayBusy(false);
370  delete [] name_arm;
371  }
372  break;
373  }
375  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
376  }
377  }
378 
379  vpRotationMatrix eRc(_erc);
380  this->_eMc.buildFrom(_etc, eRc);
381 
382  setToolType(tool);
383  return ;
384 }
385 
396 void
398  const unsigned int &image_width,
399  const unsigned int &image_height)
400 {
401  if (toolCustom)
402  {
403  vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
404  }
405  // Set default parameters
406  switch (getToolType()) {
407  case vpAfma6::TOOL_CCMOP: {
408  // Set default intrinsic camera parameters for 640x480 images
409  if (image_width == 640 && image_height == 480)
410  {
411  std::cout << "Get default camera parameters for camera \""
412  << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\"" << std::endl;
413  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
414  }
415  else {
416  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
417  }
418  break;
419  }
420  case vpAfma6::TOOL_GRIPPER: {
421  // Set default intrinsic camera parameters for 640x480 images
422  if (image_width == 640 && image_height == 480) {
423  std::cout << "Get default camera parameters for camera \""
424  << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
425  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
426  }
427  else {
428  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
429  }
430  break;
431  }
433  case vpAfma6::TOOL_VACUUM: {
434  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
435  break;
436  }
437  default:
438  vpERROR_TRACE ("This error should not occur!");
439  break;
440  }
441  return;
442 }
443 
452 void
454  const vpImage<unsigned char> &I_)
455 {
456  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
457 }
458 
467 void
469  const vpImage<vpRGBa> &I_)
470 {
471  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
472 }
473 
474 
480 void
482 {
483  px_int = cam.get_px();
484  py_int = cam.get_py();
485  toolCustom = true;
486 }
487 
488 
492 void
494 {
495  double tcur_1 = tcur;// temporary variable used to store the last time since the last command
496 
497  while (!robotStop)
498  {
499  //Get current time
500  tprev = tcur_1;
502 
504  setVelocityCalled = false;
505 
507 
508  double ellapsedTime = (tcur - tprev) * 1e-3;
509  if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
510  ellapsedTime = getSamplingTime(); // in second
511  }
512 
513  vpColVector articularCoordinates = get_artCoord();
514  articularCoordinates = get_artCoord();
515  vpColVector articularVelocities = get_artVel();
516 
517  if (jointLimit)
518  {
519  double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
520  if (art <= _joint_min[jointLimitArt-1] || art >= _joint_max[jointLimitArt-1]) {
521  if (verbose_) {
522  std::cout << "Joint " << jointLimitArt-1
523  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt-1]) << " < "
524  << vpMath::deg(art) << " < " << vpMath::deg(_joint_max[jointLimitArt-1]) << std::endl;
525  }
526 
527  articularVelocities = 0.0;
528  }
529  else
530  jointLimit = false;
531  }
532 
533  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
534  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
535  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
536  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
537  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
538  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
539 
540  int jl = isInJointLimit();
541 
542  if (jl != 0 && jointLimit == false)
543  {
544  if (jl < 0)
545  ellapsedTime = (_joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
546  else
547  ellapsedTime = (_joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
548 
549  for (unsigned int i = 0; i < 6; i++)
550  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
551 
552  jointLimit = true;
553  jointLimitArt = (unsigned int)fabs((double)jl);
554  }
555 
556  set_artCoord(articularCoordinates);
557  set_artVel(articularVelocities);
558 
559  compute_fMi();
560 
561  if (displayAllowed)
562  {
566  }
567 
569  {
570  while (get_displayBusy()) vpTime::wait(2);
572  set_displayBusy(false);
573  }
574 
575 
576  if (0/*displayType == MODEL_DH && displayAllowed*/)
577  {
578  vpHomogeneousMatrix fMit[8];
579  get_fMi(fMit);
580 
581  //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
582 
583  vpImagePoint iP, iP_1;
584  vpPoint pt;
585  pt.setWorldCoordinates (0,0,0);
586 
589  pt.track(getExternalCameraPosition ()*fMit[0]);
592  for (unsigned int k = 1; k < 7; k++)
593  {
594  pt.track(getExternalCameraPosition ()*fMit[k-1]);
596 
597  pt.track(getExternalCameraPosition ()*fMit[k]);
599 
601  }
603  }
604 
606 
607 
608  vpTime::wait( tcur, 1000*getSamplingTime() );
609  tcur_1 = tcur;
610  }else{
612  }
613  }
614 }
615 
623 void
625 {
626  //vpColVector q = get_artCoord();
627  vpColVector q(6);//; = get_artCoord();
628  q = get_artCoord();
629 
630  vpHomogeneousMatrix fMit[8];
631 
632  double q1 = q[0];
633  double q2 = q[1];
634  double q3 = q[2];
635  double q4 = q[3];
636  double q5 = q[4];
637  double q6 = q[5];
638 
639  double c4 = cos(q4);
640  double s4 = sin(q4);
641  double c5 = cos(q5);
642  double s5 = sin(q5);
643  double c6 = cos(q6);
644  double s6 = sin(q6);
645 
646  fMit[0][0][0] = 1;
647  fMit[0][1][0] = 0;
648  fMit[0][2][0] = 0;
649  fMit[0][0][1] = 0;
650  fMit[0][1][1] = 1;
651  fMit[0][2][1] = 0;
652  fMit[0][0][2] = 0;
653  fMit[0][1][2] = 0;
654  fMit[0][2][2] = 1;
655  fMit[0][0][3] = q1;
656  fMit[0][1][3] = 0;
657  fMit[0][2][3] = 0;
658 
659  fMit[1][0][0] = 1;
660  fMit[1][1][0] = 0;
661  fMit[1][2][0] = 0;
662  fMit[1][0][1] = 0;
663  fMit[1][1][1] = 1;
664  fMit[1][2][1] = 0;
665  fMit[1][0][2] = 0;
666  fMit[1][1][2] = 0;
667  fMit[1][2][2] = 1;
668  fMit[1][0][3] = q1;
669  fMit[1][1][3] = q2;
670  fMit[1][2][3] = 0;
671 
672  fMit[2][0][0] = 1;
673  fMit[2][1][0] = 0;
674  fMit[2][2][0] = 0;
675  fMit[2][0][1] = 0;
676  fMit[2][1][1] = 1;
677  fMit[2][2][1] = 0;
678  fMit[2][0][2] = 0;
679  fMit[2][1][2] = 0;
680  fMit[2][2][2] = 1;
681  fMit[2][0][3] = q1;
682  fMit[2][1][3] = q2;
683  fMit[2][2][3] = q3;
684 
685  fMit[3][0][0] = s4;
686  fMit[3][1][0] = -c4;
687  fMit[3][2][0] = 0;
688  fMit[3][0][1] = c4;
689  fMit[3][1][1] = s4;
690  fMit[3][2][1] = 0;
691  fMit[3][0][2] = 0;
692  fMit[3][1][2] = 0;
693  fMit[3][2][2] = 1;
694  fMit[3][0][3] = q1;
695  fMit[3][1][3] = q2;
696  fMit[3][2][3] = q3;
697 
698  fMit[4][0][0] = s4*s5;
699  fMit[4][1][0] = -c4*s5;
700  fMit[4][2][0] = c5;
701  fMit[4][0][1] = s4*c5;
702  fMit[4][1][1] = -c4*c5;
703  fMit[4][2][1] = -s5;
704  fMit[4][0][2] = c4;
705  fMit[4][1][2] = s4;
706  fMit[4][2][2] = 0;
707  fMit[4][0][3] = c4*this->_long_56+q1;
708  fMit[4][1][3] = s4*this->_long_56+q2;
709  fMit[4][2][3] = q3;
710 
711  fMit[5][0][0] = s4*s5*c6+c4*s6;
712  fMit[5][1][0] = -c4*s5*c6+s4*s6;
713  fMit[5][2][0] = c5*c6;
714  fMit[5][0][1] = -s4*s5*s6+c4*c6;
715  fMit[5][1][1] = c4*s5*s6+s4*c6;
716  fMit[5][2][1] = -c5*s6;
717  fMit[5][0][2] = -s4*c5;
718  fMit[5][1][2] = c4*c5;
719  fMit[5][2][2] = s5;
720  fMit[5][0][3] = c4*this->_long_56+q1;
721  fMit[5][1][3] = s4*this->_long_56+q2;
722  fMit[5][2][3] = q3;
723 
724  fMit[6][0][0] = fMit[5][0][0];
725  fMit[6][1][0] = fMit[5][1][0];
726  fMit[6][2][0] = fMit[5][2][0];
727  fMit[6][0][1] = fMit[5][0][1];
728  fMit[6][1][1] = fMit[5][1][1];
729  fMit[6][2][1] = fMit[5][2][1];
730  fMit[6][0][2] = fMit[5][0][2];
731  fMit[6][1][2] = fMit[5][1][2];
732  fMit[6][2][2] = fMit[5][2][2];
733  fMit[6][0][3] = fMit[5][0][3];
734  fMit[6][1][3] = fMit[5][1][3];
735  fMit[6][2][3] = fMit[5][2][3];
736 
737 // vpHomogeneousMatrix cMe;
738 // get_cMe(cMe);
739 // cMe = cMe.inverse();
740 // fMit[7] = fMit[6] * cMe;
741  vpAfma6::get_fMc(q,fMit[7]);
742 
743  #if defined(_WIN32)
744  WaitForSingleObject(mutex_fMi,INFINITE);
745  for (int i = 0; i < 8; i++)
746  fMi[i] = fMit[i];
747  ReleaseMutex(mutex_fMi);
748  #elif defined(VISP_HAVE_PTHREAD)
749  pthread_mutex_lock (&mutex_fMi);
750  for (int i = 0; i < 8; i++)
751  fMi[i] = fMit[i];
752  pthread_mutex_unlock (&mutex_fMi);
753  #endif
754 }
755 
756 
764 {
765  switch (newState) {
766  case vpRobot::STATE_STOP: {
767  // Start primitive STOP only if the current state is Velocity
769  stopMotion();
770  }
771  break;
772  }
775  std::cout << "Change the control mode from velocity to position control.\n";
776  stopMotion();
777  }
778  else {
779  //std::cout << "Change the control mode from stop to position control.\n";
780  }
781  break;
782  }
785  std::cout << "Change the control mode from stop to velocity control.\n";
786  }
787  break;
788  }
790  default:
791  break ;
792  }
793 
794  return vpRobot::setRobotState (newState);
795 }
796 
866 void
868 {
870  vpERROR_TRACE ("Cannot send a velocity to the robot "
871  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
873  "Cannot send a velocity to the robot "
874  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
875  }
876 
877  vpColVector vel_sat(6);
878 
879  double scale_trans_sat = 1;
880  double scale_rot_sat = 1;
881  double scale_sat = 1;
882  double vel_trans_max = getMaxTranslationVelocity();
883  double vel_rot_max = getMaxRotationVelocity();
884 
885  double vel_abs; // Absolute value
886 
887  // Velocity saturation
888  switch(frame)
889  {
890  // saturation in cartesian space
891  case vpRobot::CAMERA_FRAME :
893  {
894  if (vel.getRows() != 6)
895  {
896  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
897  throw;
898  }
899 
900  for (unsigned int i = 0 ; i < 3; ++ i)
901  {
902  vel_abs = fabs (vel[i]);
903  if (vel_abs > vel_trans_max && !jointLimit)
904  {
905  vel_trans_max = vel_abs;
906  vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
907  "(axis nr. %d).", vel[i], i+1);
908  }
909 
910  vel_abs = fabs (vel[i+3]);
911  if (vel_abs > vel_rot_max && !jointLimit) {
912  vel_rot_max = vel_abs;
913  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
914  "(axis nr. %d).", vel[i+3], i+4);
915  }
916  }
917 
918  if (vel_trans_max > getMaxTranslationVelocity())
919  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
920 
921  if (vel_rot_max > getMaxRotationVelocity())
922  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
923 
924  if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
925  {
926  if (scale_trans_sat < scale_rot_sat)
927  scale_sat = scale_trans_sat;
928  else
929  scale_sat = scale_rot_sat;
930  }
931  break;
932  }
933 
934  // saturation in joint space
936  {
937  if (vel.getRows() != 6)
938  {
939  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
940  throw;
941  }
942  for (unsigned int i = 0 ; i < 6; ++ i)
943  {
944  vel_abs = fabs (vel[i]);
945  if (vel_abs > vel_rot_max && !jointLimit)
946  {
947  vel_rot_max = vel_abs;
948  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
949  "(axis nr. %d).", vel[i], i+1);
950  }
951  }
952  if (vel_rot_max > getMaxRotationVelocity())
953  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
954  if ( scale_rot_sat < 1 )
955  scale_sat = scale_rot_sat;
956  break;
957  }
958  case vpRobot::MIXT_FRAME :
959  {
960  break;
961  }
962  }
963 
964  set_velocity (vel * scale_sat);
965  setRobotFrame (frame);
966  setVelocityCalled = true;
967 }
968 
969 
973 void
975 {
977 
978  double scale_rot_sat = 1;
979  double scale_sat = 1;
980  double vel_rot_max = getMaxRotationVelocity();
981  double vel_abs;
982 
983  vpColVector articularCoordinates = get_artCoord();
984  vpColVector velocityframe = get_velocity();
985  vpColVector articularVelocity;
986 
987  switch(frame)
988  {
989  case vpRobot::CAMERA_FRAME :
990  {
991  vpMatrix eJe_;
993  vpAfma6::get_eJe(articularCoordinates,eJe_);
994  eJe_ = eJe_.pseudoInverse();
996  singularityTest(articularCoordinates,eJe_);
997  articularVelocity = eJe_*eVc*velocityframe;
998  set_artVel (articularVelocity);
999  break;
1000  }
1002  {
1003  vpMatrix fJe_;
1004  vpAfma6::get_fJe(articularCoordinates,fJe_);
1005  fJe_ = fJe_.pseudoInverse();
1007  singularityTest(articularCoordinates,fJe_);
1008  articularVelocity = fJe_*velocityframe;
1009  set_artVel (articularVelocity);
1010  break;
1011  }
1013  {
1014  articularVelocity = velocityframe;
1015  set_artVel (articularVelocity);
1016  break;
1017  }
1018  case vpRobot::MIXT_FRAME :
1019  {
1020  break;
1021  }
1022  }
1023 
1024 
1025 
1026  switch(frame)
1027  {
1028  case vpRobot::CAMERA_FRAME :
1030  {
1031  for (unsigned int i = 0 ; i < 6; ++ i)
1032  {
1033  vel_abs = fabs (articularVelocity[i]);
1034  if (vel_abs > vel_rot_max && !jointLimit)
1035  {
1036  vel_rot_max = vel_abs;
1037  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
1038  "(axis nr. %d).", articularVelocity[i], i+1);
1039  }
1040  }
1041  if (vel_rot_max > getMaxRotationVelocity())
1042  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1043  if ( scale_rot_sat < 1 )
1044  scale_sat = scale_rot_sat;
1045 
1046  set_artVel(articularVelocity * scale_sat);
1047  break;
1048  }
1050  case vpRobot::MIXT_FRAME :
1051  {
1052  break;
1053  }
1054  }
1055 }
1056 
1057 
1104 void
1106 {
1107  vel.resize(6);
1108 
1109  vpColVector articularCoordinates = get_artCoord();
1110  vpColVector articularVelocity = get_artVel();
1111 
1112  switch(frame)
1113  {
1114  case vpRobot::CAMERA_FRAME :
1115  {
1116  vpMatrix eJe_;
1118  vpAfma6::get_eJe(articularCoordinates,eJe_);
1119  vel = cVe*eJe_*articularVelocity;
1120  break ;
1121  }
1122  case vpRobot::ARTICULAR_FRAME :
1123  {
1124  vel = articularVelocity;
1125  break ;
1126  }
1127  case vpRobot::REFERENCE_FRAME :
1128  {
1129  vpMatrix fJe_;
1130  vpAfma6::get_fJe(articularCoordinates,fJe_);
1131  vel = fJe_*articularVelocity;
1132  break ;
1133  }
1134  case vpRobot::MIXT_FRAME :
1135  {
1136  break ;
1137  }
1138  default:
1139  {
1140  vpERROR_TRACE ("Error in spec of vpRobot. "
1141  "Case not taken in account.");
1142  return;
1143  }
1144  }
1145 }
1146 
1163 void
1165 {
1166  timestamp = vpTime::measureTimeSecond();
1167  getVelocity(frame, vel);
1168 }
1169 
1214 {
1215  vpColVector vel(6);
1216  getVelocity (frame, vel);
1217 
1218  return vel;
1219 }
1220 
1235 {
1236  timestamp = vpTime::measureTimeSecond();
1237  vpColVector vel(6);
1238  getVelocity (frame, vel);
1239 
1240  return vel;
1241 }
1242 
1243 void
1245 {
1246  double vel_rot_max = getMaxRotationVelocity();
1247  double velmax = fabs(q[0]);
1248  for (unsigned int i = 1; i < 6; i++)
1249  {
1250  if (velmax < fabs(q[i]))
1251  velmax = fabs(q[i]);
1252  }
1253 
1254  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
1255  q = q * alpha;
1256 }
1257 
1332 void
1334 {
1336  {
1337  vpERROR_TRACE ("Robot was not in position-based control\n"
1338  "Modification of the robot state");
1339  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1340  }
1341 
1342  vpColVector articularCoordinates = get_artCoord();
1343 
1344  vpColVector error(6);
1345  double errsqr = 0;
1346  switch(frame)
1347  {
1348  case vpRobot::CAMERA_FRAME :
1349  {
1350  int nbSol;
1351  vpColVector qdes(6);
1352 
1353  vpTranslationVector txyz;
1354  vpRxyzVector rxyz;
1355  for (unsigned int i=0; i < 3; i++)
1356  {
1357  txyz[i] = q[i];
1358  rxyz[i] = q[i+3];
1359  }
1360 
1361  vpRotationMatrix cRc2(rxyz);
1362  vpHomogeneousMatrix cMc2(txyz, cRc2);
1363 
1364  vpHomogeneousMatrix fMc_;
1365  vpAfma6::get_fMc(articularCoordinates, fMc_);
1366 
1367  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1368 
1369  do
1370  {
1371  articularCoordinates = get_artCoord();
1372  qdes = articularCoordinates;
1373  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1374  setVelocityCalled = true;
1375  if (nbSol > 0)
1376  {
1377  error = qdes - articularCoordinates;
1378  errsqr = error.sumSquare();
1379  //findHighestPositioningSpeed(error);
1380  set_artVel(error);
1381  if (errsqr < 1e-4)
1382  {
1383  set_artCoord (qdes);
1384  error = 0;
1385  set_artVel(error);
1386  set_velocity(error);
1387  break;
1388  }
1389  }
1390  else
1391  {
1392  vpERROR_TRACE ("Positionning error.");
1394  "Position out of range.");
1395  }
1396  }while (errsqr > 1e-8 && nbSol > 0);
1397 
1398  break ;
1399  }
1400 
1402  {
1403  do
1404  {
1405  articularCoordinates = get_artCoord();
1406  error = q - articularCoordinates;
1407  errsqr = error.sumSquare();
1408  //findHighestPositioningSpeed(error);
1409  set_artVel(error);
1410  setVelocityCalled = true;
1411  if (errsqr < 1e-4)
1412  {
1413  set_artCoord (q);
1414  error = 0;
1415  set_artVel(error);
1416  set_velocity(error);
1417  break;
1418  }
1419  }while (errsqr > 1e-8);
1420  break ;
1421  }
1422 
1424  {
1425  int nbSol;
1426  vpColVector qdes(6);
1427 
1428  vpTranslationVector txyz;
1429  vpRxyzVector rxyz;
1430  for (unsigned int i=0; i < 3; i++)
1431  {
1432  txyz[i] = q[i];
1433  rxyz[i] = q[i+3];
1434  }
1435 
1436  vpRotationMatrix fRc(rxyz);
1437  vpHomogeneousMatrix fMc_(txyz, fRc);
1438 
1439  do
1440  {
1441  articularCoordinates = get_artCoord();
1442  qdes = articularCoordinates;
1443  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1444  setVelocityCalled = true;
1445  if (nbSol > 0)
1446  {
1447  error = qdes - articularCoordinates;
1448  errsqr = error.sumSquare();
1449  //findHighestPositioningSpeed(error);
1450  set_artVel(error);
1451  if (errsqr < 1e-4)
1452  {
1453  set_artCoord (qdes);
1454  error = 0;
1455  set_artVel(error);
1456  set_velocity(error);
1457  break;
1458  }
1459  }
1460  else
1461  vpERROR_TRACE ("Positionning error. Position unreachable");
1462  }while (errsqr > 1e-8 && nbSol > 0);
1463  break ;
1464  }
1465  case vpRobot::MIXT_FRAME:
1466  {
1467  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1469  "Positionning error: "
1470  "Mixt frame not implemented.");
1471  break ;
1472  }
1473  }
1474 }
1475 
1540  const double pos1,
1541  const double pos2,
1542  const double pos3,
1543  const double pos4,
1544  const double pos5,
1545  const double pos6)
1546 {
1547  try{
1548  vpColVector position(6) ;
1549  position[0] = pos1 ;
1550  position[1] = pos2 ;
1551  position[2] = pos3 ;
1552  position[3] = pos4 ;
1553  position[4] = pos5 ;
1554  position[5] = pos6 ;
1555 
1556  setPosition(frame, position) ;
1557  }
1558  catch(...)
1559  {
1560  vpERROR_TRACE("Error caught");
1561  throw ;
1562  }
1563 }
1564 
1600 void vpSimulatorAfma6::setPosition(const char *filename)
1601 {
1602  vpColVector q;
1603  bool ret;
1604 
1605  ret = this->readPosFile(filename, q);
1606 
1607  if (ret == false) {
1608  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1610  "Bad position in filename.");
1611  }
1614 }
1615 
1675 void
1677 {
1678  q.resize(6);
1679 
1680  switch(frame)
1681  {
1682  case vpRobot::CAMERA_FRAME :
1683  {
1684  q = 0;
1685  break ;
1686  }
1687 
1689  {
1690  q = get_artCoord();
1691  break ;
1692  }
1693 
1695  {
1696  vpHomogeneousMatrix fMc_;
1697  vpAfma6::get_fMc (get_artCoord(), fMc_);
1698 
1699  vpRotationMatrix fRc;
1700  fMc_.extract(fRc);
1701  vpRxyzVector rxyz(fRc);
1702 
1703  vpTranslationVector txyz;
1704  fMc_.extract(txyz);
1705 
1706  for (unsigned int i=0; i <3; i++)
1707  {
1708  q[i] = txyz[i];
1709  q[i+3] = rxyz[i];
1710  }
1711  break ;
1712  }
1713 
1714  case vpRobot::MIXT_FRAME:
1715  {
1716  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1718  "Positionning error: "
1719  "Mixt frame not implemented.");
1720  break ;
1721  }
1722  }
1723 }
1724 
1751 void
1753 {
1754  timestamp = vpTime::measureTimeSecond();
1755  getPosition(frame, q);
1756 }
1757 
1758 
1769 void
1771 {
1772  vpColVector posRxyz;
1773  //recupere position en Rxyz
1774  this->getPosition(frame,posRxyz);
1775 
1776  //recupere le vecteur thetaU correspondant
1777  vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
1778 
1779  //remplit le vpPoseVector avec translation et rotation ThetaU
1780  for(unsigned int j=0;j<3;j++)
1781  {
1782  position[j]=posRxyz[j];
1783  position[j+3]=RtuVect[j];
1784  }
1785 }
1786 
1797 void
1799  vpPoseVector &position, double &timestamp)
1800 {
1801  timestamp = vpTime::measureTimeSecond();
1802  getPosition(frame, position);
1803 }
1804 
1811 void
1813 {
1814  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
1815  {
1816  vpTRACE("Joint limit vector has not a size of 6 !");
1817  return;
1818  }
1819 
1820  _joint_min[0] = limitMin[0];
1821  _joint_min[1] = limitMin[1];
1822  _joint_min[2] = limitMin[2];
1823  _joint_min[3] = limitMin[3];
1824  _joint_min[4] = limitMin[4];
1825  _joint_min[5] = limitMin[5];
1826 
1827  _joint_max[0] = limitMax[0];
1828  _joint_max[1] = limitMax[1];
1829  _joint_max[2] = limitMax[2];
1830  _joint_max[3] = limitMax[3];
1831  _joint_max[4] = limitMax[4];
1832  _joint_max[5] = limitMax[5];
1833 
1834 }
1835 
1841 bool
1843 {
1844  double q5 = q[4];
1845 
1846  bool cond = fabs(q5-M_PI/2) < 1e-1;
1847 
1848  if(cond)
1849  {
1850  J[0][3] = 0;
1851  J[0][4] = 0;
1852  J[1][3] = 0;
1853  J[1][4] = 0;
1854  J[3][3] = 0;
1855  J[3][4] = 0;
1856  J[5][3] = 0;
1857  J[5][4] = 0;
1858  return true;
1859  }
1860 
1861  return false;
1862 }
1863 
1867 int
1869 {
1870  int artNumb = 0;
1871  double diff = 0;
1872  double difft = 0;
1873 
1874  vpColVector articularCoordinates = get_artCoord();
1875 
1876  for (unsigned int i = 0; i < 6; i++)
1877  {
1878  if (articularCoordinates[i] <= _joint_min[i])
1879  {
1880  difft = _joint_min[i] - articularCoordinates[i];
1881  if (difft > diff)
1882  {
1883  diff = difft;
1884  artNumb = -(int)i-1;
1885  }
1886  }
1887  }
1888 
1889  for (unsigned int i = 0; i < 6; i++)
1890  {
1891  if (articularCoordinates[i] >= _joint_max[i])
1892  {
1893  difft = articularCoordinates[i] - _joint_max[i];
1894  if (difft > diff)
1895  {
1896  diff = difft;
1897  artNumb = (int)(i+1);
1898  }
1899  }
1900  }
1901 
1902  if (artNumb != 0)
1903  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
1904 
1905  return artNumb;
1906 }
1907 
1919 void
1920 vpSimulatorAfma6::getCameraDisplacement(vpColVector &displacement)
1921 {
1922  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1923 }
1924 
1934 void
1935 vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
1936 {
1938 }
1939 
1958 void
1960  vpColVector &displacement)
1961 {
1962  displacement.resize (6);
1963  displacement = 0;
1964  vpColVector q_cur(6);
1965 
1966  q_cur = get_artCoord();
1967 
1968  if ( ! first_time_getdis )
1969  {
1970  switch (frame)
1971  {
1972  case vpRobot::CAMERA_FRAME:
1973  {
1974  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1975  return;
1976  break ;
1977  }
1978 
1980  {
1981  displacement = q_cur - q_prev_getdis;
1982  break ;
1983  }
1984 
1986  {
1987  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1988  return;
1989  break ;
1990  }
1991 
1992  case vpRobot::MIXT_FRAME:
1993  {
1994  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1995  return;
1996  break ;
1997  }
1998  }
1999  }
2000  else
2001  {
2002  first_time_getdis = false;
2003  }
2004 
2005  // Memorize the joint position for the next call
2006  q_prev_getdis = q_cur;
2007 }
2008 
2055 bool
2057 {
2058  FILE * fd ;
2059  fd = fopen(filename, "r") ;
2060  if (fd == NULL)
2061  return false;
2062 
2063  char line[FILENAME_MAX];
2064  char dummy[FILENAME_MAX];
2065  char head[] = "R:";
2066  bool sortie = false;
2067 
2068  do {
2069  // Saut des lignes commencant par #
2070  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2071  if ( strncmp (line, "#", 1) != 0) {
2072  // La ligne n'est pas un commentaire
2073  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2074  sortie = true; // Position robot trouvee.
2075  }
2076  // else
2077  // return (false); // fin fichier sans position robot.
2078  }
2079  }
2080  else {
2081  fclose(fd) ;
2082  return (false); /* fin fichier */
2083  }
2084 
2085  }
2086  while ( sortie != true );
2087 
2088  // Lecture des positions
2089  q.resize(njoint);
2090  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2091  dummy,
2092  &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2093 
2094  if (ret != 7) {
2095  fclose(fd) ;
2096  return false;
2097  }
2098 
2099  // converts rotations from degrees into radians
2100  //q.deg2rad();
2101 
2102  q[3] = vpMath::rad(q[3]);
2103  q[4] = vpMath::rad(q[4]);
2104  q[5] = vpMath::rad(q[5]);
2105 
2106  fclose(fd) ;
2107  return (true);
2108 }
2109 
2132 bool
2133 vpSimulatorAfma6::savePosFile(const char *filename, const vpColVector &q)
2134 {
2135 
2136  FILE * fd ;
2137  fd = fopen(filename, "w") ;
2138  if (fd == NULL)
2139  return false;
2140 
2141  fprintf(fd, "\
2142 #AFMA6 - Position - Version 2.01\n\
2143 #\n\
2144 # R: X Y Z A B C\n\
2145 # Joint position: X, Y, Z: translations in meters\n\
2146 # A, B, C: rotations in degrees\n\
2147 #\n\
2148 #\n\n");
2149 
2150  // Save positions in mm and deg
2151  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
2152  q[0],
2153  q[1],
2154  q[2],
2155  vpMath::deg(q[3]),
2156  vpMath::deg(q[4]),
2157  vpMath::deg(q[5]));
2158 
2159  fclose(fd) ;
2160  return (true);
2161 }
2162 
2170 void
2171 vpSimulatorAfma6::move(const char *filename)
2172 {
2173  vpColVector q;
2174 
2175  try
2176  {
2177  this->readPosFile(filename, q);
2180  }
2181  catch(...)
2182  {
2183  throw;
2184  }
2185 }
2186 
2196 void
2198 {
2199  vpAfma6::get_cMe(cMe) ;
2200 }
2201 
2209 void
2211 {
2212  vpHomogeneousMatrix cMe ;
2213  vpAfma6::get_cMe(cMe) ;
2214 
2215  cVe.buildFrom(cMe) ;
2216 }
2217 
2227 void
2229 {
2230  try
2231  {
2232  vpAfma6::get_eJe(get_artCoord(), eJe_) ;
2233  }
2234  catch(...)
2235  {
2236  vpERROR_TRACE("catch exception ") ;
2237  throw ;
2238  }
2239 }
2240 
2251 void
2253 {
2254  try
2255  {
2256  vpColVector articularCoordinates = get_artCoord();
2257  vpAfma6::get_fJe(articularCoordinates, fJe_) ;
2258  }
2259  catch(...)
2260  {
2261  vpERROR_TRACE("Error caught");
2262  throw ;
2263  }
2264 }
2265 
2269 void
2271 {
2273  return;
2274 
2275  vpColVector stop(6);
2276  stop = 0;
2277  set_artVel(stop);
2278  set_velocity(stop);
2280 }
2281 
2282 
2283 
2284 /**********************************************************************************/
2285 /**********************************************************************************/
2286 /**********************************************************************************/
2287 /**********************************************************************************/
2288 
2297 void
2299 {
2300  // set scene_dir from #define VISP_SCENE_DIR if it exists
2301  std::string scene_dir_;
2302  if (vpIoTools::checkDirectory(VISP_SCENES_DIR) == true) // directory exists
2303  scene_dir_ = VISP_SCENES_DIR;
2304  else {
2305  try {
2306  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2307  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2308  }
2309  catch (...) {
2310  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2311  }
2312  }
2313 
2314  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2315  if (scene_dir_.size() > FILENAME_MAX)
2316  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2317  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2318  if (full_length > FILENAME_MAX)
2319  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2320 
2321  char *name_cam = new char [full_length];
2322 
2323  strcpy(name_cam, scene_dir_.c_str());
2324  strcat(name_cam,"/camera.bnd");
2325  set_scene(name_cam,&camera,cameraFactor);
2326 
2327  if (arm_dir.size() > FILENAME_MAX)
2328  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2329  full_length = (unsigned int)arm_dir.size() + name_length;
2330  if (full_length > FILENAME_MAX)
2331  throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
2332 
2333  char *name_arm = new char [full_length];
2334  strcpy(name_arm, arm_dir.c_str());
2335  strcat(name_arm,"/afma6_gate.bnd");
2336  set_scene(name_arm, robotArms, 1.0);
2337  strcpy(name_arm, arm_dir.c_str());
2338  strcat(name_arm,"/afma6_arm1.bnd");
2339  set_scene(name_arm, robotArms+1, 1.0);
2340  strcpy(name_arm, arm_dir.c_str());
2341  strcat(name_arm,"/afma6_arm2.bnd");
2342  set_scene(name_arm, robotArms+2, 1.0);
2343  strcpy(name_arm, arm_dir.c_str());
2344  strcat(name_arm,"/afma6_arm3.bnd");
2345  set_scene(name_arm, robotArms+3, 1.0);
2346  strcpy(name_arm, arm_dir.c_str());
2347  strcat(name_arm,"/afma6_arm4.bnd");
2348  set_scene(name_arm, robotArms+4, 1.0);
2349 
2351  tool = getToolType();
2352  strcpy(name_arm, arm_dir.c_str());
2353  switch (tool) {
2354  case vpAfma6::TOOL_CCMOP: {
2355  strcat(name_arm,"/afma6_tool_ccmop.bnd");
2356  break;
2357  }
2358  case vpAfma6::TOOL_GRIPPER: {
2359  strcat(name_arm,"/afma6_tool_gripper.bnd");
2360  break;
2361  }
2362  case vpAfma6::TOOL_VACUUM: {
2363  strcat(name_arm,"/afma6_tool_vacuum.bnd");
2364  break;
2365  }
2367  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2368  break;
2369  }
2370  }
2371  set_scene(name_arm, robotArms+5, 1.0);
2372 
2373  add_rfstack(IS_BACK);
2374 
2375  add_vwstack ("start","depth", 0.0, 100.0);
2376  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
2377  add_vwstack ("start","type", PERSPECTIVE);
2378 //
2379 // sceneInitialized = true;
2380 // displayObject = true;
2381  displayCamera = true;
2382 
2383  delete [] name_cam;
2384  delete [] name_arm;
2385 }
2386 
2387 
2388 void
2390 {
2391  bool changed = false;
2392  vpHomogeneousMatrix displacement = navigation(I_,changed);
2393 
2394  //if (displacement[2][3] != 0)
2395  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2396  camMf2 = camMf2*displacement;
2397 
2398  f2Mf = camMf2.inverse()*camMf;
2399 
2400  camMf = camMf2* displacement * f2Mf;
2401 
2402  double u;
2403  double v;
2404  //if(px_ext != 1 && py_ext != 1)
2405  // we assume px_ext and py_ext > 0
2406  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
2407  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
2408  {
2409  u = (double)I_.getWidth()/(2*px_ext);
2410  v = (double)I_.getHeight()/(2*py_ext);
2411  }
2412  else
2413  {
2414  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2415  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2416  }
2417 
2418  float w44o[4][4],w44cext[4][4],x,y,z;
2419 
2420  vp2jlc_matrix(camMf.inverse(),w44cext);
2421 
2422  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2423  x = w44cext[2][0] + w44cext[3][0];
2424  y = w44cext[2][1] + w44cext[3][1];
2425  z = w44cext[2][2] + w44cext[3][2];
2426  add_vwstack ("start","vrp", x,y,z);
2427  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2428  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2429  add_vwstack ("start","window", -u, u, -v, v);
2430 
2431  vpHomogeneousMatrix fMit[8];
2432  get_fMi(fMit);
2433 
2434  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
2435  display_scene(w44o,robotArms[0],I_, curColor);
2436 
2437  vp2jlc_matrix(fMit[0],w44o);
2438  display_scene(w44o,robotArms[1],I_, curColor);
2439 
2440  vp2jlc_matrix(fMit[2],w44o);
2441  display_scene(w44o,robotArms[2],I_, curColor);
2442 
2443  vp2jlc_matrix(fMit[3],w44o);
2444  display_scene(w44o,robotArms[3],I_, curColor);
2445 
2446  vp2jlc_matrix(fMit[4],w44o);
2447  display_scene(w44o,robotArms[4],I_, curColor);
2448 
2449  vp2jlc_matrix(fMit[5],w44o);
2450  display_scene(w44o,robotArms[5],I_, curColor);
2451 
2452  if (displayCamera)
2453  {
2454  vpHomogeneousMatrix cMe;
2455  get_cMe(cMe);
2456  cMe = cMe.inverse();
2457  cMe = fMit[6] * cMe;
2458  vp2jlc_matrix(cMe,w44o);
2459  display_scene(w44o,camera, I_, camColor);
2460  }
2461 
2462  if (displayObject)
2463  {
2464  vp2jlc_matrix(fMo,w44o);
2465  display_scene(w44o,scene,I_, curColor);
2466  }
2467 }
2468 
2485 bool
2487 {
2488  vpColVector stop(6);
2489  bool status = true;
2490  stop = 0;
2491  set_artVel(stop);
2492  set_velocity(stop);
2493  vpHomogeneousMatrix fMc_;
2494  fMc_ = fMo * cMo_.inverse();
2495 
2496  vpColVector articularCoordinates = get_artCoord();
2497  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2498 
2499  if (nbSol == 0) {
2500  status = false;
2501  vpERROR_TRACE ("Positionning error. Position unreachable");
2502  }
2503 
2504  if (verbose_)
2505  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2506 
2507  set_artCoord(articularCoordinates);
2508 
2509  compute_fMi();
2510 
2511  return status;
2512 }
2513 
2526 void
2528 {
2529  vpColVector stop(6);
2530  stop = 0;
2531  set_artVel(stop);
2532  set_velocity(stop);
2533  vpHomogeneousMatrix fMit[8];
2534  get_fMi(fMit);
2535  fMo = fMit[7] * cMo_;
2536 }
2537 
2548 bool
2550 {
2551  // get rid of max velocity
2552  double vMax = getMaxTranslationVelocity();
2553  double wMax = getMaxRotationVelocity();
2554  setMaxTranslationVelocity(10.*vMax);
2555  setMaxRotationVelocity(10.*wMax);
2556 
2557  vpColVector v(3),w(3),vel(6);
2558  vpHomogeneousMatrix cdMc;
2560  vpColVector err(6);err=1.;
2561  const double lambda = 5.;
2562  double t;
2563 
2565 
2566  unsigned int i,iter=0;
2567  while((iter++<300) & (err.euclideanNorm()>errMax))
2568  {
2569  t = vpTime::measureTimeMs();
2570 
2571  // update image
2572  if(Iint != NULL)
2573  {
2574  vpDisplay::display(*Iint);
2575  getInternalView(*Iint);
2576  vpDisplay::flush(*Iint);
2577  }
2578 
2579  // update pose error
2580  cdMc = cdMo_*get_cMo().inverse();
2581  cdMc.extract(cdRc);
2582  cdMc.extract(cdTc);
2583  cdTUc.buildFrom(cdRc);
2584 
2585  // compute v,w and velocity
2586  v = -lambda*cdRc.t()*cdTc;
2587  w = -lambda*cdTUc;
2588  for(i=0;i<3;++i)
2589  {
2590  vel[i] = v[i];
2591  vel[i+3] = w[i];
2592  err[i] = cdTc[i];
2593  err[i+3] = cdTUc[i];
2594  }
2595 
2596  // update feat
2598 
2599  // wait for it
2600  vpTime::wait(t,10);
2601  }
2602  vel=0.;
2603  set_velocity(vel);
2604  set_artVel(vel);
2606  setMaxRotationVelocity(wMax);
2607 
2608  //std::cout << "setPosition: final error " << err.t() << std::endl;
2609  return(err.euclideanNorm()<= errMax);
2610 }
2611 
2612 #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:509
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:335
unsigned int getWidth() const
Definition: vpImage.h:159
void setMaxTranslationVelocity(const double maxVt)
Definition: vpRobot.cpp:227
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:208
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:239
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
return sum of the Aij^2 (for all i, for all j)
Definition: vpMatrix.cpp:809
static const vpColor green
Definition: vpColor.h:170
vpHomogeneousMatrix fMo
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:1994
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:264
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:190
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:206
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:861
vpRowVector t() const
transpose of 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 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)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay.cpp:371
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:251
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:2899
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:150
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:197
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:1861
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:92
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:94