ViSP
vpSimulatorViper850.cpp
1 /****************************************************************************
2  *
3  * $Id: vpSimulatorViper850.cpp 5171 2015-01-15 15:11:54Z fspindle $
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 Viper850.
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 
43 
44 #include <visp/vpSimulatorViper850.h>
45 #include <visp/vpTime.h>
46 #include <visp/vpImagePoint.h>
47 #include <visp/vpPoint.h>
48 #include <visp/vpMeterPixelConversion.h>
49 #include <visp/vpIoTools.h>
50 #include <cmath> // std::fabs
51 #include <limits> // numeric_limits
52 #include <string>
53 #if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
54 
56 
62  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
63  zeroPos(), reposPos(), toolCustom(false), arm_dir()
64 {
65  init();
66  initDisplay();
67 
69 
70  #if defined(_WIN32)
71  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
72  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
73  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
74  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
75  mutex_display = CreateMutex(NULL,FALSE,NULL);
76 
77 
78  DWORD dwThreadIdArray;
79  hThread = CreateThread(
80  NULL, // default security attributes
81  0, // use default stack size
82  launcher, // thread function name
83  this, // argument to thread function
84  0, // use default creation flags
85  &dwThreadIdArray); // returns the thread identifier
86  #elif defined (VISP_HAVE_PTHREAD)
87  pthread_mutex_init(&mutex_fMi, NULL);
88  pthread_mutex_init(&mutex_artVel, NULL);
89  pthread_mutex_init(&mutex_artCoord, NULL);
90  pthread_mutex_init(&mutex_velocity, NULL);
91  pthread_mutex_init(&mutex_display, NULL);
92 
93  pthread_attr_init(&attr);
94  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
95 
96  pthread_create(&thread, NULL, launcher, (void *)this);
97  #endif
98 
99  compute_fMi();
100 }
101 
109  : vpRobotWireFrameSimulator(do_display),
110  q_prev_getdis(), first_time_getdis(true), positioningVelocity(defaultPositioningVelocity),
111  zeroPos(), reposPos(), toolCustom(false), arm_dir()
112 {
113  init();
114  initDisplay();
115 
117 
118  #if defined(_WIN32)
119  mutex_fMi = CreateMutex(NULL,FALSE,NULL);
120  mutex_artVel = CreateMutex(NULL,FALSE,NULL);
121  mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
122  mutex_velocity = CreateMutex(NULL,FALSE,NULL);
123  mutex_display = CreateMutex(NULL,FALSE,NULL);
124 
125 
126  DWORD dwThreadIdArray;
127  hThread = CreateThread(
128  NULL, // default security attributes
129  0, // use default stack size
130  launcher, // thread function name
131  this, // argument to thread function
132  0, // use default creation flags
133  &dwThreadIdArray); // returns the thread identifier
134  #elif defined(VISP_HAVE_PTHREAD)
135  pthread_mutex_init(&mutex_fMi, NULL);
136  pthread_mutex_init(&mutex_artVel, NULL);
137  pthread_mutex_init(&mutex_artCoord, NULL);
138  pthread_mutex_init(&mutex_velocity, NULL);
139  pthread_mutex_init(&mutex_display, NULL);
140 
141  pthread_attr_init(&attr);
142  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
143 
144  pthread_create(&thread, NULL, launcher, (void *)this);
145  #endif
146 
147  compute_fMi();
148 }
149 
154 {
155  robotStop = true;
156 
157  #if defined(_WIN32)
158  WaitForSingleObject(hThread,INFINITE);
159  CloseHandle(hThread);
160  CloseHandle(mutex_fMi);
161  CloseHandle(mutex_artVel);
162  CloseHandle(mutex_artCoord);
163  CloseHandle(mutex_velocity);
164  CloseHandle(mutex_display);
165  #elif defined(VISP_HAVE_PTHREAD)
166  pthread_attr_destroy(&attr);
167  pthread_join(thread, NULL);
168  pthread_mutex_destroy(&mutex_fMi);
169  pthread_mutex_destroy(&mutex_artVel);
170  pthread_mutex_destroy(&mutex_artCoord);
171  pthread_mutex_destroy(&mutex_velocity);
172  pthread_mutex_destroy(&mutex_display);
173  #endif
174 
175  if (robotArms != NULL)
176  {
177  // free_Bound_scene (&(camera));
178  for(int i = 0; i < 6; i++)
179  free_Bound_scene (&(robotArms[i]));
180  }
181 
182  delete[] robotArms;
183  delete[] fMi;
184 }
185 
194 void
196 {
197  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
198  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
199  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
200  bool armDirExists = false;
201  for(size_t i=0; i < arm_dirs.size(); i++)
202  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
203  arm_dir = arm_dirs[i];
204  armDirExists = true;
205  break;
206  }
207  if (! armDirExists) {
208  try {
209  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
210  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
211  }
212  catch (...) {
213  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
214  }
215  }
216 
218  toolCustom = false;
219 
220  size_fMi = 8;
221  fMi = new vpHomogeneousMatrix[8];
224 
225  zeroPos.resize(njoint);
226  zeroPos = 0;
227  zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
228  reposPos.resize(njoint);
229  reposPos = 0;
230  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
231 
232  artCoord = reposPos;
233  artVel = 0;
234 
235  q_prev_getdis.resize(njoint);
236  q_prev_getdis = 0;
237  first_time_getdis = true;
238 
239  positioningVelocity = defaultPositioningVelocity ;
240 
243 
244  // Software joint limits in radians
245  //joint_min.resize(njoint);
246  joint_min[0] = vpMath::rad(-50);
247  joint_min[1] = vpMath::rad(-135);
248  joint_min[2] = vpMath::rad(-40);
249  joint_min[3] = vpMath::rad(-190);
250  joint_min[4] = vpMath::rad(-110);
251  joint_min[5] = vpMath::rad(-184);
252  //joint_max.resize(njoint);
253  joint_max[0] = vpMath::rad(50);
254  joint_max[1] = vpMath::rad(-40);
255  joint_max[2] = vpMath::rad(215);
256  joint_max[3] = vpMath::rad(190);
257  joint_max[4] = vpMath::rad(110);
258  joint_max[5] = vpMath::rad(184);
259 }
260 
264 void
266 {
267  robotArms = NULL;
268  robotArms = new Bound_scene[6];
269  initArms();
271  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
273  vpCameraParameters tmp;
274  getCameraParameters(tmp,640,480);
275  px_int = tmp.get_px();
276  py_int = tmp.get_py();
277  sceneInitialized = true;
278 }
279 
280 
296 void
299 {
300  this->projModel = proj_model;
301 
302  // Use here default values of the robot constant parameters.
303  switch (tool) {
305  erc[0] = vpMath::rad(0.07); // rx
306  erc[1] = vpMath::rad(2.76); // ry
307  erc[2] = vpMath::rad(-91.50); // rz
308  etc[0] = -0.0453; // tx
309  etc[1] = 0.0005; // ty
310  etc[2] = 0.0728; // tz
311 
312  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
313  break;
314  }
316  erc[0] = vpMath::rad(0.1527764261); // rx
317  erc[1] = vpMath::rad(1.285092455); // ry
318  erc[2] = vpMath::rad(-90.75777848); // rz
319  etc[0] = -0.04558630174; // tx
320  etc[1] = -0.00134326752; // ty
321  etc[2] = 0.001000828017; // tz
322 
323  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
324  break;
325  }
328  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
329  break;
330  }
331  }
332 
333  vpRotationMatrix eRc(erc);
334  this->eMc.buildFrom(etc, eRc);
335 
336  setToolType(tool);
337  return ;
338 }
339 
350 void
352  const unsigned int &image_width,
353  const unsigned int &image_height)
354 {
355  if (toolCustom)
356  {
357  vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
358  }
359  // Set default parameters
360  switch (getToolType()) {
362  // Set default intrinsic camera parameters for 640x480 images
363  if (image_width == 640 && image_height == 480)
364  {
365  std::cout << "Get default camera parameters for camera \""
366  << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
367  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
368  }
369  else {
370  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
371  }
372  break;
373  }
375  // Set default intrinsic camera parameters for 640x480 images
376  if (image_width == 640 && image_height == 480) {
377  std::cout << "Get default camera parameters for camera \""
378  << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
379  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
380  }
381  else {
382  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
383  }
384  break;
385  }
388  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
389  break;
390  }
391  }
392  return;
393 }
394 
403 void
405  const vpImage<unsigned char> &I_)
406 {
407  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
408 }
409 
418 void
420  const vpImage<vpRGBa> &I_)
421 {
422  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
423 }
424 
425 
431 void
433 {
434  px_int = cam.get_px();
435  py_int = cam.get_py();
436  toolCustom = true;
437 }
438 
439 
443 void
445 {
446  double tcur_1 = tcur;// temporary variable used to store the last time since the last command
447 
448  while (!robotStop)
449  {
450  //Get current time
451  tprev = tcur_1;
453 
454 
456  setVelocityCalled = false;
458 
459  double ellapsedTime = (tcur - tprev) * 1e-3;
460  if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
461  ellapsedTime = getSamplingTime(); // in second
462  }
463 
464  vpColVector articularCoordinates = get_artCoord();
465  articularCoordinates = get_artCoord();
466  vpColVector articularVelocities = get_artVel();
467 
468  if (jointLimit)
469  {
470  double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
471  if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
472  if (verbose_) {
473  std::cout << "Joint " << jointLimitArt-1
474  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
475  }
476  articularVelocities = 0.0;
477  }
478  else
479  jointLimit = false;
480  }
481 
482  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
483  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
484  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
485  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
486  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
487  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
488 
489  int jl = isInJointLimit();
490 
491  if (jl != 0 && jointLimit == false)
492  {
493  if (jl < 0)
494  ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
495  else
496  ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
497 
498  for (unsigned int i = 0; i < 6; i++)
499  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
500 
501  jointLimit = true;
502  jointLimitArt = (unsigned int)fabs((double)jl);
503  }
504 
505  set_artCoord(articularCoordinates);
506  set_artVel(articularVelocities);
507 
508  compute_fMi();
509 
510  if (displayAllowed)
511  {
515  }
516 
518  {
519  while (get_displayBusy()) vpTime::wait(2);
521  set_displayBusy(false);
522  }
523 
524 
526  {
527  vpHomogeneousMatrix fMit[8];
528  get_fMi(fMit);
529 
530  //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
531 
532  vpImagePoint iP, iP_1;
533  vpPoint pt;
534  pt.setWorldCoordinates (0,0,0);
535 
538  pt.track(getExternalCameraPosition ()*fMit[0]);
541  for (int k = 1; k < 7; k++)
542  {
543  pt.track(getExternalCameraPosition ()*fMit[k-1]);
545 
546  pt.track(getExternalCameraPosition ()*fMit[k]);
548 
550  }
552  }
553 
555 
556  vpTime::wait( tcur, 1000 * getSamplingTime() );
557  tcur_1 = tcur;
558  }else{
560  }
561  }
562 }
563 
574 void
576 {
577  //vpColVector q = get_artCoord();
578  vpColVector q(6);//; = get_artCoord();
579  q = get_artCoord();
580 
581  vpHomogeneousMatrix fMit[8];
582 
583  double q1 = q[0];
584  double q2 = q[1];
585  double q3 = q[2];
586  double q4 = q[3];
587  double q5 = q[4];
588  double q6 = q[5];
589 
590  double c1 = cos(q1);
591  double s1 = sin(q1);
592  double c2 = cos(q2);
593  double s2 = sin(q2);
594  double c23 = cos(q2+q3);
595  double s23 = sin(q2+q3);
596  double c4 = cos(q4);
597  double s4 = sin(q4);
598  double c5 = cos(q5);
599  double s5 = sin(q5);
600  double c6 = cos(q6);
601  double s6 = sin(q6);
602 
603  fMit[0][0][0] = c1;
604  fMit[0][1][0] = s1;
605  fMit[0][2][0] = 0;
606  fMit[0][0][1] = 0;
607  fMit[0][1][1] = 0;
608  fMit[0][2][1] = -1;
609  fMit[0][0][2] = -s1;
610  fMit[0][1][2] = c1;
611  fMit[0][2][2] = 0;
612  fMit[0][0][3] = a1*c1;
613  fMit[0][1][3] = a1*s1;
614  fMit[0][2][3] = d1;
615 
616  fMit[1][0][0] = c1*c2;
617  fMit[1][1][0] = s1*c2;
618  fMit[1][2][0] = -s2;
619  fMit[1][0][1] = -c1*s2;
620  fMit[1][1][1] = -s1*s2;
621  fMit[1][2][1] = -c2;
622  fMit[1][0][2] = -s1;
623  fMit[1][1][2] = c1;
624  fMit[1][2][2] = 0;
625  fMit[1][0][3] = c1*(a2*c2+a1);
626  fMit[1][1][3] = s1*(a2*c2+a1);
627  fMit[1][2][3] = d1-a2*s2;
628 
629  double quickcomp1 = a3*c23-a2*c2-a1;
630 
631  fMit[2][0][0] = -c1*c23;
632  fMit[2][1][0] = -s1*c23;
633  fMit[2][2][0] = s23;
634  fMit[2][0][1] = s1;
635  fMit[2][1][1] = -c1;
636  fMit[2][2][1] = 0;
637  fMit[2][0][2] = c1*s23;
638  fMit[2][1][2] = s1*s23;
639  fMit[2][2][2] = c23;
640  fMit[2][0][3] = -c1*quickcomp1;
641  fMit[2][1][3] = -s1*quickcomp1;
642  fMit[2][2][3] = a3*s23-a2*s2+d1;
643 
644  double quickcomp2 = c1*(s23*d4 - quickcomp1);
645  double quickcomp3 = s1*(s23*d4 - quickcomp1);
646 
647  fMit[3][0][0] = -c1*c23*c4+s1*s4;
648  fMit[3][1][0] = -s1*c23*c4-c1*s4;
649  fMit[3][2][0] = s23*c4;
650  fMit[3][0][1] = c1*s23;
651  fMit[3][1][1] = s1*s23;
652  fMit[3][2][1] = c23;
653  fMit[3][0][2] = -c1*c23*s4-s1*c4;
654  fMit[3][1][2] = -s1*c23*s4+c1*c4;
655  fMit[3][2][2] = s23*s4;
656  fMit[3][0][3] = quickcomp2;
657  fMit[3][1][3] = quickcomp3;
658  fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
659 
660  fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
661  fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
662  fMit[4][2][0] = s23*c4*c5+c23*s5;
663  fMit[4][0][1] = c1*c23*s4+s1*c4;
664  fMit[4][1][1] = s1*c23*s4-c1*c4;
665  fMit[4][2][1] = -s23*s4;
666  fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
667  fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
668  fMit[4][2][2] = -s23*c4*s5+c23*c5;
669  fMit[4][0][3] = quickcomp2;
670  fMit[4][1][3] = quickcomp3;
671  fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
672 
673  fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
674  fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
675  fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
676  fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
677  fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
678  fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
679  fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
680  fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
681  fMit[5][2][2] = -s23*c4*s5+c23*c5;
682  fMit[5][0][3] = quickcomp2;
683  fMit[5][1][3] = quickcomp3;
684  fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
685 
686  fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
687  fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
688  fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
689  fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
690  fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
691  fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
692  fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
693  fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
694  fMit[6][2][2] = -s23*c4*s5+c23*c5;
695  fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
696  fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
697  fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
698 
700  get_cMe(cMe);
701  cMe = cMe.inverse();
702 // fMit[7] = fMit[6] * cMe;
703  vpViper::get_fMc(q,fMit[7]);
704 
705  #if defined(_WIN32)
706  WaitForSingleObject(mutex_fMi,INFINITE);
707  for (int i = 0; i < 8; i++)
708  fMi[i] = fMit[i];
709  ReleaseMutex(mutex_fMi);
710  #elif defined(VISP_HAVE_PTHREAD)
711  pthread_mutex_lock (&mutex_fMi);
712  for (int i = 0; i < 8; i++)
713  fMi[i] = fMit[i];
714  pthread_mutex_unlock (&mutex_fMi);
715  #endif
716 }
717 
718 
726 {
727  switch (newState) {
728  case vpRobot::STATE_STOP: {
729  // Start primitive STOP only if the current state is Velocity
731  stopMotion();
732  }
733  break;
734  }
737  std::cout << "Change the control mode from velocity to position control.\n";
738  stopMotion();
739  }
740  else {
741  //std::cout << "Change the control mode from stop to position control.\n";
742  }
743  break;
744  }
747  std::cout << "Change the control mode from stop to velocity control.\n";
748  }
749  break;
750  }
752  default:
753  break ;
754  }
755 
756  return vpRobot::setRobotState (newState);
757 }
758 
828 void
830 {
832  vpERROR_TRACE ("Cannot send a velocity to the robot "
833  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
835  "Cannot send a velocity to the robot "
836  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
837  }
838 
839  vpColVector vel_sat(6);
840 
841  double scale_trans_sat = 1;
842  double scale_rot_sat = 1;
843  double scale_sat = 1;
844  double vel_trans_max = getMaxTranslationVelocity();
845  double vel_rot_max = getMaxRotationVelocity();
846 
847  double vel_abs; // Absolute value
848 
849  // Velocity saturation
850  switch(frame)
851  {
852  // saturation in cartesian space
853  case vpRobot::CAMERA_FRAME :
855  {
856  if (vel.getRows() != 6)
857  {
858  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
859  throw;
860  }
861 
862  for (unsigned int i = 0 ; i < 3; ++ i)
863  {
864  vel_abs = fabs (vel[i]);
865  if (vel_abs > vel_trans_max && !jointLimit)
866  {
867  vel_trans_max = vel_abs;
868  vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
869  "(axis nr. %d).", vel[i], i+1);
870  }
871 
872  vel_abs = fabs (vel[i+3]);
873  if (vel_abs > vel_rot_max && !jointLimit) {
874  vel_rot_max = vel_abs;
875  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
876  "(axis nr. %d).", vel[i+3], i+4);
877  }
878  }
879 
880  if (vel_trans_max > getMaxTranslationVelocity())
881  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
882 
883  if (vel_rot_max > getMaxRotationVelocity())
884  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
885 
886  if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
887  {
888  if (scale_trans_sat < scale_rot_sat)
889  scale_sat = scale_trans_sat;
890  else
891  scale_sat = scale_rot_sat;
892  }
893  break;
894  }
895 
896  // saturation in joint space
898  {
899  if (vel.getRows() != 6)
900  {
901  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
902  throw;
903  }
904  for (unsigned int i = 0 ; i < 6; ++ i)
905  {
906  vel_abs = fabs (vel[i]);
907  if (vel_abs > vel_rot_max && !jointLimit)
908  {
909  vel_rot_max = vel_abs;
910  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
911  "(axis nr. %d).", vel[i], i+1);
912  }
913  }
914  if (vel_rot_max > getMaxRotationVelocity())
915  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
916  if ( scale_rot_sat < 1 )
917  scale_sat = scale_rot_sat;
918  break;
919  }
920  case vpRobot::MIXT_FRAME :
921  {
922  break;
923  }
924  }
925 
926  set_velocity (vel * scale_sat);
927  setRobotFrame (frame);
928  setVelocityCalled = true;
929 }
930 
931 
935 void
937 {
939 
940  double scale_rot_sat = 1;
941  double scale_sat = 1;
942  double vel_rot_max = getMaxRotationVelocity();
943  double vel_abs;
944 
945  vpColVector articularCoordinates = get_artCoord();
946  vpColVector velocityframe = get_velocity();
947  vpColVector articularVelocity;
948 
949  switch(frame)
950  {
951  case vpRobot::CAMERA_FRAME :
952  {
953  vpMatrix eJe_;
955  vpViper850::get_eJe(articularCoordinates,eJe_);
956  eJe_ = eJe_.pseudoInverse();
958  singularityTest(articularCoordinates,eJe_);
959  articularVelocity = eJe_*eVc*velocityframe;
960  set_artVel (articularVelocity);
961  break;
962  }
964  {
965  vpMatrix fJe_;
966  vpViper850::get_fJe(articularCoordinates,fJe_);
967  fJe_ = fJe_.pseudoInverse();
969  singularityTest(articularCoordinates,fJe_);
970  articularVelocity = fJe_*velocityframe;
971  set_artVel (articularVelocity);
972  break;
973  }
975  {
976  articularVelocity = velocityframe;
977  set_artVel (articularVelocity);
978  break;
979  }
980  case vpRobot::MIXT_FRAME :
981  {
982  break;
983  }
984  }
985 
986 
987 
988  switch(frame)
989  {
990  case vpRobot::CAMERA_FRAME :
992  {
993  for (unsigned int i = 0 ; i < 6; ++ i)
994  {
995  vel_abs = fabs (articularVelocity[i]);
996  if (vel_abs > vel_rot_max && !jointLimit)
997  {
998  vel_rot_max = vel_abs;
999  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
1000  "(axis nr. %d).", articularVelocity[i], i+1);
1001  }
1002  }
1003  if (vel_rot_max > getMaxRotationVelocity())
1004  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1005  if ( scale_rot_sat < 1 )
1006  scale_sat = scale_rot_sat;
1007 
1008  set_artVel(articularVelocity * scale_sat);
1009  break;
1010  }
1012  case vpRobot::MIXT_FRAME :
1013  {
1014  break;
1015  }
1016  }
1017 }
1018 
1019 
1066 void
1068 {
1069  vel.resize(6);
1070 
1071  vpColVector articularCoordinates = get_artCoord();
1072  vpColVector articularVelocity = get_artVel();
1073 
1074  switch(frame)
1075  {
1076  case vpRobot::CAMERA_FRAME :
1077  {
1078  vpMatrix eJe_;
1080  vpViper850::get_eJe(articularCoordinates,eJe_);
1081  vel = cVe*eJe_*articularVelocity;
1082  break ;
1083  }
1084  case vpRobot::ARTICULAR_FRAME :
1085  {
1086  vel = articularVelocity;
1087  break ;
1088  }
1089  case vpRobot::REFERENCE_FRAME :
1090  {
1091  vpMatrix fJe_;
1092  vpViper850::get_fJe(articularCoordinates,fJe_);
1093  vel = fJe_*articularVelocity;
1094  break ;
1095  }
1096  case vpRobot::MIXT_FRAME :
1097  {
1098  break ;
1099  }
1100  default:
1101  {
1102  vpERROR_TRACE ("Error in spec of vpRobot. "
1103  "Case not taken in account.");
1104  return;
1105  }
1106  }
1107 }
1108 
1125 void
1127 {
1128  timestamp = vpTime::measureTimeSecond();
1129  getVelocity(frame, vel);
1130 }
1131 
1176 {
1177  vpColVector vel(6);
1178  getVelocity (frame, vel);
1179 
1180  return vel;
1181 }
1182 
1197 {
1198  timestamp = vpTime::measureTimeSecond();
1199  vpColVector vel(6);
1200  getVelocity (frame, vel);
1201 
1202  return vel;
1203 }
1204 
1205 void
1207 {
1208  double vel_rot_max = getMaxRotationVelocity();
1209  double velmax = fabs(q[0]);
1210  for (unsigned int i = 1; i < 6; i++)
1211  {
1212  if (velmax < fabs(q[i]))
1213  velmax = fabs(q[i]);
1214  }
1215 
1216  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
1217  q = q * alpha;
1218 }
1219 
1294 void
1296 {
1298  {
1299  vpERROR_TRACE ("Robot was not in position-based control\n"
1300  "Modification of the robot state");
1301  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1302  }
1303 
1304  vpColVector articularCoordinates = get_artCoord();
1305 
1306  vpColVector error(6);
1307  double errsqr = 0;
1308  switch(frame)
1309  {
1310  case vpRobot::CAMERA_FRAME :
1311  {
1312  unsigned int nbSol;
1313  vpColVector qdes(6);
1314 
1315  vpTranslationVector txyz;
1316  vpRxyzVector rxyz;
1317  for (unsigned int i=0; i < 3; i++)
1318  {
1319  txyz[i] = q[i];
1320  rxyz[i] = q[i+3];
1321  }
1322 
1323  vpRotationMatrix cRc2(rxyz);
1324  vpHomogeneousMatrix cMc2(txyz, cRc2);
1325 
1326  vpHomogeneousMatrix fMc_;
1327  vpViper850::get_fMc(articularCoordinates, fMc_);
1328 
1329  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1330 
1331  do
1332  {
1333  articularCoordinates = get_artCoord();
1334  qdes = articularCoordinates;
1335  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1336  setVelocityCalled = true;
1337  if (nbSol > 0)
1338  {
1339  error = qdes - articularCoordinates;
1340  errsqr = error.sumSquare();
1341  //findHighestPositioningSpeed(error);
1342  set_artVel(error);
1343  if (errsqr < 1e-4)
1344  {
1345  set_artCoord (qdes);
1346  error = 0;
1347  set_artVel(error);
1348  set_velocity(error);
1349  break;
1350  }
1351  }
1352  else
1353  {
1354  vpERROR_TRACE ("Positionning error.");
1356  "Position out of range.");
1357  }
1358  }while (errsqr > 1e-8 && nbSol > 0);
1359 
1360  break ;
1361  }
1362 
1364  {
1365  do
1366  {
1367  articularCoordinates = get_artCoord();
1368  error = q - articularCoordinates;
1369  errsqr = error.sumSquare();
1370  //findHighestPositioningSpeed(error);
1371  set_artVel(error);
1372  setVelocityCalled = true;
1373  if (errsqr < 1e-4)
1374  {
1375  set_artCoord (q);
1376  error = 0;
1377  set_artVel(error);
1378  set_velocity(error);
1379  break;
1380  }
1381  }while (errsqr > 1e-8);
1382  break ;
1383  }
1384 
1386  {
1387  unsigned int nbSol;
1388  vpColVector qdes(6);
1389 
1390  vpTranslationVector txyz;
1391  vpRxyzVector rxyz;
1392  for (unsigned int i=0; i < 3; i++)
1393  {
1394  txyz[i] = q[i];
1395  rxyz[i] = q[i+3];
1396  }
1397 
1398  vpRotationMatrix fRc(rxyz);
1399  vpHomogeneousMatrix fMc_(txyz, fRc);
1400 
1401  do
1402  {
1403  articularCoordinates = get_artCoord();
1404  qdes = articularCoordinates;
1405  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1406  if (nbSol > 0)
1407  {
1408  error = qdes - articularCoordinates;
1409  errsqr = error.sumSquare();
1410  //findHighestPositioningSpeed(error);
1411  set_artVel(error);
1412  setVelocityCalled = true;
1413  if (errsqr < 1e-4)
1414  {
1415  set_artCoord (qdes);
1416  error = 0;
1417  set_artVel(error);
1418  set_velocity(error);
1419  break;
1420  }
1421  }
1422  else
1423  vpERROR_TRACE ("Positionning error. Position unreachable");
1424  }while (errsqr > 1e-8 && nbSol > 0);
1425  break ;
1426  }
1427  case vpRobot::MIXT_FRAME:
1428  {
1429  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1431  "Positionning error: "
1432  "Mixt frame not implemented.");
1433  break ;
1434  }
1435  }
1436 }
1437 
1502  const double pos1,
1503  const double pos2,
1504  const double pos3,
1505  const double pos4,
1506  const double pos5,
1507  const double pos6)
1508 {
1509  try{
1510  vpColVector position(6) ;
1511  position[0] = pos1 ;
1512  position[1] = pos2 ;
1513  position[2] = pos3 ;
1514  position[3] = pos4 ;
1515  position[4] = pos5 ;
1516  position[5] = pos6 ;
1517 
1518  setPosition(frame, position) ;
1519  }
1520  catch(...)
1521  {
1522  vpERROR_TRACE("Error caught");
1523  throw ;
1524  }
1525 }
1526 
1562 void vpSimulatorViper850::setPosition(const char *filename)
1563 {
1564  vpColVector q;
1565  bool ret;
1566 
1567  ret = this->readPosFile(filename, q);
1568 
1569  if (ret == false) {
1570  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1572  "Bad position in filename.");
1573  }
1576 }
1577 
1637 void
1639 {
1640  q.resize(6);
1641 
1642  switch(frame)
1643  {
1644  case vpRobot::CAMERA_FRAME :
1645  {
1646  q = 0;
1647  break ;
1648  }
1649 
1651  {
1652  q = get_artCoord();
1653  break ;
1654  }
1655 
1657  {
1658  vpHomogeneousMatrix fMc_;
1659  vpViper::get_fMc (get_artCoord(), fMc_);
1660 
1661  vpRotationMatrix fRc;
1662  fMc_.extract(fRc);
1663  vpRxyzVector rxyz(fRc);
1664 
1665  vpTranslationVector txyz;
1666  fMc_.extract(txyz);
1667 
1668  for (unsigned int i=0; i <3; i++)
1669  {
1670  q[i] = txyz[i];
1671  q[i+3] = rxyz[i];
1672  }
1673  break ;
1674  }
1675 
1676  case vpRobot::MIXT_FRAME:
1677  {
1678  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1680  "Positionning error: "
1681  "Mixt frame not implemented.");
1682  break ;
1683  }
1684  }
1685 }
1686 
1713 void
1715 {
1716  timestamp = vpTime::measureTimeSecond();
1717  getPosition(frame, q);
1718 }
1719 
1720 
1731 void
1733  vpPoseVector &position)
1734 {
1735  vpColVector posRxyz;
1736  //recupere position en Rxyz
1737  this->getPosition(frame,posRxyz);
1738 
1739  //recupere le vecteur thetaU correspondant
1740  vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
1741 
1742  //remplit le vpPoseVector avec translation et rotation ThetaU
1743  for(unsigned int j=0;j<3;j++)
1744  {
1745  position[j]=posRxyz[j];
1746  position[j+3]=RtuVect[j];
1747  }
1748 }
1749 
1760 void
1762  vpPoseVector &position, double &timestamp)
1763 {
1764  timestamp = vpTime::measureTimeSecond();
1765  getPosition(frame, position);
1766 }
1767 
1774 void
1776 {
1777  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
1778  {
1779  vpTRACE("Joint limit vector has not a size of 6 !");
1780  return;
1781  }
1782 
1783  joint_min[0] = limitMin[0];
1784  joint_min[1] = limitMin[1];
1785  joint_min[2] = limitMin[2];
1786  joint_min[3] = limitMin[3];
1787  joint_min[4] = limitMin[4];
1788  joint_min[5] = limitMin[5];
1789 
1790  joint_max[0] = limitMax[0];
1791  joint_max[1] = limitMax[1];
1792  joint_max[2] = limitMax[2];
1793  joint_max[3] = limitMax[3];
1794  joint_max[4] = limitMax[4];
1795  joint_max[5] = limitMax[5];
1796 
1797 }
1798 
1804 bool
1806 {
1807  double q2 = q[1];
1808  double q3 = q[2];
1809  double q5 = q[4];
1810 
1811  double c2 = cos(q2);
1812  double c3 = cos(q3);
1813  double s3 = sin(q3);
1814  double c23 = cos(q2+q3);
1815  double s23 = sin(q2+q3);
1816  double s5 = sin(q5);
1817 
1818  bool cond1 = fabs(s5) < 1e-1;
1819  bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
1820  bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
1821 
1822  if(cond1)
1823  {
1824  J[3][0] = 0;
1825  J[5][0] = 0;
1826  J[3][1] = 0;
1827  J[5][1] = 0;
1828  J[3][2] = 0;
1829  J[5][2] = 0;
1830  J[3][3] = 0;
1831  J[5][3] = 0;
1832  J[3][4] = 0;
1833  J[5][4] = 0;
1834  J[3][5] = 0;
1835  J[5][5] = 0;
1836  return true;
1837  }
1838 
1839  if(cond2)
1840  {
1841  J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1842  J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1843  J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1844  return true;
1845  }
1846 
1847  if(cond3)
1848  {
1849  J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1850  J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1851  }
1852 
1853  return false;
1854 }
1855 
1859 int
1861 {
1862  int artNumb = 0;
1863  double diff = 0;
1864  double difft = 0;
1865 
1866  vpColVector articularCoordinates = get_artCoord();
1867 
1868  for (unsigned int i = 0; i < 6; i++)
1869  {
1870  if (articularCoordinates[i] <= joint_min[i])
1871  {
1872  difft = joint_min[i] - articularCoordinates[i];
1873  if (difft > diff)
1874  {
1875  diff = difft;
1876  artNumb = -(int)i-1;
1877  }
1878  }
1879  }
1880 
1881  for (unsigned int i = 0; i < 6; i++)
1882  {
1883  if (articularCoordinates[i] >= joint_max[i])
1884  {
1885  difft = articularCoordinates[i] - joint_max[i];
1886  if (difft > diff)
1887  {
1888  diff = difft;
1889  artNumb = (int)(i+1);
1890  }
1891  }
1892  }
1893 
1894  if (artNumb != 0)
1895  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
1896 
1897  return artNumb;
1898 }
1899 
1911 void
1912 vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
1913 {
1914  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1915 }
1916 
1926 void
1927 vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
1928 {
1930 }
1931 
1950 void
1952  vpColVector &displacement)
1953 {
1954  displacement.resize (6);
1955  displacement = 0;
1956  vpColVector q_cur(6);
1957 
1958  q_cur = get_artCoord();
1959 
1960  if ( ! first_time_getdis )
1961  {
1962  switch (frame)
1963  {
1964  case vpRobot::CAMERA_FRAME:
1965  {
1966  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1967  return;
1968  break ;
1969  }
1970 
1972  {
1973  displacement = q_cur - q_prev_getdis;
1974  break ;
1975  }
1976 
1978  {
1979  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1980  return;
1981  break ;
1982  }
1983 
1984  case vpRobot::MIXT_FRAME:
1985  {
1986  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1987  return;
1988  break ;
1989  }
1990  }
1991  }
1992  else
1993  {
1994  first_time_getdis = false;
1995  }
1996 
1997  // Memorize the joint position for the next call
1998  q_prev_getdis = q_cur;
1999 }
2000 
2062 bool
2064 {
2065  FILE * fd ;
2066  fd = fopen(filename, "r") ;
2067  if (fd == NULL)
2068  return false;
2069 
2070  char line[FILENAME_MAX];
2071  char dummy[FILENAME_MAX];
2072  char head[] = "R:";
2073  bool sortie = false;
2074 
2075  do {
2076  // Saut des lignes commencant par #
2077  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2078  if ( strncmp (line, "#", 1) != 0) {
2079  // La ligne n'est pas un commentaire
2080  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2081  sortie = true; // Position robot trouvee.
2082  }
2083  // else
2084  // return (false); // fin fichier sans position robot.
2085  }
2086  }
2087  else {
2088  fclose(fd) ;
2089  return (false); /* fin fichier */
2090  }
2091  }
2092  while ( sortie != true );
2093 
2094  // Lecture des positions
2095  q.resize(njoint);
2096  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2097  dummy,
2098  &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2099  if (ret != 7) {
2100  fclose(fd) ;
2101  return false;
2102  }
2103 
2104  // converts rotations from degrees into radians
2105  q.deg2rad();
2106 
2107  fclose(fd) ;
2108  return (true);
2109 }
2110 
2132 bool
2133 vpSimulatorViper850::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 #Viper - Position - Version 1.0\n\
2143 #\n\
2144 # R: A B C D E F\n\
2145 # Joint position in degrees\n\
2146 #\n\
2147 #\n\n");
2148 
2149  // Save positions in mm and deg
2150  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
2151  vpMath::deg(q[0]),
2152  vpMath::deg(q[1]),
2153  vpMath::deg(q[2]),
2154  vpMath::deg(q[3]),
2155  vpMath::deg(q[4]),
2156  vpMath::deg(q[5]));
2157 
2158  fclose(fd) ;
2159  return (true);
2160 }
2161 
2169 void
2170 vpSimulatorViper850::move(const char *filename)
2171 {
2172  vpColVector q;
2173 
2174  try
2175  {
2176  this->readPosFile(filename, q);
2179  }
2180  catch(...)
2181  {
2182  throw;
2183  }
2184 }
2185 
2195 void
2197 {
2198  vpViper850::get_cMe(cMe) ;
2199 }
2200 
2208 void
2210 {
2211  vpHomogeneousMatrix cMe ;
2212  vpViper850::get_cMe(cMe) ;
2213 
2214  cVe.buildFrom(cMe) ;
2215 }
2216 
2226 void
2228 {
2229  try
2230  {
2232  }
2233  catch(...)
2234  {
2235  vpERROR_TRACE("catch exception ") ;
2236  throw ;
2237  }
2238 }
2239 
2250 void
2252 {
2253  try
2254  {
2255  vpColVector articularCoordinates = get_artCoord();
2256  vpViper850::get_fJe(articularCoordinates, fJe_) ;
2257  }
2258  catch(...)
2259  {
2260  vpERROR_TRACE("Error caught");
2261  throw ;
2262  }
2263 }
2264 
2268 void
2270 {
2272  return;
2273 
2274  vpColVector stop(6);
2275  stop = 0;
2276  set_artVel(stop);
2277  set_velocity(stop);
2279 }
2280 
2281 
2282 
2283 /**********************************************************************************/
2284 /**********************************************************************************/
2285 /**********************************************************************************/
2286 /**********************************************************************************/
2287 
2296 void
2298 {
2299  // set scene_dir from #define VISP_SCENE_DIR if it exists
2300  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2301  std::string scene_dir_;
2302  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2303  bool sceneDirExists = false;
2304  for(size_t i=0; i < scene_dirs.size(); i++)
2305  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2306  scene_dir_ = scene_dirs[i];
2307  sceneDirExists = true;
2308  break;
2309  }
2310  if (! sceneDirExists) {
2311  try {
2312  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2313  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2314  }
2315  catch (...) {
2316  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2317  }
2318  }
2319 
2320  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2321  if (scene_dir_.size() > FILENAME_MAX)
2322  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2323 
2324  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2325  if (full_length > FILENAME_MAX)
2326  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2327 
2328  char *name_cam = new char [full_length];
2329 
2330  strcpy(name_cam, scene_dir_.c_str());
2331  strcat(name_cam,"/camera.bnd");
2332  set_scene(name_cam,&camera,cameraFactor);
2333 
2334  if (arm_dir.size() > FILENAME_MAX)
2335  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2336  full_length = (unsigned int)arm_dir.size() + name_length;
2337  if (full_length > FILENAME_MAX)
2338  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2339 
2340  char *name_arm = new char [full_length];
2341  strcpy(name_arm, arm_dir.c_str());
2342  strcat(name_arm,"/viper850_arm1.bnd");
2343  set_scene(name_arm, robotArms, 1.0);
2344  strcpy(name_arm, arm_dir.c_str());
2345  strcat(name_arm,"/viper850_arm2.bnd");
2346  set_scene(name_arm, robotArms+1, 1.0);
2347  strcpy(name_arm, arm_dir.c_str());
2348  strcat(name_arm,"/viper850_arm3.bnd");
2349  set_scene(name_arm, robotArms+2, 1.0);
2350  strcpy(name_arm, arm_dir.c_str());
2351  strcat(name_arm,"/viper850_arm4.bnd");
2352  set_scene(name_arm, robotArms+3, 1.0);
2353  strcpy(name_arm, arm_dir.c_str());
2354  strcat(name_arm,"/viper850_arm5.bnd");
2355  set_scene(name_arm, robotArms+4, 1.0);
2356  strcpy(name_arm, arm_dir.c_str());
2357  strcat(name_arm,"/viper850_arm6.bnd");
2358  set_scene(name_arm, robotArms+5, 1.0);
2359 
2360 // set_scene("./arm2.bnd", robotArms+1, 1.0);
2361 // set_scene("./arm3.bnd", robotArms+2, 1.0);
2362 // set_scene("./arm4.bnd", robotArms+3, 1.0);
2363 // set_scene("./arm5.bnd", robotArms+4, 1.0);
2364 // set_scene("./arm6.bnd", robotArms+5, 1.0);
2365 
2366  add_rfstack(IS_BACK);
2367 
2368  add_vwstack ("start","depth", 0.0, 100.0);
2369  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
2370  add_vwstack ("start","type", PERSPECTIVE);
2371 //
2372 // sceneInitialized = true;
2373 // displayObject = true;
2374  displayCamera = true;
2375 
2376  delete [] name_cam;
2377  delete [] name_arm;
2378 }
2379 
2380 
2381 void
2383 {
2384  bool changed = false;
2385  vpHomogeneousMatrix displacement = navigation(I_,changed);
2386 
2387  //if (displacement[2][3] != 0)
2388  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2389  camMf2 = camMf2*displacement;
2390 
2391  f2Mf = camMf2.inverse()*camMf;
2392 
2393  camMf = camMf2* displacement * f2Mf;
2394 
2395  double u;
2396  double v;
2397  //if(px_ext != 1 && py_ext != 1)
2398  // we assume px_ext and py_ext > 0
2399  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
2400  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
2401  {
2402  u = (double)I_.getWidth()/(2*px_ext);
2403  v = (double)I_.getHeight()/(2*py_ext);
2404  }
2405  else
2406  {
2407  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2408  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2409  }
2410 
2411  float w44o[4][4],w44cext[4][4],x,y,z;
2412 
2413  vp2jlc_matrix(camMf.inverse(),w44cext);
2414 
2415  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2416  x = w44cext[2][0] + w44cext[3][0];
2417  y = w44cext[2][1] + w44cext[3][1];
2418  z = w44cext[2][2] + w44cext[3][2];
2419  add_vwstack ("start","vrp", x,y,z);
2420  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2421  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2422  add_vwstack ("start","window", -u, u, -v, v);
2423 
2424  vpHomogeneousMatrix fMit[8];
2425  get_fMi(fMit);
2426 
2427  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
2428  display_scene(w44o,robotArms[0],I_, curColor);
2429 
2430  vp2jlc_matrix(fMit[0],w44o);
2431  display_scene(w44o,robotArms[1],I_, curColor);
2432 
2433  vp2jlc_matrix(fMit[1],w44o);
2434  display_scene(w44o,robotArms[2],I_, curColor);
2435 
2436  vp2jlc_matrix(fMit[2],w44o);
2437  display_scene(w44o,robotArms[3],I_, curColor);
2438 
2439  vp2jlc_matrix(fMit[3],w44o);
2440  display_scene(w44o,robotArms[4],I_, curColor);
2441 
2442  vp2jlc_matrix(fMit[6],w44o);
2443  display_scene(w44o,robotArms[5],I_, curColor);
2444 
2445  if (displayCamera)
2446  {
2447  vpHomogeneousMatrix cMe;
2448  get_cMe(cMe);
2449  cMe = cMe.inverse();
2450  cMe = fMit[6] * cMe;
2451  vp2jlc_matrix(cMe,w44o);
2452  display_scene(w44o,camera, I_, camColor);
2453  }
2454 
2455  if (displayObject)
2456  {
2457  vp2jlc_matrix(fMo,w44o);
2458  display_scene(w44o,scene,I_, curColor);
2459  }
2460 }
2461 
2477 bool
2479 {
2480  vpColVector stop(6);
2481  bool status = true;
2482  stop = 0;
2483  set_artVel(stop);
2484  set_velocity(stop);
2485  vpHomogeneousMatrix fMc_;
2486  fMc_ = fMo * cMo_.inverse();
2487 
2488  vpColVector articularCoordinates = get_artCoord();
2489  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2490 
2491  if (nbSol == 0) {
2492  status = false;
2493  vpERROR_TRACE ("Positionning error. Position unreachable");
2494  }
2495 
2496  if (verbose_)
2497  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2498 
2499  set_artCoord(articularCoordinates);
2500 
2501  compute_fMi();
2502 
2503  return status;
2504 }
2505 
2518 void
2520 {
2521  vpColVector stop(6);
2522  stop = 0;
2523  set_artVel(stop);
2524  set_velocity(stop);
2525  vpHomogeneousMatrix fMit[8];
2526  get_fMi(fMit);
2527  fMo = fMit[7] * cMo_;
2528 }
2529 
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:513
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
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:985
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
bool singularityTest(const vpColVector q, vpMatrix &J)
double a3
for joint 3
Definition: vpViper.h:154
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:146
Error that can be emited by the vpRobot class and its derivates.
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:315
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:99
unsigned int getWidth() const
Definition: vpImage.h:161
void get_cMe(vpHomogeneousMatrix &cMe)
double getSamplingTime() const
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
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 ...
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:204
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:254
static const vpColor none
Definition: vpColor.h:179
Initialize the position controller.
Definition: vpRobot.h:71
error that can be emited by ViSP classes.
Definition: vpException.h:76
void track(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe)
vpControlFrameType
Definition: vpRobot.h:78
vpRxyzVector erc
Definition: vpViper.h:149
double get_py() const
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 const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:85
static int wait(double t0, double t)
Definition: vpTime.cpp:149
double sumSquare() const
Definition: vpMatrix.cpp:868
static const vpColor green
Definition: vpColor.h:170
vpHomogeneousMatrix fMo
double a2
for joint 2
Definition: vpViper.h:153
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:279
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
Class that defines what is a point.
Definition: vpPoint.h:65
void deg2rad()
Definition: vpColVector.h:192
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
The vpRotationMatrix considers the particular case of a rotation matrix.
double d1
for joint 1
Definition: vpViper.h:152
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
double a1
Definition: vpViper.h:152
void move(const char *filename)
Initialize the velocity controller.
Definition: vpRobot.h:70
vpRobotStateType
Definition: vpRobot.h:66
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:136
void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
vpTranslationVector etc
Definition: vpViper.h:148
Initialize the acceleration controller.
Definition: vpRobot.h:72
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomogeneousMatrix f2Mf
void set_displayBusy(const bool &status)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
vpRowVector t() const
Transpose of a vector.
vpColVector joint_max
Definition: vpViper.h:160
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:91
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:66
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:615
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.h:136
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:126
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1457
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1)
Definition: vpDisplay.cpp:375
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:148
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:577
void set_velocity(const vpColVector &vel)
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
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1177
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:931
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: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
void setCameraParameters(const vpCameraParameters &cam)
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:140
unsigned int getHeight() const
Definition: vpImage.h:152
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:212
double d4
for joint 4
Definition: vpViper.h:155
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Definition: vpMatrix.cpp:1932
static double measureTimeSecond()
Definition: vpTime.cpp:225
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:93
void set_artCoord(const vpColVector &coord)
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:144
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
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.
Class that consider the case of the parameterization for the rotation.
double getPositioningVelocity(void)
static double minTimeForUsleepCall
Definition: vpTime.h:82
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:143
vpHomogeneousMatrix camMf2
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:86
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 get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98
double d6
for joint 6
Definition: vpViper.h:156
vpColVector joint_min
Definition: vpViper.h:161