ViSP  2.10.0
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  if (vpIoTools::checkDirectory(VISP_ROBOT_ARMS_DIR) == true) // directory exists
199  arm_dir = VISP_ROBOT_ARMS_DIR;
200  else {
201  try {
202  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
203  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
204  }
205  catch (...) {
206  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
207  }
208  }
209 
211  toolCustom = false;
212 
213  size_fMi = 8;
214  fMi = new vpHomogeneousMatrix[8];
217 
218  zeroPos.resize(njoint);
219  zeroPos = 0;
220  zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
221  reposPos.resize(njoint);
222  reposPos = 0;
223  reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
224 
225  artCoord = reposPos;
226  artVel = 0;
227 
228  q_prev_getdis.resize(njoint);
229  q_prev_getdis = 0;
230  first_time_getdis = true;
231 
232  positioningVelocity = defaultPositioningVelocity ;
233 
236 
237  // Software joint limits in radians
238  //joint_min.resize(njoint);
239  joint_min[0] = vpMath::rad(-50);
240  joint_min[1] = vpMath::rad(-135);
241  joint_min[2] = vpMath::rad(-40);
242  joint_min[3] = vpMath::rad(-190);
243  joint_min[4] = vpMath::rad(-110);
244  joint_min[5] = vpMath::rad(-184);
245  //joint_max.resize(njoint);
246  joint_max[0] = vpMath::rad(50);
247  joint_max[1] = vpMath::rad(-40);
248  joint_max[2] = vpMath::rad(215);
249  joint_max[3] = vpMath::rad(190);
250  joint_max[4] = vpMath::rad(110);
251  joint_max[5] = vpMath::rad(184);
252 }
253 
257 void
259 {
260  robotArms = NULL;
261  robotArms = new Bound_scene[6];
262  initArms();
264  cameraParam.initPersProjWithoutDistortion (558.5309599, 556.055053, 320, 240);
266  vpCameraParameters tmp;
267  getCameraParameters(tmp,640,480);
268  px_int = tmp.get_px();
269  py_int = tmp.get_py();
270  sceneInitialized = true;
271 }
272 
273 
289 void
292 {
293  this->projModel = proj_model;
294 
295  // Use here default values of the robot constant parameters.
296  switch (tool) {
298  erc[0] = vpMath::rad(0.07); // rx
299  erc[1] = vpMath::rad(2.76); // ry
300  erc[2] = vpMath::rad(-91.50); // rz
301  etc[0] = -0.0453; // tx
302  etc[1] = 0.0005; // ty
303  etc[2] = 0.0728; // tz
304 
305  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
306  break;
307  }
309  erc[0] = vpMath::rad(0.1527764261); // rx
310  erc[1] = vpMath::rad(1.285092455); // ry
311  erc[2] = vpMath::rad(-90.75777848); // rz
312  etc[0] = -0.04558630174; // tx
313  etc[1] = -0.00134326752; // ty
314  etc[2] = 0.001000828017; // tz
315 
316  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
317  break;
318  }
321  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
322  break;
323  }
324  }
325 
326  vpRotationMatrix eRc(erc);
327  this->eMc.buildFrom(etc, eRc);
328 
329  setToolType(tool);
330  return ;
331 }
332 
343 void
345  const unsigned int &image_width,
346  const unsigned int &image_height)
347 {
348  if (toolCustom)
349  {
350  vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
351  }
352  // Set default parameters
353  switch (getToolType()) {
355  // Set default intrinsic camera parameters for 640x480 images
356  if (image_width == 640 && image_height == 480)
357  {
358  std::cout << "Get default camera parameters for camera \""
359  << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\"" << std::endl;
360  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
361  }
362  else {
363  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
364  }
365  break;
366  }
368  // Set default intrinsic camera parameters for 640x480 images
369  if (image_width == 640 && image_height == 480) {
370  std::cout << "Get default camera parameters for camera \""
371  << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
372  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
373  }
374  else {
375  vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
376  }
377  break;
378  }
381  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
382  break;
383  }
384  }
385  return;
386 }
387 
396 void
398  const vpImage<unsigned char> &I_)
399 {
400  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
401 }
402 
411 void
413  const vpImage<vpRGBa> &I_)
414 {
415  getCameraParameters(cam,I_.getWidth(),I_.getHeight());
416 }
417 
418 
424 void
426 {
427  px_int = cam.get_px();
428  py_int = cam.get_py();
429  toolCustom = true;
430 }
431 
432 
436 void
438 {
439  double tcur_1 = tcur;// temporary variable used to store the last time since the last command
440 
441  while (!robotStop)
442  {
443  //Get current time
444  tprev = tcur_1;
446 
447 
449  setVelocityCalled = false;
451 
452  double ellapsedTime = (tcur - tprev) * 1e-3;
453  if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
454  ellapsedTime = getSamplingTime(); // in second
455  }
456 
457  vpColVector articularCoordinates = get_artCoord();
458  articularCoordinates = get_artCoord();
459  vpColVector articularVelocities = get_artVel();
460 
461  if (jointLimit)
462  {
463  double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
464  if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
465  if (verbose_) {
466  std::cout << "Joint " << jointLimitArt-1
467  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
468  }
469  articularVelocities = 0.0;
470  }
471  else
472  jointLimit = false;
473  }
474 
475  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime*articularVelocities[0];
476  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime*articularVelocities[1];
477  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime*articularVelocities[2];
478  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime*articularVelocities[3];
479  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime*articularVelocities[4];
480  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime*articularVelocities[5];
481 
482  int jl = isInJointLimit();
483 
484  if (jl != 0 && jointLimit == false)
485  {
486  if (jl < 0)
487  ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(unsigned int)(-jl-1)]);
488  else
489  ellapsedTime = (joint_max[(unsigned int)(jl-1)] - articularCoordinates[(unsigned int)(jl-1)])/(articularVelocities[(unsigned int)(jl-1)]);
490 
491  for (unsigned int i = 0; i < 6; i++)
492  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime*articularVelocities[i];
493 
494  jointLimit = true;
495  jointLimitArt = (unsigned int)fabs((double)jl);
496  }
497 
498  set_artCoord(articularCoordinates);
499  set_artVel(articularVelocities);
500 
501  compute_fMi();
502 
503  if (displayAllowed)
504  {
508  }
509 
511  {
512  while (get_displayBusy()) vpTime::wait(2);
514  set_displayBusy(false);
515  }
516 
517 
519  {
520  vpHomogeneousMatrix fMit[8];
521  get_fMi(fMit);
522 
523  //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
524 
525  vpImagePoint iP, iP_1;
526  vpPoint pt;
527  pt.setWorldCoordinates (0,0,0);
528 
531  pt.track(getExternalCameraPosition ()*fMit[0]);
534  for (int k = 1; k < 7; k++)
535  {
536  pt.track(getExternalCameraPosition ()*fMit[k-1]);
538 
539  pt.track(getExternalCameraPosition ()*fMit[k]);
541 
543  }
545  }
546 
548 
549  vpTime::wait( tcur, 1000 * getSamplingTime() );
550  tcur_1 = tcur;
551  }else{
553  }
554  }
555 }
556 
567 void
569 {
570  //vpColVector q = get_artCoord();
571  vpColVector q(6);//; = get_artCoord();
572  q = get_artCoord();
573 
574  vpHomogeneousMatrix fMit[8];
575 
576  double q1 = q[0];
577  double q2 = q[1];
578  double q3 = q[2];
579  double q4 = q[3];
580  double q5 = q[4];
581  double q6 = q[5];
582 
583  double c1 = cos(q1);
584  double s1 = sin(q1);
585  double c2 = cos(q2);
586  double s2 = sin(q2);
587  double c23 = cos(q2+q3);
588  double s23 = sin(q2+q3);
589  double c4 = cos(q4);
590  double s4 = sin(q4);
591  double c5 = cos(q5);
592  double s5 = sin(q5);
593  double c6 = cos(q6);
594  double s6 = sin(q6);
595 
596  fMit[0][0][0] = c1;
597  fMit[0][1][0] = s1;
598  fMit[0][2][0] = 0;
599  fMit[0][0][1] = 0;
600  fMit[0][1][1] = 0;
601  fMit[0][2][1] = -1;
602  fMit[0][0][2] = -s1;
603  fMit[0][1][2] = c1;
604  fMit[0][2][2] = 0;
605  fMit[0][0][3] = a1*c1;
606  fMit[0][1][3] = a1*s1;
607  fMit[0][2][3] = d1;
608 
609  fMit[1][0][0] = c1*c2;
610  fMit[1][1][0] = s1*c2;
611  fMit[1][2][0] = -s2;
612  fMit[1][0][1] = -c1*s2;
613  fMit[1][1][1] = -s1*s2;
614  fMit[1][2][1] = -c2;
615  fMit[1][0][2] = -s1;
616  fMit[1][1][2] = c1;
617  fMit[1][2][2] = 0;
618  fMit[1][0][3] = c1*(a2*c2+a1);
619  fMit[1][1][3] = s1*(a2*c2+a1);
620  fMit[1][2][3] = d1-a2*s2;
621 
622  double quickcomp1 = a3*c23-a2*c2-a1;
623 
624  fMit[2][0][0] = -c1*c23;
625  fMit[2][1][0] = -s1*c23;
626  fMit[2][2][0] = s23;
627  fMit[2][0][1] = s1;
628  fMit[2][1][1] = -c1;
629  fMit[2][2][1] = 0;
630  fMit[2][0][2] = c1*s23;
631  fMit[2][1][2] = s1*s23;
632  fMit[2][2][2] = c23;
633  fMit[2][0][3] = -c1*quickcomp1;
634  fMit[2][1][3] = -s1*quickcomp1;
635  fMit[2][2][3] = a3*s23-a2*s2+d1;
636 
637  double quickcomp2 = c1*(s23*d4 - quickcomp1);
638  double quickcomp3 = s1*(s23*d4 - quickcomp1);
639 
640  fMit[3][0][0] = -c1*c23*c4+s1*s4;
641  fMit[3][1][0] = -s1*c23*c4-c1*s4;
642  fMit[3][2][0] = s23*c4;
643  fMit[3][0][1] = c1*s23;
644  fMit[3][1][1] = s1*s23;
645  fMit[3][2][1] = c23;
646  fMit[3][0][2] = -c1*c23*s4-s1*c4;
647  fMit[3][1][2] = -s1*c23*s4+c1*c4;
648  fMit[3][2][2] = s23*s4;
649  fMit[3][0][3] = quickcomp2;
650  fMit[3][1][3] = quickcomp3;
651  fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
652 
653  fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
654  fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
655  fMit[4][2][0] = s23*c4*c5+c23*s5;
656  fMit[4][0][1] = c1*c23*s4+s1*c4;
657  fMit[4][1][1] = s1*c23*s4-c1*c4;
658  fMit[4][2][1] = -s23*s4;
659  fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
660  fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
661  fMit[4][2][2] = -s23*c4*s5+c23*c5;
662  fMit[4][0][3] = quickcomp2;
663  fMit[4][1][3] = quickcomp3;
664  fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
665 
666  fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
667  fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
668  fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
669  fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
670  fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
671  fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
672  fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
673  fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
674  fMit[5][2][2] = -s23*c4*s5+c23*c5;
675  fMit[5][0][3] = quickcomp2;
676  fMit[5][1][3] = quickcomp3;
677  fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
678 
679  fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
680  fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
681  fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
682  fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
683  fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
684  fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
685  fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
686  fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
687  fMit[6][2][2] = -s23*c4*s5+c23*c5;
688  fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
689  fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
690  fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
691 
693  get_cMe(cMe);
694  cMe = cMe.inverse();
695 // fMit[7] = fMit[6] * cMe;
696  vpViper::get_fMc(q,fMit[7]);
697 
698  #if defined(_WIN32)
699  WaitForSingleObject(mutex_fMi,INFINITE);
700  for (int i = 0; i < 8; i++)
701  fMi[i] = fMit[i];
702  ReleaseMutex(mutex_fMi);
703  #elif defined(VISP_HAVE_PTHREAD)
704  pthread_mutex_lock (&mutex_fMi);
705  for (int i = 0; i < 8; i++)
706  fMi[i] = fMit[i];
707  pthread_mutex_unlock (&mutex_fMi);
708  #endif
709 }
710 
711 
719 {
720  switch (newState) {
721  case vpRobot::STATE_STOP: {
722  // Start primitive STOP only if the current state is Velocity
724  stopMotion();
725  }
726  break;
727  }
730  std::cout << "Change the control mode from velocity to position control.\n";
731  stopMotion();
732  }
733  else {
734  //std::cout << "Change the control mode from stop to position control.\n";
735  }
736  break;
737  }
740  std::cout << "Change the control mode from stop to velocity control.\n";
741  }
742  break;
743  }
745  default:
746  break ;
747  }
748 
749  return vpRobot::setRobotState (newState);
750 }
751 
821 void
823 {
825  vpERROR_TRACE ("Cannot send a velocity to the robot "
826  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
828  "Cannot send a velocity to the robot "
829  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
830  }
831 
832  vpColVector vel_sat(6);
833 
834  double scale_trans_sat = 1;
835  double scale_rot_sat = 1;
836  double scale_sat = 1;
837  double vel_trans_max = getMaxTranslationVelocity();
838  double vel_rot_max = getMaxRotationVelocity();
839 
840  double vel_abs; // Absolute value
841 
842  // Velocity saturation
843  switch(frame)
844  {
845  // saturation in cartesian space
846  case vpRobot::CAMERA_FRAME :
848  {
849  if (vel.getRows() != 6)
850  {
851  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
852  throw;
853  }
854 
855  for (unsigned int i = 0 ; i < 3; ++ i)
856  {
857  vel_abs = fabs (vel[i]);
858  if (vel_abs > vel_trans_max && !jointLimit)
859  {
860  vel_trans_max = vel_abs;
861  vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
862  "(axis nr. %d).", vel[i], i+1);
863  }
864 
865  vel_abs = fabs (vel[i+3]);
866  if (vel_abs > vel_rot_max && !jointLimit) {
867  vel_rot_max = vel_abs;
868  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
869  "(axis nr. %d).", vel[i+3], i+4);
870  }
871  }
872 
873  if (vel_trans_max > getMaxTranslationVelocity())
874  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
875 
876  if (vel_rot_max > getMaxRotationVelocity())
877  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
878 
879  if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
880  {
881  if (scale_trans_sat < scale_rot_sat)
882  scale_sat = scale_trans_sat;
883  else
884  scale_sat = scale_rot_sat;
885  }
886  break;
887  }
888 
889  // saturation in joint space
891  {
892  if (vel.getRows() != 6)
893  {
894  vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
895  throw;
896  }
897  for (unsigned int i = 0 ; i < 6; ++ i)
898  {
899  vel_abs = fabs (vel[i]);
900  if (vel_abs > vel_rot_max && !jointLimit)
901  {
902  vel_rot_max = vel_abs;
903  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
904  "(axis nr. %d).", vel[i], i+1);
905  }
906  }
907  if (vel_rot_max > getMaxRotationVelocity())
908  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
909  if ( scale_rot_sat < 1 )
910  scale_sat = scale_rot_sat;
911  break;
912  }
913  case vpRobot::MIXT_FRAME :
914  {
915  break;
916  }
917  }
918 
919  set_velocity (vel * scale_sat);
920  setRobotFrame (frame);
921  setVelocityCalled = true;
922 }
923 
924 
928 void
930 {
932 
933  double scale_rot_sat = 1;
934  double scale_sat = 1;
935  double vel_rot_max = getMaxRotationVelocity();
936  double vel_abs;
937 
938  vpColVector articularCoordinates = get_artCoord();
939  vpColVector velocityframe = get_velocity();
940  vpColVector articularVelocity;
941 
942  switch(frame)
943  {
944  case vpRobot::CAMERA_FRAME :
945  {
946  vpMatrix eJe_;
948  vpViper850::get_eJe(articularCoordinates,eJe_);
949  eJe_ = eJe_.pseudoInverse();
951  singularityTest(articularCoordinates,eJe_);
952  articularVelocity = eJe_*eVc*velocityframe;
953  set_artVel (articularVelocity);
954  break;
955  }
957  {
958  vpMatrix fJe_;
959  vpViper850::get_fJe(articularCoordinates,fJe_);
960  fJe_ = fJe_.pseudoInverse();
962  singularityTest(articularCoordinates,fJe_);
963  articularVelocity = fJe_*velocityframe;
964  set_artVel (articularVelocity);
965  break;
966  }
968  {
969  articularVelocity = velocityframe;
970  set_artVel (articularVelocity);
971  break;
972  }
973  case vpRobot::MIXT_FRAME :
974  {
975  break;
976  }
977  }
978 
979 
980 
981  switch(frame)
982  {
983  case vpRobot::CAMERA_FRAME :
985  {
986  for (unsigned int i = 0 ; i < 6; ++ i)
987  {
988  vel_abs = fabs (articularVelocity[i]);
989  if (vel_abs > vel_rot_max && !jointLimit)
990  {
991  vel_rot_max = vel_abs;
992  vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
993  "(axis nr. %d).", articularVelocity[i], i+1);
994  }
995  }
996  if (vel_rot_max > getMaxRotationVelocity())
997  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
998  if ( scale_rot_sat < 1 )
999  scale_sat = scale_rot_sat;
1000 
1001  set_artVel(articularVelocity * scale_sat);
1002  break;
1003  }
1005  case vpRobot::MIXT_FRAME :
1006  {
1007  break;
1008  }
1009  }
1010 }
1011 
1012 
1059 void
1061 {
1062  vel.resize(6);
1063 
1064  vpColVector articularCoordinates = get_artCoord();
1065  vpColVector articularVelocity = get_artVel();
1066 
1067  switch(frame)
1068  {
1069  case vpRobot::CAMERA_FRAME :
1070  {
1071  vpMatrix eJe_;
1073  vpViper850::get_eJe(articularCoordinates,eJe_);
1074  vel = cVe*eJe_*articularVelocity;
1075  break ;
1076  }
1077  case vpRobot::ARTICULAR_FRAME :
1078  {
1079  vel = articularVelocity;
1080  break ;
1081  }
1082  case vpRobot::REFERENCE_FRAME :
1083  {
1084  vpMatrix fJe_;
1085  vpViper850::get_fJe(articularCoordinates,fJe_);
1086  vel = fJe_*articularVelocity;
1087  break ;
1088  }
1089  case vpRobot::MIXT_FRAME :
1090  {
1091  break ;
1092  }
1093  default:
1094  {
1095  vpERROR_TRACE ("Error in spec of vpRobot. "
1096  "Case not taken in account.");
1097  return;
1098  }
1099  }
1100 }
1101 
1118 void
1120 {
1121  timestamp = vpTime::measureTimeSecond();
1122  getVelocity(frame, vel);
1123 }
1124 
1169 {
1170  vpColVector vel(6);
1171  getVelocity (frame, vel);
1172 
1173  return vel;
1174 }
1175 
1190 {
1191  timestamp = vpTime::measureTimeSecond();
1192  vpColVector vel(6);
1193  getVelocity (frame, vel);
1194 
1195  return vel;
1196 }
1197 
1198 void
1200 {
1201  double vel_rot_max = getMaxRotationVelocity();
1202  double velmax = fabs(q[0]);
1203  for (unsigned int i = 1; i < 6; i++)
1204  {
1205  if (velmax < fabs(q[i]))
1206  velmax = fabs(q[i]);
1207  }
1208 
1209  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
1210  q = q * alpha;
1211 }
1212 
1287 void
1289 {
1291  {
1292  vpERROR_TRACE ("Robot was not in position-based control\n"
1293  "Modification of the robot state");
1294  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1295  }
1296 
1297  vpColVector articularCoordinates = get_artCoord();
1298 
1299  vpColVector error(6);
1300  double errsqr = 0;
1301  switch(frame)
1302  {
1303  case vpRobot::CAMERA_FRAME :
1304  {
1305  unsigned int nbSol;
1306  vpColVector qdes(6);
1307 
1308  vpTranslationVector txyz;
1309  vpRxyzVector rxyz;
1310  for (unsigned int i=0; i < 3; i++)
1311  {
1312  txyz[i] = q[i];
1313  rxyz[i] = q[i+3];
1314  }
1315 
1316  vpRotationMatrix cRc2(rxyz);
1317  vpHomogeneousMatrix cMc2(txyz, cRc2);
1318 
1319  vpHomogeneousMatrix fMc_;
1320  vpViper850::get_fMc(articularCoordinates, fMc_);
1321 
1322  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1323 
1324  do
1325  {
1326  articularCoordinates = get_artCoord();
1327  qdes = articularCoordinates;
1328  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1329  setVelocityCalled = true;
1330  if (nbSol > 0)
1331  {
1332  error = qdes - articularCoordinates;
1333  errsqr = error.sumSquare();
1334  //findHighestPositioningSpeed(error);
1335  set_artVel(error);
1336  if (errsqr < 1e-4)
1337  {
1338  set_artCoord (qdes);
1339  error = 0;
1340  set_artVel(error);
1341  set_velocity(error);
1342  break;
1343  }
1344  }
1345  else
1346  {
1347  vpERROR_TRACE ("Positionning error.");
1349  "Position out of range.");
1350  }
1351  }while (errsqr > 1e-8 && nbSol > 0);
1352 
1353  break ;
1354  }
1355 
1357  {
1358  do
1359  {
1360  articularCoordinates = get_artCoord();
1361  error = q - articularCoordinates;
1362  errsqr = error.sumSquare();
1363  //findHighestPositioningSpeed(error);
1364  set_artVel(error);
1365  setVelocityCalled = true;
1366  if (errsqr < 1e-4)
1367  {
1368  set_artCoord (q);
1369  error = 0;
1370  set_artVel(error);
1371  set_velocity(error);
1372  break;
1373  }
1374  }while (errsqr > 1e-8);
1375  break ;
1376  }
1377 
1379  {
1380  unsigned int nbSol;
1381  vpColVector qdes(6);
1382 
1383  vpTranslationVector txyz;
1384  vpRxyzVector rxyz;
1385  for (unsigned int i=0; i < 3; i++)
1386  {
1387  txyz[i] = q[i];
1388  rxyz[i] = q[i+3];
1389  }
1390 
1391  vpRotationMatrix fRc(rxyz);
1392  vpHomogeneousMatrix fMc_(txyz, fRc);
1393 
1394  do
1395  {
1396  articularCoordinates = get_artCoord();
1397  qdes = articularCoordinates;
1398  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1399  if (nbSol > 0)
1400  {
1401  error = qdes - articularCoordinates;
1402  errsqr = error.sumSquare();
1403  //findHighestPositioningSpeed(error);
1404  set_artVel(error);
1405  setVelocityCalled = true;
1406  if (errsqr < 1e-4)
1407  {
1408  set_artCoord (qdes);
1409  error = 0;
1410  set_artVel(error);
1411  set_velocity(error);
1412  break;
1413  }
1414  }
1415  else
1416  vpERROR_TRACE ("Positionning error. Position unreachable");
1417  }while (errsqr > 1e-8 && nbSol > 0);
1418  break ;
1419  }
1420  case vpRobot::MIXT_FRAME:
1421  {
1422  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1424  "Positionning error: "
1425  "Mixt frame not implemented.");
1426  break ;
1427  }
1428  }
1429 }
1430 
1495  const double pos1,
1496  const double pos2,
1497  const double pos3,
1498  const double pos4,
1499  const double pos5,
1500  const double pos6)
1501 {
1502  try{
1503  vpColVector position(6) ;
1504  position[0] = pos1 ;
1505  position[1] = pos2 ;
1506  position[2] = pos3 ;
1507  position[3] = pos4 ;
1508  position[4] = pos5 ;
1509  position[5] = pos6 ;
1510 
1511  setPosition(frame, position) ;
1512  }
1513  catch(...)
1514  {
1515  vpERROR_TRACE("Error caught");
1516  throw ;
1517  }
1518 }
1519 
1555 void vpSimulatorViper850::setPosition(const char *filename)
1556 {
1557  vpColVector q;
1558  bool ret;
1559 
1560  ret = this->readPosFile(filename, q);
1561 
1562  if (ret == false) {
1563  vpERROR_TRACE ("Bad position in \"%s\"", filename);
1565  "Bad position in filename.");
1566  }
1569 }
1570 
1630 void
1632 {
1633  q.resize(6);
1634 
1635  switch(frame)
1636  {
1637  case vpRobot::CAMERA_FRAME :
1638  {
1639  q = 0;
1640  break ;
1641  }
1642 
1644  {
1645  q = get_artCoord();
1646  break ;
1647  }
1648 
1650  {
1651  vpHomogeneousMatrix fMc_;
1652  vpViper::get_fMc (get_artCoord(), fMc_);
1653 
1654  vpRotationMatrix fRc;
1655  fMc_.extract(fRc);
1656  vpRxyzVector rxyz(fRc);
1657 
1658  vpTranslationVector txyz;
1659  fMc_.extract(txyz);
1660 
1661  for (unsigned int i=0; i <3; i++)
1662  {
1663  q[i] = txyz[i];
1664  q[i+3] = rxyz[i];
1665  }
1666  break ;
1667  }
1668 
1669  case vpRobot::MIXT_FRAME:
1670  {
1671  vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
1673  "Positionning error: "
1674  "Mixt frame not implemented.");
1675  break ;
1676  }
1677  }
1678 }
1679 
1706 void
1708 {
1709  timestamp = vpTime::measureTimeSecond();
1710  getPosition(frame, q);
1711 }
1712 
1713 
1724 void
1726  vpPoseVector &position)
1727 {
1728  vpColVector posRxyz;
1729  //recupere position en Rxyz
1730  this->getPosition(frame,posRxyz);
1731 
1732  //recupere le vecteur thetaU correspondant
1733  vpThetaUVector RtuVect(posRxyz[3],posRxyz[4],posRxyz[5]);
1734 
1735  //remplit le vpPoseVector avec translation et rotation ThetaU
1736  for(unsigned int j=0;j<3;j++)
1737  {
1738  position[j]=posRxyz[j];
1739  position[j+3]=RtuVect[j];
1740  }
1741 }
1742 
1753 void
1755  vpPoseVector &position, double &timestamp)
1756 {
1757  timestamp = vpTime::measureTimeSecond();
1758  getPosition(frame, position);
1759 }
1760 
1767 void
1769 {
1770  if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
1771  {
1772  vpTRACE("Joint limit vector has not a size of 6 !");
1773  return;
1774  }
1775 
1776  joint_min[0] = limitMin[0];
1777  joint_min[1] = limitMin[1];
1778  joint_min[2] = limitMin[2];
1779  joint_min[3] = limitMin[3];
1780  joint_min[4] = limitMin[4];
1781  joint_min[5] = limitMin[5];
1782 
1783  joint_max[0] = limitMax[0];
1784  joint_max[1] = limitMax[1];
1785  joint_max[2] = limitMax[2];
1786  joint_max[3] = limitMax[3];
1787  joint_max[4] = limitMax[4];
1788  joint_max[5] = limitMax[5];
1789 
1790 }
1791 
1797 bool
1799 {
1800  double q2 = q[1];
1801  double q3 = q[2];
1802  double q5 = q[4];
1803 
1804  double c2 = cos(q2);
1805  double c3 = cos(q3);
1806  double s3 = sin(q3);
1807  double c23 = cos(q2+q3);
1808  double s23 = sin(q2+q3);
1809  double s5 = sin(q5);
1810 
1811  bool cond1 = fabs(s5) < 1e-1;
1812  bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
1813  bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
1814 
1815  if(cond1)
1816  {
1817  J[3][0] = 0;
1818  J[5][0] = 0;
1819  J[3][1] = 0;
1820  J[5][1] = 0;
1821  J[3][2] = 0;
1822  J[5][2] = 0;
1823  J[3][3] = 0;
1824  J[5][3] = 0;
1825  J[3][4] = 0;
1826  J[5][4] = 0;
1827  J[3][5] = 0;
1828  J[5][5] = 0;
1829  return true;
1830  }
1831 
1832  if(cond2)
1833  {
1834  J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1835  J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1836  J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
1837  return true;
1838  }
1839 
1840  if(cond3)
1841  {
1842  J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
1843  J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
1844  }
1845 
1846  return false;
1847 }
1848 
1852 int
1854 {
1855  int artNumb = 0;
1856  double diff = 0;
1857  double difft = 0;
1858 
1859  vpColVector articularCoordinates = get_artCoord();
1860 
1861  for (unsigned int i = 0; i < 6; i++)
1862  {
1863  if (articularCoordinates[i] <= joint_min[i])
1864  {
1865  difft = joint_min[i] - articularCoordinates[i];
1866  if (difft > diff)
1867  {
1868  diff = difft;
1869  artNumb = -(int)i-1;
1870  }
1871  }
1872  }
1873 
1874  for (unsigned int i = 0; i < 6; i++)
1875  {
1876  if (articularCoordinates[i] >= joint_max[i])
1877  {
1878  difft = articularCoordinates[i] - joint_max[i];
1879  if (difft > diff)
1880  {
1881  diff = difft;
1882  artNumb = (int)(i+1);
1883  }
1884  }
1885  }
1886 
1887  if (artNumb != 0)
1888  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
1889 
1890  return artNumb;
1891 }
1892 
1904 void
1905 vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
1906 {
1907  getDisplacement(vpRobot::CAMERA_FRAME, displacement);
1908 }
1909 
1919 void
1920 vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
1921 {
1923 }
1924 
1943 void
1945  vpColVector &displacement)
1946 {
1947  displacement.resize (6);
1948  displacement = 0;
1949  vpColVector q_cur(6);
1950 
1951  q_cur = get_artCoord();
1952 
1953  if ( ! first_time_getdis )
1954  {
1955  switch (frame)
1956  {
1957  case vpRobot::CAMERA_FRAME:
1958  {
1959  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1960  return;
1961  break ;
1962  }
1963 
1965  {
1966  displacement = q_cur - q_prev_getdis;
1967  break ;
1968  }
1969 
1971  {
1972  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1973  return;
1974  break ;
1975  }
1976 
1977  case vpRobot::MIXT_FRAME:
1978  {
1979  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1980  return;
1981  break ;
1982  }
1983  }
1984  }
1985  else
1986  {
1987  first_time_getdis = false;
1988  }
1989 
1990  // Memorize the joint position for the next call
1991  q_prev_getdis = q_cur;
1992 }
1993 
2055 bool
2057 {
2058  FILE * fd ;
2059  fd = fopen(filename, "r") ;
2060  if (fd == NULL)
2061  return false;
2062 
2063  char line[FILENAME_MAX];
2064  char dummy[FILENAME_MAX];
2065  char head[] = "R:";
2066  bool sortie = false;
2067 
2068  do {
2069  // Saut des lignes commencant par #
2070  if (fgets (line, FILENAME_MAX, fd) != NULL) {
2071  if ( strncmp (line, "#", 1) != 0) {
2072  // La ligne n'est pas un commentaire
2073  if ( strncmp (line, head, sizeof(head)-1) == 0) {
2074  sortie = true; // Position robot trouvee.
2075  }
2076  // else
2077  // return (false); // fin fichier sans position robot.
2078  }
2079  }
2080  else {
2081  fclose(fd) ;
2082  return (false); /* fin fichier */
2083  }
2084  }
2085  while ( sortie != true );
2086 
2087  // Lecture des positions
2088  q.resize(njoint);
2089  int ret = sscanf(line, "%s %lf %lf %lf %lf %lf %lf",
2090  dummy,
2091  &q[0], &q[1], &q[2], &q[3], &q[4], &q[5]);
2092  if (ret != 7) {
2093  fclose(fd) ;
2094  return false;
2095  }
2096 
2097  // converts rotations from degrees into radians
2098  q.deg2rad();
2099 
2100  fclose(fd) ;
2101  return (true);
2102 }
2103 
2125 bool
2126 vpSimulatorViper850::savePosFile(const char *filename, const vpColVector &q)
2127 {
2128 
2129  FILE * fd ;
2130  fd = fopen(filename, "w") ;
2131  if (fd == NULL)
2132  return false;
2133 
2134  fprintf(fd, "\
2135 #Viper - Position - Version 1.0\n\
2136 #\n\
2137 # R: A B C D E F\n\
2138 # Joint position in degrees\n\
2139 #\n\
2140 #\n\n");
2141 
2142  // Save positions in mm and deg
2143  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
2144  vpMath::deg(q[0]),
2145  vpMath::deg(q[1]),
2146  vpMath::deg(q[2]),
2147  vpMath::deg(q[3]),
2148  vpMath::deg(q[4]),
2149  vpMath::deg(q[5]));
2150 
2151  fclose(fd) ;
2152  return (true);
2153 }
2154 
2162 void
2163 vpSimulatorViper850::move(const char *filename)
2164 {
2165  vpColVector q;
2166 
2167  try
2168  {
2169  this->readPosFile(filename, q);
2172  }
2173  catch(...)
2174  {
2175  throw;
2176  }
2177 }
2178 
2188 void
2190 {
2191  vpViper850::get_cMe(cMe) ;
2192 }
2193 
2201 void
2203 {
2204  vpHomogeneousMatrix cMe ;
2205  vpViper850::get_cMe(cMe) ;
2206 
2207  cVe.buildFrom(cMe) ;
2208 }
2209 
2219 void
2221 {
2222  try
2223  {
2225  }
2226  catch(...)
2227  {
2228  vpERROR_TRACE("catch exception ") ;
2229  throw ;
2230  }
2231 }
2232 
2243 void
2245 {
2246  try
2247  {
2248  vpColVector articularCoordinates = get_artCoord();
2249  vpViper850::get_fJe(articularCoordinates, fJe_) ;
2250  }
2251  catch(...)
2252  {
2253  vpERROR_TRACE("Error caught");
2254  throw ;
2255  }
2256 }
2257 
2261 void
2263 {
2265  return;
2266 
2267  vpColVector stop(6);
2268  stop = 0;
2269  set_artVel(stop);
2270  set_velocity(stop);
2272 }
2273 
2274 
2275 
2276 /**********************************************************************************/
2277 /**********************************************************************************/
2278 /**********************************************************************************/
2279 /**********************************************************************************/
2280 
2289 void
2291 {
2292  // set scene_dir from #define VISP_SCENE_DIR if it exists
2293  std::string scene_dir_;
2294  if (vpIoTools::checkDirectory(VISP_SCENES_DIR) == true) // directory exists
2295  scene_dir_ = VISP_SCENES_DIR;
2296  else {
2297  try {
2298  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2299  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2300  }
2301  catch (...) {
2302  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2303  }
2304  }
2305 
2306  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2307  if (scene_dir_.size() > FILENAME_MAX)
2308  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2309 
2310  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2311  if (full_length > FILENAME_MAX)
2312  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2313 
2314  char *name_cam = new char [full_length];
2315 
2316  strcpy(name_cam, scene_dir_.c_str());
2317  strcat(name_cam,"/camera.bnd");
2318  set_scene(name_cam,&camera,cameraFactor);
2319 
2320  if (arm_dir.size() > FILENAME_MAX)
2321  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2322  full_length = (unsigned int)arm_dir.size() + name_length;
2323  if (full_length > FILENAME_MAX)
2324  throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
2325 
2326  char *name_arm = new char [full_length];
2327  strcpy(name_arm, arm_dir.c_str());
2328  strcat(name_arm,"/viper850_arm1.bnd");
2329  set_scene(name_arm, robotArms, 1.0);
2330  strcpy(name_arm, arm_dir.c_str());
2331  strcat(name_arm,"/viper850_arm2.bnd");
2332  set_scene(name_arm, robotArms+1, 1.0);
2333  strcpy(name_arm, arm_dir.c_str());
2334  strcat(name_arm,"/viper850_arm3.bnd");
2335  set_scene(name_arm, robotArms+2, 1.0);
2336  strcpy(name_arm, arm_dir.c_str());
2337  strcat(name_arm,"/viper850_arm4.bnd");
2338  set_scene(name_arm, robotArms+3, 1.0);
2339  strcpy(name_arm, arm_dir.c_str());
2340  strcat(name_arm,"/viper850_arm5.bnd");
2341  set_scene(name_arm, robotArms+4, 1.0);
2342  strcpy(name_arm, arm_dir.c_str());
2343  strcat(name_arm,"/viper850_arm6.bnd");
2344  set_scene(name_arm, robotArms+5, 1.0);
2345 
2346 // set_scene("./arm2.bnd", robotArms+1, 1.0);
2347 // set_scene("./arm3.bnd", robotArms+2, 1.0);
2348 // set_scene("./arm4.bnd", robotArms+3, 1.0);
2349 // set_scene("./arm5.bnd", robotArms+4, 1.0);
2350 // set_scene("./arm6.bnd", robotArms+5, 1.0);
2351 
2352  add_rfstack(IS_BACK);
2353 
2354  add_vwstack ("start","depth", 0.0, 100.0);
2355  add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
2356  add_vwstack ("start","type", PERSPECTIVE);
2357 //
2358 // sceneInitialized = true;
2359 // displayObject = true;
2360  displayCamera = true;
2361 
2362  delete [] name_cam;
2363  delete [] name_arm;
2364 }
2365 
2366 
2367 void
2369 {
2370  bool changed = false;
2371  vpHomogeneousMatrix displacement = navigation(I_,changed);
2372 
2373  //if (displacement[2][3] != 0)
2374  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2375  camMf2 = camMf2*displacement;
2376 
2377  f2Mf = camMf2.inverse()*camMf;
2378 
2379  camMf = camMf2* displacement * f2Mf;
2380 
2381  double u;
2382  double v;
2383  //if(px_ext != 1 && py_ext != 1)
2384  // we assume px_ext and py_ext > 0
2385  if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
2386  && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
2387  {
2388  u = (double)I_.getWidth()/(2*px_ext);
2389  v = (double)I_.getHeight()/(2*py_ext);
2390  }
2391  else
2392  {
2393  u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2394  v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
2395  }
2396 
2397  float w44o[4][4],w44cext[4][4],x,y,z;
2398 
2399  vp2jlc_matrix(camMf.inverse(),w44cext);
2400 
2401  add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
2402  x = w44cext[2][0] + w44cext[3][0];
2403  y = w44cext[2][1] + w44cext[3][1];
2404  z = w44cext[2][2] + w44cext[3][2];
2405  add_vwstack ("start","vrp", x,y,z);
2406  add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
2407  add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
2408  add_vwstack ("start","window", -u, u, -v, v);
2409 
2410  vpHomogeneousMatrix fMit[8];
2411  get_fMi(fMit);
2412 
2413  vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
2414  display_scene(w44o,robotArms[0],I_, curColor);
2415 
2416  vp2jlc_matrix(fMit[0],w44o);
2417  display_scene(w44o,robotArms[1],I_, curColor);
2418 
2419  vp2jlc_matrix(fMit[1],w44o);
2420  display_scene(w44o,robotArms[2],I_, curColor);
2421 
2422  vp2jlc_matrix(fMit[2],w44o);
2423  display_scene(w44o,robotArms[3],I_, curColor);
2424 
2425  vp2jlc_matrix(fMit[3],w44o);
2426  display_scene(w44o,robotArms[4],I_, curColor);
2427 
2428  vp2jlc_matrix(fMit[6],w44o);
2429  display_scene(w44o,robotArms[5],I_, curColor);
2430 
2431  if (displayCamera)
2432  {
2433  vpHomogeneousMatrix cMe;
2434  get_cMe(cMe);
2435  cMe = cMe.inverse();
2436  cMe = fMit[6] * cMe;
2437  vp2jlc_matrix(cMe,w44o);
2438  display_scene(w44o,camera, I_, camColor);
2439  }
2440 
2441  if (displayObject)
2442  {
2443  vp2jlc_matrix(fMo,w44o);
2444  display_scene(w44o,scene,I_, curColor);
2445  }
2446 }
2447 
2463 bool
2465 {
2466  vpColVector stop(6);
2467  bool status = true;
2468  stop = 0;
2469  set_artVel(stop);
2470  set_velocity(stop);
2471  vpHomogeneousMatrix fMc_;
2472  fMc_ = fMo * cMo_.inverse();
2473 
2474  vpColVector articularCoordinates = get_artCoord();
2475  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2476 
2477  if (nbSol == 0) {
2478  status = false;
2479  vpERROR_TRACE ("Positionning error. Position unreachable");
2480  }
2481 
2482  if (verbose_)
2483  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2484 
2485  set_artCoord(articularCoordinates);
2486 
2487  compute_fMi();
2488 
2489  return status;
2490 }
2491 
2504 void
2506 {
2507  vpColVector stop(6);
2508  stop = 0;
2509  set_artVel(stop);
2510  set_velocity(stop);
2511  vpHomogeneousMatrix fMit[8];
2512  get_fMi(fMit);
2513  fMo = fMit[7] * cMo_;
2514 }
2515 
2516 #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 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