Visual Servoing Platform  version 3.0.0
vpSimulatorViper850.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2015 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Class which provides a simulator for the robot Viper850.
32  *
33  * Authors:
34  * Nicolas Melchior
35  *
36  *****************************************************************************/
37 
38 
39 
40 #include <visp3/robot/vpSimulatorViper850.h>
41 
42 #if defined(VISP_HAVE_MODULE_GUI) && (defined(_WIN32) || defined(VISP_HAVE_PTHREAD))
43 
44 #include <visp3/core/vpTime.h>
45 #include <visp3/core/vpImagePoint.h>
46 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpMeterPixelConversion.h>
48 #include <visp3/core/vpIoTools.h>
49 #include <cmath> // std::fabs
50 #include <limits> // numeric_limits
51 #include <string>
52 
54 
60  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
61  zeroPos(), reposPos(), toolCustom(false), arm_dir()
62 {
63  init();
64  initDisplay();
65 
67 
68  #if defined(_WIN32)
69  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
70  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
71  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
72  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
73  mutex_display = CreateMutex(NULL,FALSE,NULL);
74 
75 
76  DWORD dwThreadIdArray;
77  hThread = CreateThread(
78  NULL, // default security attributes
79  0, // use default stack size
80  launcher, // thread function name
81  this, // argument to thread function
82  0, // use default creation flags
83  &dwThreadIdArray); // returns the thread identifier
84  #elif defined (VISP_HAVE_PTHREAD)
85  pthread_mutex_init(&mutex_fMi, NULL);
86  pthread_mutex_init(&mutex_artVel, NULL);
87  pthread_mutex_init(&mutex_artCoord, NULL);
88  pthread_mutex_init(&mutex_velocity, NULL);
89  pthread_mutex_init(&mutex_display, NULL);
90 
91  pthread_attr_init(&attr);
92  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
93 
94  pthread_create(&thread, NULL, launcher, (void *)this);
95  #endif
96 
97  compute_fMi();
98 }
99 
107  : vpRobotWireFrameSimulator(do_display),
108  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
109  zeroPos(), reposPos(), toolCustom(false), arm_dir()
110 {
111  init();
112  initDisplay();
113 
115 
116  #if defined(_WIN32)
117  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
118  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
119  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
120  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
121  mutex_display = CreateMutex(NULL,FALSE,NULL);
122 
123 
124  DWORD dwThreadIdArray;
125  hThread = CreateThread(
126  NULL, // default security attributes
127  0, // use default stack size
128  launcher, // thread function name
129  this, // argument to thread function
130  0, // use default creation flags
131  &dwThreadIdArray); // returns the thread identifier
132  #elif defined(VISP_HAVE_PTHREAD)
133  pthread_mutex_init(&mutex_fMi, NULL);
134  pthread_mutex_init(&mutex_artVel, NULL);
135  pthread_mutex_init(&mutex_artCoord, NULL);
136  pthread_mutex_init(&mutex_velocity, NULL);
137  pthread_mutex_init(&mutex_display, NULL);
138 
139  pthread_attr_init(&attr);
140  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
141 
142  pthread_create(&thread, NULL, launcher, (void *)this);
143  #endif
144 
145  compute_fMi();
146 }
147 
152 {
153  robotStop = true;
154 
155  #if defined(_WIN32)
156  WaitForSingleObject(hThread,INFINITE);
157  CloseHandle(hThread);
158  CloseHandle(mutex_fMi);
159  CloseHandle(mutex_artVel);
160  CloseHandle(mutex_artCoord);
161  CloseHandle(mutex_velocity);
162  CloseHandle(mutex_display);
163  #elif defined(VISP_HAVE_PTHREAD)
164  pthread_attr_destroy(&attr);
165  pthread_join(thread, NULL);
166  pthread_mutex_destroy(&mutex_fMi);
167  pthread_mutex_destroy(&mutex_artVel);
168  pthread_mutex_destroy(&mutex_artCoord);
169  pthread_mutex_destroy(&mutex_velocity);
170  pthread_mutex_destroy(&mutex_display);
171  #endif
172 
173  if (robotArms != NULL)
174  {
175  // free_Bound_scene (&(camera));
176  for(int i = 0; i < 6; i++)
177  free_Bound_scene (&(robotArms[i]));
178  }
179 
180  delete[] robotArms;
181  delete[] fMi;
182 }
183 
192 void
194 {
195  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
196  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
197  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
198  bool armDirExists = false;
199  for(size_t i=0; i < arm_dirs.size(); i++)
200  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
201  arm_dir = arm_dirs[i];
202  armDirExists = true;
203  break;
204  }
205  if (! armDirExists) {
206  try {
207  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
208  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
209  }
210  catch (...) {
211  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
212  }
213  }
214 
216  toolCustom = false;
217 
218  size_fMi = 8;
219  fMi = new vpHomogeneousMatrix[8];
222 
223  zeroPos.resize(njoint);
224  zeroPos = 0;
225  zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
226  reposPos.resize(njoint);
227  reposPos = 0;
228  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
229 
230  artCoord = reposPos;
231  artVel = 0;
232 
233  q_prev_getdis.resize(njoint);
234  q_prev_getdis = 0;
235  first_time_getdis = true;
236 
237  positioningVelocity = defaultPositioningVelocity ;
238 
241 
242  // Software joint limits in radians
243  //joint_min.resize(njoint);
244  joint_min[0] = vpMath::rad(-50);
245  joint_min[1] = vpMath::rad(-135);
246  joint_min[2] = vpMath::rad(-40);
247  joint_min[3] = vpMath::rad(-190);
248  joint_min[4] = vpMath::rad(-110);
249  joint_min[5] = vpMath::rad(-184);
250  //joint_max.resize(njoint);
251  joint_max[0] = vpMath::rad(50);
252  joint_max[1] = vpMath::rad(-40);
253  joint_max[2] = vpMath::rad(215);
254  joint_max[3] = vpMath::rad(190);
255  joint_max[4] = vpMath::rad(110);
256  joint_max[5] = vpMath::rad(184);
257 }
258 
262 void
264 {
265  robotArms = NULL;
266  robotArms = new Bound_scene[6];
267  initArms();
269  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
271  vpCameraParameters tmp;
272  getCameraParameters(tmp,640,480);
273  px_int = tmp.get_px();
274  py_int = tmp.get_py();
275  sceneInitialized = true;
276 }
277 
278 
294 void
297 {
298  this->projModel = proj_model;
299 
300  // Use here default values of the robot constant parameters.
301  switch (tool) {
303  erc[0] = vpMath::rad(0.07); // rx
304  erc[1] = vpMath::rad(2.76); // ry
305  erc[2] = vpMath::rad(-91.50); // rz
306  etc[0] = -0.0453; // tx
307  etc[1] = 0.0005; // ty
308  etc[2] = 0.0728; // tz
309 
310  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
311  break;
312  }
314  erc[0] = vpMath::rad(0.1527764261); // rx
315  erc[1] = vpMath::rad(1.285092455); // ry
316  erc[2] = vpMath::rad(-90.75777848); // rz
317  etc[0] = -0.04558630174; // tx
318  etc[1] = -0.00134326752; // ty
319  etc[2] = 0.001000828017; // tz
320 
321  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
322  break;
323  }
326  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
327  break;
328  }
329  }
330 
331  vpRotationMatrix eRc(erc);
332  this->eMc.buildFrom(etc, eRc);
333 
334  setToolType(tool);
335  return ;
336 }
337 
348 void
350  const unsigned int &image_width,
351  const unsigned int &image_height)
352 {
353  if (toolCustom)
354  {
355  vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
356  }
357  // Set default parameters
358  switch (getToolType()) {
360  // Set default intrinsic camera parameters for 640x480 images
361  if (image_width == 640 && image_height == 480)
362  {
363  std::cout << "Get default camera parameters for camera \""
364  << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
365  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
366  }
367  else {
368  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
369  }
370  break;
371  }
373  // Set default intrinsic camera parameters for 640x480 images
374  if (image_width == 640 && image_height == 480) {
375  std::cout << "Get default camera parameters for camera \""
376  << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
377  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
378  }
379  else {
380  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
381  }
382  break;
383  }
386  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
387  break;
388  }
389  }
390  return;
391 }
392 
401 void
403  const vpImage<unsigned char> &I_)
404 {
405  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
406 }
407 
416 void
418  const vpImage<vpRGBa> &I_)
419 {
420  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
421 }
422 
423 
429 void
431 {
432  px_int = cam.get_px();
433  py_int = cam.get_py();
434  toolCustom = true;
435 }
436 
437 
441 void
443 {
444  double tcur_1 = tcur;// temporary variable used to store the last time since the last command
445 
446  while (!robotStop)
447  {
448  //Get current time
449  tprev = tcur_1;
451 
452 
454  setVelocityCalled = false;
456 
457  double ellapsedTime = (tcur - tprev) * 1e-3;
458  if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
459  ellapsedTime = getSamplingTime(); // in second
460  }
461 
462  vpColVector articularCoordinates = get_artCoord();
463  articularCoordinates = get_artCoord();
464  vpColVector articularVelocities = get_artVel();
465 
466  if (jointLimit)
467  {
468  double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
469  if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
470  if (verbose_) {
471  std::cout << "Joint " << jointLimitArt-1
472  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
473  }
474  articularVelocities = 0.0;
475  }
476  else
477  jointLimit = false;
478  }
479 
480  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
481  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
482  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
483  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
484  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
485  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
486 
487  int jl = isInJointLimit();
488 
489  if (jl != 0 && jointLimit == false)
490  {
491  if (jl < 0)
492  ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
493  else
494  ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
495 
496  for (unsigned int i = 0; i < 6; i++)
497  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
498 
499  jointLimit = true;
500  jointLimitArt = (unsigned int)fabs((double)jl);
501  }
502 
503  set_artCoord(articularCoordinates);
504  set_artVel(articularVelocities);
505 
506  compute_fMi();
507 
508  if (displayAllowed)
509  {
513  }
514 
516  {
517  while (get_displayBusy()) vpTime::wait(2);
519  set_displayBusy(false);
520  }
521 
522 
524  {
525  vpHomogeneousMatrix fMit[8];
526  get_fMi(fMit);
527 
528  //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
529 
530  vpImagePoint iP, iP_1;
531  vpPoint pt(0,0,0);
532 
535  pt.track(getExternalCameraPosition ()*fMit[0]);
538  for (int k = 1; k < 7; k++)
539  {
540  pt.track(getExternalCameraPosition ()*fMit[k-1]);
542 
543  pt.track(getExternalCameraPosition ()*fMit[k]);
545 
547  }
549  }
550 
552 
553  vpTime::wait( tcur, 1000 * getSamplingTime() );
554  tcur_1 = tcur;
555  }else{
557  }
558  }
559 }
560 
571 void
573 {
574  //vpColVector q = get_artCoord();
575  vpColVector q(6);//; = get_artCoord();
576  q = get_artCoord();
577 
578  vpHomogeneousMatrix fMit[8];
579 
580  double q1 = q[0];
581  double q2 = q[1];
582  double q3 = q[2];
583  double q4 = q[3];
584  double q5 = q[4];
585  double q6 = q[5];
586 
587  double c1 = cos(q1);
588  double s1 = sin(q1);
589  double c2 = cos(q2);
590  double s2 = sin(q2);
591  double c23 = cos(q2+q3);
592  double s23 = sin(q2+q3);
593  double c4 = cos(q4);
594  double s4 = sin(q4);
595  double c5 = cos(q5);
596  double s5 = sin(q5);
597  double c6 = cos(q6);
598  double s6 = sin(q6);
599 
600  fMit[0][0][0] = c1;
601  fMit[0][1][0] = s1;
602  fMit[0][2][0] = 0;
603  fMit[0][0][1] = 0;
604  fMit[0][1][1] = 0;
605  fMit[0][2][1] = -1;
606  fMit[0][0][2] = -s1;
607  fMit[0][1][2] = c1;
608  fMit[0][2][2] = 0;
609  fMit[0][0][3] = a1*c1;
610  fMit[0][1][3] = a1*s1;
611  fMit[0][2][3] = d1;
612 
613  fMit[1][0][0] = c1*c2;
614  fMit[1][1][0] = s1*c2;
615  fMit[1][2][0] = -s2;
616  fMit[1][0][1] = -c1*s2;
617  fMit[1][1][1] = -s1*s2;
618  fMit[1][2][1] = -c2;
619  fMit[1][0][2] = -s1;
620  fMit[1][1][2] = c1;
621  fMit[1][2][2] = 0;
622  fMit[1][0][3] = c1*(a2*c2+a1);
623  fMit[1][1][3] = s1*(a2*c2+a1);
624  fMit[1][2][3] = d1-a2*s2;
625 
626  double quickcomp1 = a3*c23-a2*c2-a1;
627 
628  fMit[2][0][0] = -c1*c23;
629  fMit[2][1][0] = -s1*c23;
630  fMit[2][2][0] = s23;
631  fMit[2][0][1] = s1;
632  fMit[2][1][1] = -c1;
633  fMit[2][2][1] = 0;
634  fMit[2][0][2] = c1*s23;
635  fMit[2][1][2] = s1*s23;
636  fMit[2][2][2] = c23;
637  fMit[2][0][3] = -c1*quickcomp1;
638  fMit[2][1][3] = -s1*quickcomp1;
639  fMit[2][2][3] = a3*s23-a2*s2+d1;
640 
641  double quickcomp2 = c1*(s23*d4 - quickcomp1);
642  double quickcomp3 = s1*(s23*d4 - quickcomp1);
643 
644  fMit[3][0][0] = -c1*c23*c4+s1*s4;
645  fMit[3][1][0] = -s1*c23*c4-c1*s4;
646  fMit[3][2][0] = s23*c4;
647  fMit[3][0][1] = c1*s23;
648  fMit[3][1][1] = s1*s23;
649  fMit[3][2][1] = c23;
650  fMit[3][0][2] = -c1*c23*s4-s1*c4;
651  fMit[3][1][2] = -s1*c23*s4+c1*c4;
652  fMit[3][2][2] = s23*s4;
653  fMit[3][0][3] = quickcomp2;
654  fMit[3][1][3] = quickcomp3;
655  fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
656 
657  fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
658  fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
659  fMit[4][2][0] = s23*c4*c5+c23*s5;
660  fMit[4][0][1] = c1*c23*s4+s1*c4;
661  fMit[4][1][1] = s1*c23*s4-c1*c4;
662  fMit[4][2][1] = -s23*s4;
663  fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
664  fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
665  fMit[4][2][2] = -s23*c4*s5+c23*c5;
666  fMit[4][0][3] = quickcomp2;
667  fMit[4][1][3] = quickcomp3;
668  fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
669 
670  fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
671  fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
672  fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
673  fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
674  fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
675  fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
676  fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
677  fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
678  fMit[5][2][2] = -s23*c4*s5+c23*c5;
679  fMit[5][0][3] = quickcomp2;
680  fMit[5][1][3] = quickcomp3;
681  fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
682 
683  fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
684  fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
685  fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
686  fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
687  fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
688  fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
689  fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
690  fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
691  fMit[6][2][2] = -s23*c4*s5+c23*c5;
692  fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
693  fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
694  fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
695 
697  get_cMe(cMe);
698  cMe = cMe.inverse();
699 // fMit[7] = fMit[6] * cMe;
700  vpViper::get_fMc(q,fMit[7]);
701 
702  #if defined(_WIN32)
703  WaitForSingleObject(mutex_fMi,INFINITE);
704  for (int i = 0; i < 8; i++)
705  fMi[i] = fMit[i];
706  ReleaseMutex(mutex_fMi);
707  #elif defined(VISP_HAVE_PTHREAD)
708  pthread_mutex_lock (&mutex_fMi);
709  for (int i = 0; i < 8; i++)
710  fMi[i] = fMit[i];
711  pthread_mutex_unlock (&mutex_fMi);
712  #endif
713 }
714 
715 
723 {
724  switch (newState) {
725  case vpRobot::STATE_STOP: {
726  // Start primitive STOP only if the current state is Velocity
728  stopMotion();
729  }
730  break;
731  }
734  std::cout << "Change the control mode from velocity to position control.\n";
735  stopMotion();
736  }
737  else {
738  //std::cout << "Change the control mode from stop to position control.\n";
739  }
740  break;
741  }
744  std::cout << "Change the control mode from stop to velocity control.\n";
745  }
746  break;
747  }
749  default:
750  break ;
751  }
752 
753  return vpRobot::setRobotState (newState);
754 }
755 
825 void
827 {
829  vpERROR_TRACE ("Cannot send a velocity to the robot "
830  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
832  "Cannot send a velocity to the robot "
833  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
834  }
835 
836  vpColVector vel_sat(6);
837 
838  double scale_trans_sat = 1;
839  double scale_rot_sat = 1;
840  double scale_sat = 1;
841  double vel_trans_max = getMaxTranslationVelocity();
842  double vel_rot_max = getMaxRotationVelocity();
843 
844  double vel_abs; // Absolute value
845 
846  // Velocity saturation
847  switch(frame)
848  {
849  // saturation in cartesian space
850  case vpRobot::CAMERA_FRAME :
852  {
853  if (vel.getRows() != 6)
854  {
855  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
856  throw;
857  }
858 
859  for (unsigned int i = 0 ; i < 3; ++ i)
860  {
861  vel_abs = fabs (vel[i]);
862  if (vel_abs > vel_trans_max && !jointLimit)
863  {
864  vel_trans_max = vel_abs;
865  vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
866  "(axis nr. %d).", vel[i], i+1);
867  }
868 
869  vel_abs = fabs (vel[i+3]);
870  if (vel_abs > vel_rot_max && !jointLimit) {
871  vel_rot_max = vel_abs;
872  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
873  "(axis nr. %d).", vel[i+3], i+4);
874  }
875  }
876 
877  if (vel_trans_max > getMaxTranslationVelocity())
878  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
879 
880  if (vel_rot_max > getMaxRotationVelocity())
881  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
882 
883  if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
884  {
885  if (scale_trans_sat < scale_rot_sat)
886  scale_sat = scale_trans_sat;
887  else
888  scale_sat = scale_rot_sat;
889  }
890  break;
891  }
892 
893  // saturation in joint space
895  {
896  if (vel.getRows() != 6)
897  {
898  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
899  throw;
900  }
901  for (unsigned int i = 0 ; i < 6; ++ i)
902  {
903  vel_abs = fabs (vel[i]);
904  if (vel_abs > vel_rot_max && !jointLimit)
905  {
906  vel_rot_max = vel_abs;
907  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
908  "(axis nr. %d).", vel[i], i+1);
909  }
910  }
911  if (vel_rot_max > getMaxRotationVelocity())
912  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
913  if ( scale_rot_sat < 1 )
914  scale_sat = scale_rot_sat;
915  break;
916  }
917  case vpRobot::MIXT_FRAME :
918  {
919  break;
920  }
921  }
922 
923  set_velocity (vel * scale_sat);
924  setRobotFrame (frame);
925  setVelocityCalled = true;
926 }
927 
928 
932 void
934 {
936 
937  double scale_rot_sat = 1;
938  double scale_sat = 1;
939  double vel_rot_max = getMaxRotationVelocity();
940  double vel_abs;
941 
942  vpColVector articularCoordinates = get_artCoord();
943  vpColVector velocityframe = get_velocity();
944  vpColVector articularVelocity;
945 
946  switch(frame)
947  {
948  case vpRobot::CAMERA_FRAME :
949  {
950  vpMatrix eJe_;
952  vpViper850::get_eJe(articularCoordinates,eJe_);
953  eJe_ = eJe_.pseudoInverse();
955  singularityTest(articularCoordinates,eJe_);
956  articularVelocity = eJe_*eVc*velocityframe;
957  set_artVel (articularVelocity);
958  break;
959  }
961  {
962  vpMatrix fJe_;
963  vpViper850::get_fJe(articularCoordinates,fJe_);
964  fJe_ = fJe_.pseudoInverse();
966  singularityTest(articularCoordinates,fJe_);
967  articularVelocity = fJe_*velocityframe;
968  set_artVel (articularVelocity);
969  break;
970  }
972  {
973  articularVelocity = velocityframe;
974  set_artVel (articularVelocity);
975  break;
976  }
977  case vpRobot::MIXT_FRAME :
978  {
979  break;
980  }
981  }
982 
983 
984 
985  switch(frame)
986  {
987  case vpRobot::CAMERA_FRAME :
989  {
990  for (unsigned int i = 0 ; i < 6; ++ i)
991  {
992  vel_abs = fabs (articularVelocity[i]);
993  if (vel_abs > vel_rot_max && !jointLimit)
994  {
995  vel_rot_max = vel_abs;
996  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
997  "(axis nr. %d).", articularVelocity[i], i+1);
998  }
999  }
1000  if (vel_rot_max > getMaxRotationVelocity())
1001  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1002  if ( scale_rot_sat < 1 )
1003  scale_sat = scale_rot_sat;
1004 
1005  set_artVel(articularVelocity * scale_sat);
1006  break;
1007  }
1009  case vpRobot::MIXT_FRAME :
1010  {
1011  break;
1012  }
1013  }
1014 }
1015 
1016 
1063 void
1065 {
1066  vel.resize(6);
1067 
1068  vpColVector articularCoordinates = get_artCoord();
1069  vpColVector articularVelocity = get_artVel();
1070 
1071  switch(frame)
1072  {
1073  case vpRobot::CAMERA_FRAME :
1074  {
1075  vpMatrix eJe_;
1077  vpViper850::get_eJe(articularCoordinates,eJe_);
1078  vel = cVe*eJe_*articularVelocity;
1079  break ;
1080  }
1081  case vpRobot::ARTICULAR_FRAME :
1082  {
1083  vel = articularVelocity;
1084  break ;
1085  }
1086  case vpRobot::REFERENCE_FRAME :
1087  {
1088  vpMatrix fJe_;
1089  vpViper850::get_fJe(articularCoordinates,fJe_);
1090  vel = fJe_*articularVelocity;
1091  break ;
1092  }
1093  case vpRobot::MIXT_FRAME :
1094  {
1095  break ;
1096  }
1097  default:
1098  {
1099  vpERROR_TRACE ("Error in spec of vpRobot. "
1100  "Case not taken in account.");
1101  return;
1102  }
1103  }
1104 }
1105 
1122 void
1124 {
1125  timestamp = vpTime::measureTimeSecond();
1126  getVelocity(frame, vel);
1127 }
1128 
1173 {
1174  vpColVector vel(6);
1175  getVelocity (frame, vel);
1176 
1177  return vel;
1178 }
1179 
1194 {
1195  timestamp = vpTime::measureTimeSecond();
1196  vpColVector vel(6);
1197  getVelocity (frame, vel);
1198 
1199  return vel;
1200 }
1201 
1202 void
1204 {
1205  double vel_rot_max = getMaxRotationVelocity();
1206  double velmax = fabs(q[0]);
1207  for (unsigned int i = 1; i < 6; i++)
1208  {
1209  if (velmax < fabs(q[i]))
1210  velmax = fabs(q[i]);
1211  }
1212 
1213  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
1214  q = q * alpha;
1215 }
1216 
1291 void
1293 {
1295  {
1296  vpERROR_TRACE ("Robot was not in position-based control\n"
1297  "Modification of the robot state");
1298  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1299  }
1300 
1301  vpColVector articularCoordinates = get_artCoord();
1302 
1303  vpColVector error(6);
1304  double errsqr = 0;
1305  switch(frame)
1306  {
1307  case vpRobot::CAMERA_FRAME :
1308  {
1309  unsigned int nbSol;
1310  vpColVector qdes(6);
1311 
1312  vpTranslationVector txyz;
1313  vpRxyzVector rxyz;
1314  for (unsigned int i=0; i < 3; i++)
1315  {
1316  txyz[i] = q[i];
1317  rxyz[i] = q[i+3];
1318  }
1319 
1320  vpRotationMatrix cRc2(rxyz);
1321  vpHomogeneousMatrix cMc2(txyz, cRc2);
1322 
1323  vpHomogeneousMatrix fMc_;
1324  vpViper850::get_fMc(articularCoordinates, fMc_);
1325 
1326  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1327 
1328  do
1329  {
1330  articularCoordinates = get_artCoord();
1331  qdes = articularCoordinates;
1332  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1333  setVelocityCalled = true;
1334  if (nbSol > 0)
1335  {
1336  error = qdes - articularCoordinates;
1337  errsqr = error.sumSquare();
1338  //findHighestPositioningSpeed(error);
1339  set_artVel(error);
1340  if (errsqr < 1e-4)
1341  {
1342  set_artCoord (qdes);
1343  error = 0;
1344  set_artVel(error);
1345  set_velocity(error);
1346  break;
1347  }
1348  }
1349  else
1350  {
1351  vpERROR_TRACE ("Positionning error.");
1353  "Position out of range.");
1354  }
1355  }while (errsqr > 1e-8 && nbSol > 0);
1356 
1357  break ;
1358  }
1359 
1361  {
1362  do
1363  {
1364  articularCoordinates = get_artCoord();
1365  error = q - articularCoordinates;
1366  errsqr = error.sumSquare();
1367  //findHighestPositioningSpeed(error);
1368  set_artVel(error);
1369  setVelocityCalled = true;
1370  if (errsqr < 1e-4)
1371  {
1372  set_artCoord (q);
1373  error = 0;
1374  set_artVel(error);
1375  set_velocity(error);
1376  break;
1377  }
1378  }while (errsqr > 1e-8);
1379  break ;
1380  }
1381 
1383  {
1384  unsigned int nbSol;
1385  vpColVector qdes(6);
1386 
1387  vpTranslationVector txyz;
1388  vpRxyzVector rxyz;
1389  for (unsigned int i=0; i < 3; i++)
1390  {
1391  txyz[i] = q[i];
1392  rxyz[i] = q[i+3];
1393  }
1394 
1395  vpRotationMatrix fRc(rxyz);
1396  vpHomogeneousMatrix fMc_(txyz, fRc);
1397 
1398  do
1399  {
1400  articularCoordinates = get_artCoord();
1401  qdes = articularCoordinates;
1402  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1403  if (nbSol > 0)
1404  {
1405  error = qdes - articularCoordinates;
1406  errsqr = error.sumSquare();
1407  //findHighestPositioningSpeed(error);
1408  set_artVel(error);
1409  setVelocityCalled = true;
1410  if (errsqr < 1e-4)
1411  {
1412  set_artCoord (qdes);
1413  error = 0;
1414  set_artVel(error);
1415  set_velocity(error);
1416  break;
1417  }
1418  }
1419  else
1420  vpERROR_TRACE ("Positionning error. Position unreachable");
1421  }while (errsqr > 1e-8 && nbSol > 0);
1422  break ;
1423  }
1424  case vpRobot::MIXT_FRAME:
1425  {
1426  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1428  "Positionning error: "
1429  "Mixt frame not implemented.");
1430  break ;
1431  }
1432  }
1433 }
1434 
1499  const double pos1,
1500  const double pos2,
1501  const double pos3,
1502  const double pos4,
1503  const double pos5,
1504  const double pos6)
1505 {
1506  try{
1507  vpColVector position(6) ;
1508  position[0] = pos1 ;
1509  position[1] = pos2 ;
1510  position[2] = pos3 ;
1511  position[3] = pos4 ;
1512  position[4] = pos5 ;
1513  position[5] = pos6 ;
1514 
1515  setPosition(frame, position) ;
1516  }
1517  catch(...)
1518  {
1519  vpERROR_TRACE("Error caught");
1520  throw ;
1521  }
1522 }
1523 
1559 void vpSimulatorViper850::setPosition(const char *filename)
1560 {
1561  vpColVector q;
1562  bool ret;
1563 
1564  ret = this->readPosFile(filename, q);
1565 
1566  if (ret == false) {
1567  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1569  "Bad position in filename.");
1570  }
1573 }
1574 
1634 void
1636 {
1637  q.resize(6);
1638 
1639  switch(frame)
1640  {
1641  case vpRobot::CAMERA_FRAME :
1642  {
1643  q = 0;
1644  break ;
1645  }
1646 
1648  {
1649  q = get_artCoord();
1650  break ;
1651  }
1652 
1654  {
1655  vpHomogeneousMatrix fMc_;
1656  vpViper::get_fMc (get_artCoord(), fMc_);
1657 
1658  vpRotationMatrix fRc;
1659  fMc_.extract(fRc);
1660  vpRxyzVector rxyz(fRc);
1661 
1662  vpTranslationVector txyz;
1663  fMc_.extract(txyz);
1664 
1665  for (unsigned int i=0; i <3; i++)
1666  {
1667  q[i] = txyz[i];
1668  q[i+3] = rxyz[i];
1669  }
1670  break ;
1671  }
1672 
1673  case vpRobot::MIXT_FRAME:
1674  {
1675  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1677  "Positionning error: "
1678  "Mixt frame not implemented.");
1679  break ;
1680  }
1681  }
1682 }
1683 
1710 void
1712 {
1713  timestamp = vpTime::measureTimeSecond();
1714  getPosition(frame, q);
1715 }
1716 
1717 
1728 void
1730  vpPoseVector &position)
1731 {
1732  vpColVector posRxyz;
1733  //recupere position en Rxyz
1734  this->getPosition(frame,posRxyz);
1735 
1736  //recupere le vecteur thetaU correspondant
1737  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));
1738 
1739  //remplit le vpPoseVector avec translation et rotation ThetaU
1740  for(unsigned int j=0;j<3;j++)
1741  {
1742  position[j]=posRxyz[j];
1743  position[j+3]=RtuVect[j];
1744  }
1745 }
1746 
1757 void
1759  vpPoseVector &position, double &timestamp)
1760 {
1761  timestamp = vpTime::measureTimeSecond();
1762  getPosition(frame, position);
1763 }
1764 
1771 void
1773 {
1774  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
1775  {
1776  vpTRACE("Joint limit vector has not a size of 6 !");
1777  return;
1778  }
1779 
1780  joint_min[0] = limitMin[0];
1781  joint_min[1] = limitMin[1];
1782  joint_min[2] = limitMin[2];
1783  joint_min[3] = limitMin[3];
1784  joint_min[4] = limitMin[4];
1785  joint_min[5] = limitMin[5];
1786 
1787  joint_max[0] = limitMax[0];
1788  joint_max[1] = limitMax[1];
1789  joint_max[2] = limitMax[2];
1790  joint_max[3] = limitMax[3];
1791  joint_max[4] = limitMax[4];
1792  joint_max[5] = limitMax[5];
1793 
1794 }
1795 
1801 bool
1803 {
1804  double q2 = q[1];
1805  double q3 = q[2];
1806  double q5 = q[4];
1807 
1808  double c2 = cos(q2);
1809  double c3 = cos(q3);
1810  double s3 = sin(q3);
1811  double c23 = cos(q2+q3);
1812  double s23 = sin(q2+q3);
1813  double s5 = sin(q5);
1814 
1815  bool cond1 = fabs(s5) < 1e-1;
1816  bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
1817  bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
1818 
1819  if(cond1)
1820  {
1821  J[3][0] = 0;
1822  J[5][0] = 0;
1823  J[3][1] = 0;
1824  J[5][1] = 0;
1825  J[3][2] = 0;
1826  J[5][2] = 0;
1827  J[3][3] = 0;
1828  J[5][3] = 0;
1829  J[3][4] = 0;
1830  J[5][4] = 0;
1831  J[3][5] = 0;
1832  J[5][5] = 0;
1833  return true;
1834  }
1835 
1836  if(cond2)
1837  {
1838  J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1839  J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1840  J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1841  return true;
1842  }
1843 
1844  if(cond3)
1845  {
1846  J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1847  J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1848  }
1849 
1850  return false;
1851 }
1852 
1856 int
1858 {
1859  int artNumb = 0;
1860  double diff = 0;
1861  double difft = 0;
1862 
1863  vpColVector articularCoordinates = get_artCoord();
1864 
1865  for (unsigned int i = 0; i < 6; i++)
1866  {
1867  if (articularCoordinates[i] <= joint_min[i])
1868  {
1869  difft = joint_min[i] - articularCoordinates[i];
1870  if (difft > diff)
1871  {
1872  diff = difft;
1873  artNumb = -(int)i-1;
1874  }
1875  }
1876  }
1877 
1878  for (unsigned int i = 0; i < 6; i++)
1879  {
1880  if (articularCoordinates[i] >= joint_max[i])
1881  {
1882  difft = articularCoordinates[i] - joint_max[i];
1883  if (difft > diff)
1884  {
1885  diff = difft;
1886  artNumb = (int)(i+1);
1887  }
1888  }
1889  }
1890 
1891  if (artNumb != 0)
1892  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
1893 
1894  return artNumb;
1895 }
1896 
1908 void
1909 vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
1910 {
1911  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1912 }
1913 
1923 void
1924 vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
1925 {
1927 }
1928 
1947 void
1949  vpColVector &displacement)
1950 {
1951  displacement.resize (6);
1952  displacement = 0;
1953  vpColVector q_cur(6);
1954 
1955  q_cur = get_artCoord();
1956 
1957  if ( ! first_time_getdis )
1958  {
1959  switch (frame)
1960  {
1961  case vpRobot::CAMERA_FRAME:
1962  {
1963  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1964  return;
1965  break ;
1966  }
1967 
1969  {
1970  displacement = q_cur - q_prev_getdis;
1971  break ;
1972  }
1973 
1975  {
1976  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1977  return;
1978  break ;
1979  }
1980 
1981  case vpRobot::MIXT_FRAME:
1982  {
1983  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1984  return;
1985  break ;
1986  }
1987  }
1988  }
1989  else
1990  {
1991  first_time_getdis = false;
1992  }
1993 
1994  // Memorize the joint position for the next call
1995  q_prev_getdis = q_cur;
1996 }
1997 
2059 bool
2061 {
2062  FILE * fd ;
2063  fd = fopen(filename, "r") ;
2064  if (fd == NULL)
2065  return false;
2066 
2067  char line[FILENAME_MAX];
2068  char dummy[FILENAME_MAX];
2069  char head[] = "R:";
2070  bool sortie = false;
2071 
2072  do {
2073  // Saut des lignes commencant par #
2074  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2075  if ( strncmp (line, "#", 1) != 0) {
2076  // La ligne n'est pas un commentaire
2077  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2078  sortie = true; // Position robot trouvee.
2079  }
2080  // else
2081  // return (false); // fin fichier sans position robot.
2082  }
2083  }
2084  else {
2085  fclose(fd) ;
2086  return (false); /* fin fichier */
2087  }
2088  }
2089  while ( sortie != true );
2090 
2091  // Lecture des positions
2092  q.resize(njoint);
2093  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2094  dummy,
2095  &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2096  if (ret != 7) {
2097  fclose(fd) ;
2098  return false;
2099  }
2100 
2101  // converts rotations from degrees into radians
2102  q.deg2rad();
2103 
2104  fclose(fd) ;
2105  return (true);
2106 }
2107 
2129 bool
2130 vpSimulatorViper850::savePosFile(const char *filename, const vpColVector &q)
2131 {
2132 
2133  FILE * fd ;
2134  fd = fopen(filename, "w") ;
2135  if (fd == NULL)
2136  return false;
2137 
2138  fprintf(fd, "\
2139 #Viper - Position - Version 1.0\n\
2140 #\n\
2141 # R: A B C D E F\n\
2142 # Joint position in degrees\n\
2143 #\n\
2144 #\n\n");
2145 
2146  // Save positions in mm and deg
2147  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
2148  vpMath::deg(q[0]),
2149  vpMath::deg(q[1]),
2150  vpMath::deg(q[2]),
2151  vpMath::deg(q[3]),
2152  vpMath::deg(q[4]),
2153  vpMath::deg(q[5]));
2154 
2155  fclose(fd) ;
2156  return (true);
2157 }
2158 
2166 void
2167 vpSimulatorViper850::move(const char *filename)
2168 {
2169  vpColVector q;
2170 
2171  try
2172  {
2173  this->readPosFile(filename, q);
2176  }
2177  catch(...)
2178  {
2179  throw;
2180  }
2181 }
2182 
2192 void
2194 {
2195  vpViper850::get_cMe(cMe) ;
2196 }
2197 
2205 void
2207 {
2208  vpHomogeneousMatrix cMe ;
2209  vpViper850::get_cMe(cMe) ;
2210 
2211  cVe.buildFrom(cMe) ;
2212 }
2213 
2223 void
2225 {
2226  try
2227  {
2229  }
2230  catch(...)
2231  {
2232  vpERROR_TRACE("catch exception ") ;
2233  throw ;
2234  }
2235 }
2236 
2247 void
2249 {
2250  try
2251  {
2252  vpColVector articularCoordinates = get_artCoord();
2253  vpViper850::get_fJe(articularCoordinates, fJe_) ;
2254  }
2255  catch(...)
2256  {
2257  vpERROR_TRACE("Error caught");
2258  throw ;
2259  }
2260 }
2261 
2265 void
2267 {
2269  return;
2270 
2271  vpColVector stop(6);
2272  stop = 0;
2273  set_artVel(stop);
2274  set_velocity(stop);
2276 }
2277 
2278 
2279 
2280 /**********************************************************************************/
2281 /**********************************************************************************/
2282 /**********************************************************************************/
2283 /**********************************************************************************/
2284 
2293 void
2295 {
2296  // set scene_dir from #define VISP_SCENE_DIR if it exists
2297  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2298  std::string scene_dir_;
2299  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2300  bool sceneDirExists = false;
2301  for(size_t i=0; i < scene_dirs.size(); i++)
2302  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2303  scene_dir_ = scene_dirs[i];
2304  sceneDirExists = true;
2305  break;
2306  }
2307  if (! sceneDirExists) {
2308  try {
2309  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2310  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2311  }
2312  catch (...) {
2313  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2314  }
2315  }
2316 
2317  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2318  if (scene_dir_.size() > FILENAME_MAX)
2319  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2320 
2321  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2322  if (full_length > FILENAME_MAX)
2323  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2324 
2325  char *name_cam = new char [full_length];
2326 
2327  strcpy(name_cam, scene_dir_.c_str());
2328  strcat(name_cam,"/camera.bnd");
2329  set_scene(name_cam,&camera,cameraFactor);
2330 
2331  if (arm_dir.size() > FILENAME_MAX)
2332  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2333  full_length = (unsigned int)arm_dir.size() + name_length;
2334  if (full_length > FILENAME_MAX)
2335  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2336 
2337  char *name_arm = new char [full_length];
2338  strcpy(name_arm, arm_dir.c_str());
2339  strcat(name_arm,"/viper850_arm1.bnd");
2340  set_scene(name_arm, robotArms, 1.0);
2341  strcpy(name_arm, arm_dir.c_str());
2342  strcat(name_arm,"/viper850_arm2.bnd");
2343  set_scene(name_arm, robotArms+1, 1.0);
2344  strcpy(name_arm, arm_dir.c_str());
2345  strcat(name_arm,"/viper850_arm3.bnd");
2346  set_scene(name_arm, robotArms+2, 1.0);
2347  strcpy(name_arm, arm_dir.c_str());
2348  strcat(name_arm,"/viper850_arm4.bnd");
2349  set_scene(name_arm, robotArms+3, 1.0);
2350  strcpy(name_arm, arm_dir.c_str());
2351  strcat(name_arm,"/viper850_arm5.bnd");
2352  set_scene(name_arm, robotArms+4, 1.0);
2353  strcpy(name_arm, arm_dir.c_str());
2354  strcat(name_arm,"/viper850_arm6.bnd");
2355  set_scene(name_arm, robotArms+5, 1.0);
2356 
2357 // set_scene("./arm2.bnd", robotArms+1, 1.0);
2358 // set_scene("./arm3.bnd", robotArms+2, 1.0);
2359 // set_scene("./arm4.bnd", robotArms+3, 1.0);
2360 // set_scene("./arm5.bnd", robotArms+4, 1.0);
2361 // set_scene("./arm6.bnd", robotArms+5, 1.0);
2362 
2363  add_rfstack(IS_BACK);
2364 
2365  add_vwstack ("start","depth", 0.0, 100.0);
2366  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
2367  add_vwstack ("start","type", PERSPECTIVE);
2368 //
2369 // sceneInitialized = true;
2370 // displayObject = true;
2371  displayCamera = true;
2372 
2373  delete [] name_cam;
2374  delete [] name_arm;
2375 }
2376 
2377 
2378 void
2380 {
2381  bool changed = false;
2382  vpHomogeneousMatrix displacement = navigation(I_,changed);
2383 
2384  //if (displacement[2][3] != 0)
2385  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2386  camMf2 = camMf2*displacement;
2387 
2388  f2Mf = camMf2.inverse()*camMf;
2389 
2390  camMf = camMf2* displacement * f2Mf;
2391 
2392  double u;
2393  double v;
2394  //if(px_ext != 1 && py_ext != 1)
2395  // we assume px_ext and py_ext > 0
2396  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
2397  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
2398  {
2399  u = (double)I_.getWidth()/(2*px_ext);
2400  v = (double)I_.getHeight()/(2*py_ext);
2401  }
2402  else
2403  {
2404  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2405  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2406  }
2407 
2408  float w44o[4][4],w44cext[4][4],x,y,z;
2409 
2410  vp2jlc_matrix(camMf.inverse(),w44cext);
2411 
2412  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2413  x = w44cext[2][0] + w44cext[3][0];
2414  y = w44cext[2][1] + w44cext[3][1];
2415  z = w44cext[2][2] + w44cext[3][2];
2416  add_vwstack ("start","vrp", x,y,z);
2417  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2418  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2419  add_vwstack ("start","window", -u, u, -v, v);
2420 
2421  vpHomogeneousMatrix fMit[8];
2422  get_fMi(fMit);
2423 
2424  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
2425  display_scene(w44o,robotArms[0],I_, curColor);
2426 
2427  vp2jlc_matrix(fMit[0],w44o);
2428  display_scene(w44o,robotArms[1],I_, curColor);
2429 
2430  vp2jlc_matrix(fMit[1],w44o);
2431  display_scene(w44o,robotArms[2],I_, curColor);
2432 
2433  vp2jlc_matrix(fMit[2],w44o);
2434  display_scene(w44o,robotArms[3],I_, curColor);
2435 
2436  vp2jlc_matrix(fMit[3],w44o);
2437  display_scene(w44o,robotArms[4],I_, curColor);
2438 
2439  vp2jlc_matrix(fMit[6],w44o);
2440  display_scene(w44o,robotArms[5],I_, curColor);
2441 
2442  if (displayCamera)
2443  {
2444  vpHomogeneousMatrix cMe;
2445  get_cMe(cMe);
2446  cMe = cMe.inverse();
2447  cMe = fMit[6] * cMe;
2448  vp2jlc_matrix(cMe,w44o);
2449  display_scene(w44o,camera, I_, camColor);
2450  }
2451 
2452  if (displayObject)
2453  {
2454  vp2jlc_matrix(fMo,w44o);
2455  display_scene(w44o,scene,I_, curColor);
2456  }
2457 }
2458 
2474 bool
2476 {
2477  vpColVector stop(6);
2478  bool status = true;
2479  stop = 0;
2480  set_artVel(stop);
2481  set_velocity(stop);
2482  vpHomogeneousMatrix fMc_;
2483  fMc_ = fMo * cMo_.inverse();
2484 
2485  vpColVector articularCoordinates = get_artCoord();
2486  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2487 
2488  if (nbSol == 0) {
2489  status = false;
2490  vpERROR_TRACE ("Positionning error. Position unreachable");
2491  }
2492 
2493  if (verbose_)
2494  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2495 
2496  set_artCoord(articularCoordinates);
2497 
2498  compute_fMi();
2499 
2500  return status;
2501 }
2502 
2515 void
2517 {
2518  vpColVector stop(6);
2519  stop = 0;
2520  set_artVel(stop);
2521  set_velocity(stop);
2522  vpHomogeneousMatrix fMit[8];
2523  get_fMi(fMit);
2524  fMo = fMit[7] * cMo_;
2525 }
2526 
2527 #elif !defined(VISP_BUILD_SHARED_LIBS)
2528 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o) has no symbols
2529 void dummy_vpSimulatorViper850() {};
2530 #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:514
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:92
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:981
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
bool singularityTest(const vpColVector q, vpMatrix &J)
double a3
for joint 3
Definition: vpViper.h:153
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:145
Error that can be emited by the vpRobot class and its derivates.
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:335
void get_fMi(vpHomogeneousMatrix *fMit)
static bool readPosFile(const char *filename, vpColVector &q)
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:95
unsigned int getWidth() const
Definition: vpImage.h:161
void get_cMe(vpHomogeneousMatrix &cMe)
double getSamplingTime() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
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 ...
#define vpERROR_TRACE
Definition: vpDebug.h:391
VISP_EXPORT double measureTimeSecond()
Definition: vpTime.cpp:225
void getExternalImage(vpImage< vpRGBa > &I)
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:224
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:250
static const vpColor none
Definition: vpColor.h:175
Initialize the position controller.
Definition: vpRobot.h:69
error that can be emited by ViSP classes.
Definition: vpException.h:73
void track(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe)
vpControlFrameType
Definition: vpRobot.h:76
vpRxyzVector erc
Definition: vpViper.h:148
double get_py() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:458
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:81
static const vpColor green
Definition: vpColor.h:166
vpHomogeneousMatrix fMo
double a2
for joint 2
Definition: vpViper.h:152
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2233
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:275
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Class that defines what is a point.
Definition: vpPoint.h:59
void deg2rad()
Definition: vpColVector.h:124
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:141
Implementation of a rotation matrix and operations on such kind of matrices.
double d1
for joint 1
Definition: vpViper.h:151
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
double a1
Definition: vpViper.h:151
void move(const char *filename)
Initialize the velocity controller.
Definition: vpRobot.h:68
vpRobotStateType
Definition: vpRobot.h:64
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:132
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
vpTranslationVector etc
Definition: vpViper.h:147
Initialize the acceleration controller.
Definition: vpRobot.h:70
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:414
vpHomogeneousMatrix f2Mf
void set_displayBusy(const bool &status)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:206
vpRowVector t() const
vpColVector joint_max
Definition: vpViper.h:159
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:87
Generic class defining intrinsic camera parameters.
vpHomogeneousMatrix getExternalCameraPosition() const
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:62
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:611
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:456
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:122
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1477
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:152
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:573
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double get_px() const
static double rad(double deg)
Definition: vpMath.h:104
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:160
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1173
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:93
double sumSquare() const
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:927
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, vpImagePoint offset=vpImagePoint(0, 0))
Definition: vpDisplay.cpp:373
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)
vpHomogeneousMatrix camMf
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:93
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:138
unsigned int getHeight() const
Definition: vpImage.h:152
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:154
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:208
double d4
for joint 4
Definition: vpViper.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1756
void findHighestPositioningSpeed(vpColVector &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_artCoord(const vpColVector &coord)
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:140
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:82
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
static const double defaultPositioningVelocity
static bool savePosFile(const char *filename, const vpColVector &q)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double getPositioningVelocity(void)
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:142
vpHomogeneousMatrix camMf2
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:82
void get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:217
double d6
for joint 6
Definition: vpViper.h:155
vpColVector joint_min
Definition: vpViper.h:160