Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpSimulatorViper850.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Class which provides a simulator for the robot Viper850.
33  *
34  * Authors:
35  * Nicolas Melchior
36  *
37  *****************************************************************************/
38 
39 #include <visp3/robot/vpSimulatorViper850.h>
40 
41 #if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
42 
43 #include <cmath> // std::fabs
44 #include <limits> // numeric_limits
45 #include <string>
46 #include <visp3/core/vpImagePoint.h>
47 #include <visp3/core/vpIoTools.h>
48 #include <visp3/core/vpMeterPixelConversion.h>
49 #include <visp3/core/vpPoint.h>
50 #include <visp3/core/vpTime.h>
51 
52 #include "../wireframe-simulator/vpBound.h"
53 #include "../wireframe-simulator/vpRfstack.h"
54 #include "../wireframe-simulator/vpScene.h"
55 #include "../wireframe-simulator/vpVwstack.h"
56 
58 
63  : vpRobotWireFrameSimulator(), vpViper850(), q_prev_getdis(), first_time_getdis(true),
64  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
65 {
66  init();
67  initDisplay();
68 
70 
71 #if defined(_WIN32)
72 
73  DWORD dwThreadIdArray;
74  hThread = CreateThread(NULL, // default security attributes
75  0, // use default stack size
76  launcher, // thread function name
77  this, // argument to thread function
78  0, // use default creation flags
79  &dwThreadIdArray); // returns the thread identifier
80 #elif defined(VISP_HAVE_PTHREAD)
81  pthread_attr_init(&attr);
82  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
83 
84  pthread_create(&thread, NULL, launcher, (void *)this);
85 #endif
86 
87  compute_fMi();
88 }
89 
97  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
98  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
99 {
100  init();
101  initDisplay();
102 
104 
105 #if defined(_WIN32)
106  DWORD dwThreadIdArray;
107  hThread = CreateThread(NULL, // default security attributes
108  0, // use default stack size
109  launcher, // thread function name
110  this, // argument to thread function
111  0, // use default creation flags
112  &dwThreadIdArray); // returns the thread identifier
113 #elif defined(VISP_HAVE_PTHREAD)
114  pthread_attr_init(&attr);
115  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
116 
117  pthread_create(&thread, NULL, launcher, (void *)this);
118 #endif
119 
120  compute_fMi();
121 }
122 
127 {
129  robotStop = true;
131 
132 #if defined(_WIN32)
133 #if defined(WINRT_8_1)
134  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
135 #else // pure win32
136  WaitForSingleObject(hThread, INFINITE);
137 #endif
138  CloseHandle(hThread);
139 #elif defined(VISP_HAVE_PTHREAD)
140  pthread_attr_destroy(&attr);
141  pthread_join(thread, NULL);
142 #endif
143 
144  if (robotArms != NULL) {
145  // free_Bound_scene (&(camera));
146  for (int i = 0; i < 6; i++)
147  free_Bound_scene(&(robotArms[i]));
148  }
149 
150  delete[] robotArms;
151  delete[] fMi;
152 }
153 
163 {
164  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
165  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
166  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
167  bool armDirExists = false;
168  for (size_t i = 0; i < arm_dirs.size(); i++)
169  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
170  arm_dir = arm_dirs[i];
171  armDirExists = true;
172  break;
173  }
174  if (!armDirExists) {
175  try {
176  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
177  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
178  } catch (...) {
179  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
180  }
181  }
182 
184  toolCustom = false;
185 
186  size_fMi = 8;
187  fMi = new vpHomogeneousMatrix[8];
190 
191  zeroPos.resize(njoint);
192  zeroPos = 0;
193  zeroPos[1] = -M_PI / 2;
194  zeroPos[2] = M_PI;
195  reposPos.resize(njoint);
196  reposPos = 0;
197  reposPos[1] = -M_PI / 2;
198  reposPos[2] = M_PI;
199  reposPos[4] = M_PI / 2;
200 
201  artCoord = reposPos;
202  artVel = 0;
203 
204  q_prev_getdis.resize(njoint);
205  q_prev_getdis = 0;
206  first_time_getdis = true;
207 
208  positioningVelocity = defaultPositioningVelocity;
209 
212 
213  // Software joint limits in radians
214  // joint_min.resize(njoint);
215  joint_min[0] = vpMath::rad(-50);
216  joint_min[1] = vpMath::rad(-135);
217  joint_min[2] = vpMath::rad(-40);
218  joint_min[3] = vpMath::rad(-190);
219  joint_min[4] = vpMath::rad(-110);
220  joint_min[5] = vpMath::rad(-184);
221  // joint_max.resize(njoint);
222  joint_max[0] = vpMath::rad(50);
223  joint_max[1] = vpMath::rad(-40);
224  joint_max[2] = vpMath::rad(215);
225  joint_max[3] = vpMath::rad(190);
226  joint_max[4] = vpMath::rad(110);
227  joint_max[5] = vpMath::rad(184);
228 }
229 
234 {
235  robotArms = NULL;
236  robotArms = new Bound_scene[6];
237  initArms();
238  setExternalCameraPosition(vpHomogeneousMatrix(0.0, 0.5, 1.5, vpMath::rad(90), 0, 0));
239  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
241  vpCameraParameters tmp;
242  getCameraParameters(tmp, 640, 480);
243  px_int = tmp.get_px();
244  py_int = tmp.get_py();
245  sceneInitialized = true;
246 }
247 
264 {
265  this->projModel = proj_model;
266 
267  // Use here default values of the robot constant parameters.
268  switch (tool) {
270  erc[0] = vpMath::rad(0.07); // rx
271  erc[1] = vpMath::rad(2.76); // ry
272  erc[2] = vpMath::rad(-91.50); // rz
273  etc[0] = -0.0453; // tx
274  etc[1] = 0.0005; // ty
275  etc[2] = 0.0728; // tz
276 
277  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
278  break;
279  }
281  erc[0] = vpMath::rad(0.1527764261); // rx
282  erc[1] = vpMath::rad(1.285092455); // ry
283  erc[2] = vpMath::rad(-90.75777848); // rz
284  etc[0] = -0.04558630174; // tx
285  etc[1] = -0.00134326752; // ty
286  etc[2] = 0.001000828017; // tz
287 
288  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
289  break;
290  }
294  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
295  break;
296  }
297  }
298 
299  vpRotationMatrix eRc(erc);
300 
301  m_mutex_eMc.lock();
302  this->eMc.buildFrom(etc, eRc);
304 
305  setToolType(tool);
306  return;
307 }
308 
319 void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
320  const unsigned int &image_height)
321 {
322  if (toolCustom) {
323  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
324  }
325  // Set default parameters
326  switch (getToolType()) {
328  // Set default intrinsic camera parameters for 640x480 images
329  if (image_width == 640 && image_height == 480) {
330  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
331  << std::endl;
332  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
333  } else {
334  vpTRACE("Cannot get default intrinsic camera parameters for this image "
335  "resolution");
336  }
337  break;
338  }
340  // Set default intrinsic camera parameters for 640x480 images
341  if (image_width == 640 && image_height == 480) {
342  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
343  << std::endl;
344  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
345  } else {
346  vpTRACE("Cannot get default intrinsic camera parameters for this image "
347  "resolution");
348  }
349  break;
350  }
354  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
355  break;
356  }
357  }
358  return;
359 }
360 
370 {
371  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
372 }
373 
383 {
384  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
385 }
386 
393 {
394  px_int = cam.get_px();
395  py_int = cam.get_py();
396  toolCustom = true;
397 }
398 
404 {
405  double tcur_1 = tcur; // temporary variable used to store the last time
406  // since the last command
407 
408  bool stop = false;
409  bool setVelocityCalled_ = false;
410  while (!stop) {
411  // Get current time
412  tprev = tcur_1;
414 
416  setVelocityCalled_ = setVelocityCalled;
418 
419  if (setVelocityCalled_ || !constantSamplingTimeMode) {
421  setVelocityCalled = false;
424 
425  double ellapsedTime = (tcur - tprev) * 1e-3;
426  if (constantSamplingTimeMode) { // if we want a constant velocity, we
427  // force the ellapsed time to the given
428  // samplingTime
429  ellapsedTime = getSamplingTime(); // in second
430  }
431 
432  vpColVector articularCoordinates = get_artCoord();
433  vpColVector articularVelocities = get_artVel();
434 
435  if (jointLimit) {
436  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
437  if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
438  if (verbose_) {
439  std::cout << "Joint " << jointLimitArt - 1
440  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
441  << " < " << vpMath::deg(joint_max[jointLimitArt - 1]) << std::endl;
442  }
443  articularVelocities = 0.0;
444  } else
445  jointLimit = false;
446  }
447 
448  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
449  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
450  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
451  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
452  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
453  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
454 
455  int jl = isInJointLimit();
456 
457  if (jl != 0 && jointLimit == false) {
458  if (jl < 0)
459  ellapsedTime = (joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
460  (articularVelocities[(unsigned int)(-jl - 1)]);
461  else
462  ellapsedTime = (joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
463  (articularVelocities[(unsigned int)(jl - 1)]);
464 
465  for (unsigned int i = 0; i < 6; i++)
466  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
467 
468  jointLimit = true;
469  jointLimitArt = (unsigned int)fabs((double)jl);
470  }
471 
472  set_artCoord(articularCoordinates);
473  set_artVel(articularVelocities);
474 
475  compute_fMi();
476 
477  if (displayAllowed) {
481  }
482 
483  if (displayType == MODEL_3D && displayAllowed) {
484  while (get_displayBusy()) {
485  vpTime::wait(2);
486  }
488  set_displayBusy(false);
489  }
490 
491  if (displayType == MODEL_DH && displayAllowed) {
492  vpHomogeneousMatrix fMit[8];
493  get_fMi(fMit);
494 
495  // vpDisplay::displayFrame(I,getExternalCameraPosition
496  // ()*fMi[6],cameraParam,0.2,vpColor::none);
497 
498  vpImagePoint iP, iP_1;
499  vpPoint pt(0, 0, 0);
500 
503  pt.track(getExternalCameraPosition() * fMit[0]);
506  for (int k = 1; k < 7; k++) {
507  pt.track(getExternalCameraPosition() * fMit[k - 1]);
509 
510  pt.track(getExternalCameraPosition() * fMit[k]);
512 
514  }
516  thickness_);
517  }
518 
520 
521  vpTime::wait(tcur, 1000 * getSamplingTime());
522  tcur_1 = tcur;
523  } else {
525  }
527  stop = robotStop;
529  }
530 }
531 
545 {
546  // vpColVector q = get_artCoord();
547  vpColVector q(6); //; = get_artCoord();
548  q = get_artCoord();
549 
550  vpHomogeneousMatrix fMit[8];
551 
552  double q1 = q[0];
553  double q2 = q[1];
554  double q3 = q[2];
555  double q4 = q[3];
556  double q5 = q[4];
557  double q6 = q[5];
558 
559  double c1 = cos(q1);
560  double s1 = sin(q1);
561  double c2 = cos(q2);
562  double s2 = sin(q2);
563  double c23 = cos(q2 + q3);
564  double s23 = sin(q2 + q3);
565  double c4 = cos(q4);
566  double s4 = sin(q4);
567  double c5 = cos(q5);
568  double s5 = sin(q5);
569  double c6 = cos(q6);
570  double s6 = sin(q6);
571 
572  fMit[0][0][0] = c1;
573  fMit[0][1][0] = s1;
574  fMit[0][2][0] = 0;
575  fMit[0][0][1] = 0;
576  fMit[0][1][1] = 0;
577  fMit[0][2][1] = -1;
578  fMit[0][0][2] = -s1;
579  fMit[0][1][2] = c1;
580  fMit[0][2][2] = 0;
581  fMit[0][0][3] = a1 * c1;
582  fMit[0][1][3] = a1 * s1;
583  fMit[0][2][3] = d1;
584 
585  fMit[1][0][0] = c1 * c2;
586  fMit[1][1][0] = s1 * c2;
587  fMit[1][2][0] = -s2;
588  fMit[1][0][1] = -c1 * s2;
589  fMit[1][1][1] = -s1 * s2;
590  fMit[1][2][1] = -c2;
591  fMit[1][0][2] = -s1;
592  fMit[1][1][2] = c1;
593  fMit[1][2][2] = 0;
594  fMit[1][0][3] = c1 * (a2 * c2 + a1);
595  fMit[1][1][3] = s1 * (a2 * c2 + a1);
596  fMit[1][2][3] = d1 - a2 * s2;
597 
598  double quickcomp1 = a3 * c23 - a2 * c2 - a1;
599 
600  fMit[2][0][0] = -c1 * c23;
601  fMit[2][1][0] = -s1 * c23;
602  fMit[2][2][0] = s23;
603  fMit[2][0][1] = s1;
604  fMit[2][1][1] = -c1;
605  fMit[2][2][1] = 0;
606  fMit[2][0][2] = c1 * s23;
607  fMit[2][1][2] = s1 * s23;
608  fMit[2][2][2] = c23;
609  fMit[2][0][3] = -c1 * quickcomp1;
610  fMit[2][1][3] = -s1 * quickcomp1;
611  fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
612 
613  double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
614  double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
615 
616  fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
617  fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
618  fMit[3][2][0] = s23 * c4;
619  fMit[3][0][1] = c1 * s23;
620  fMit[3][1][1] = s1 * s23;
621  fMit[3][2][1] = c23;
622  fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
623  fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
624  fMit[3][2][2] = s23 * s4;
625  fMit[3][0][3] = quickcomp2;
626  fMit[3][1][3] = quickcomp3;
627  fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
628 
629  fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
630  fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
631  fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
632  fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
633  fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
634  fMit[4][2][1] = -s23 * s4;
635  fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
636  fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
637  fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
638  fMit[4][0][3] = quickcomp2;
639  fMit[4][1][3] = quickcomp3;
640  fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
641 
642  fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
643  fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
644  fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
645  fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
646  fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
647  fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
648  fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
649  fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
650  fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
651  fMit[5][0][3] = quickcomp2;
652  fMit[5][1][3] = quickcomp3;
653  fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
654 
655  fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
656  fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
657  fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
658  fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
659  fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
660  fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
661  fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
662  fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
663  fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
664  fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
665  fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
666  fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
667 
668  // vpHomogeneousMatrix cMe;
669  // get_cMe(cMe);
670  // cMe = cMe.inverse();
671  // fMit[7] = fMit[6] * cMe;
672  m_mutex_eMc.lock();
673  vpViper::get_fMc(q, fMit[7]);
675 
676  m_mutex_fMi.lock();
677  for (int i = 0; i < 8; i++) {
678  fMi[i] = fMit[i];
679  }
681 }
682 
689 {
690  switch (newState) {
691  case vpRobot::STATE_STOP: {
692  // Start primitive STOP only if the current state is Velocity
694  stopMotion();
695  }
696  break;
697  }
700  std::cout << "Change the control mode from velocity to position control.\n";
701  stopMotion();
702  } else {
703  // std::cout << "Change the control mode from stop to position
704  // control.\n";
705  }
706  break;
707  }
710  std::cout << "Change the control mode from stop to velocity control.\n";
711  }
712  break;
713  }
715  default:
716  break;
717  }
718 
719  return vpRobot::setRobotState(newState);
720 }
721 
796 {
798  vpERROR_TRACE("Cannot send a velocity to the robot "
799  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
801  "Cannot send a velocity to the robot "
802  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
803  }
804 
805  vpColVector vel_sat(6);
806 
807  double scale_sat = 1;
808  double vel_trans_max = getMaxTranslationVelocity();
809  double vel_rot_max = getMaxRotationVelocity();
810 
811  double vel_abs; // Absolute value
812 
813  // Velocity saturation
814  switch (frame) {
815  // saturation in cartesian space
818  if (vel.getRows() != 6) {
819  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
820  throw;
821  }
822 
823  for (unsigned int i = 0; i < 3; ++i) {
824  vel_abs = fabs(vel[i]);
825  if (vel_abs > vel_trans_max && !jointLimit) {
826  vel_trans_max = vel_abs;
827  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
828  "(axis nr. %d).",
829  vel[i], i + 1);
830  }
831 
832  vel_abs = fabs(vel[i + 3]);
833  if (vel_abs > vel_rot_max && !jointLimit) {
834  vel_rot_max = vel_abs;
835  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
836  "(axis nr. %d).",
837  vel[i + 3], i + 4);
838  }
839  }
840 
841  double scale_trans_sat = 1;
842  double scale_rot_sat = 1;
843  if (vel_trans_max > getMaxTranslationVelocity())
844  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
845 
846  if (vel_rot_max > getMaxRotationVelocity())
847  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
848 
849  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
850  if (scale_trans_sat < scale_rot_sat)
851  scale_sat = scale_trans_sat;
852  else
853  scale_sat = scale_rot_sat;
854  }
855  break;
856  }
857 
858  // saturation in joint space
860  if (vel.getRows() != 6) {
861  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
862  throw;
863  }
864  for (unsigned int i = 0; i < 6; ++i) {
865  vel_abs = fabs(vel[i]);
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).",
870  vel[i], i + 1);
871  }
872  }
873  double scale_rot_sat = 1;
874  if (vel_rot_max > getMaxRotationVelocity())
875  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
876  if (scale_rot_sat < 1)
877  scale_sat = scale_rot_sat;
878  break;
879  }
881  case vpRobot::MIXT_FRAME: {
882  break;
883  }
884  }
885 
886  set_velocity(vel * scale_sat);
887 
889  setRobotFrame(frame);
891 
893  setVelocityCalled = true;
895 }
896 
901 {
903 
905  frame = getRobotFrame();
907 
908  double vel_rot_max = getMaxRotationVelocity();
909 
910  vpColVector articularCoordinates = get_artCoord();
911  vpColVector velocityframe = get_velocity();
912  vpColVector articularVelocity;
913 
914  switch (frame) {
915  case vpRobot::CAMERA_FRAME: {
916  vpMatrix eJe_;
918  vpViper850::get_eJe(articularCoordinates, eJe_);
919  eJe_ = eJe_.pseudoInverse();
921  singularityTest(articularCoordinates, eJe_);
922  articularVelocity = eJe_ * eVc * velocityframe;
923  set_artVel(articularVelocity);
924  break;
925  }
927  vpMatrix fJe_;
928  vpViper850::get_fJe(articularCoordinates, fJe_);
929  fJe_ = fJe_.pseudoInverse();
931  singularityTest(articularCoordinates, fJe_);
932  articularVelocity = fJe_ * velocityframe;
933  set_artVel(articularVelocity);
934  break;
935  }
937  articularVelocity = velocityframe;
938  set_artVel(articularVelocity);
939  break;
940  }
942  case vpRobot::MIXT_FRAME: {
943  break;
944  }
945  }
946 
947  switch (frame) {
950  for (unsigned int i = 0; i < 6; ++i) {
951  double vel_abs = fabs(articularVelocity[i]);
952  if (vel_abs > vel_rot_max && !jointLimit) {
953  vel_rot_max = vel_abs;
954  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
955  "(axis nr. %d).",
956  articularVelocity[i], i + 1);
957  }
958  }
959  double scale_rot_sat = 1;
960  double scale_sat = 1;
961 
962  if (vel_rot_max > getMaxRotationVelocity())
963  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
964  if (scale_rot_sat < 1)
965  scale_sat = scale_rot_sat;
966 
967  set_artVel(articularVelocity * scale_sat);
968  break;
969  }
972  case vpRobot::MIXT_FRAME: {
973  break;
974  }
975  }
976 }
977 
1024 {
1025  vel.resize(6);
1026 
1027  vpColVector articularCoordinates = get_artCoord();
1028  vpColVector articularVelocity = get_artVel();
1029 
1030  switch (frame) {
1031  case vpRobot::CAMERA_FRAME: {
1032  vpMatrix eJe_;
1034  vpViper850::get_eJe(articularCoordinates, eJe_);
1035  vel = cVe * eJe_ * articularVelocity;
1036  break;
1037  }
1038  case vpRobot::ARTICULAR_FRAME: {
1039  vel = articularVelocity;
1040  break;
1041  }
1042  case vpRobot::REFERENCE_FRAME: {
1043  vpMatrix fJe_;
1044  vpViper850::get_fJe(articularCoordinates, fJe_);
1045  vel = fJe_ * articularVelocity;
1046  break;
1047  }
1049  case vpRobot::MIXT_FRAME: {
1050  break;
1051  }
1052  default: {
1053  vpERROR_TRACE("Error in spec of vpRobot. "
1054  "Case not taken in account.");
1055  return;
1056  }
1057  }
1058 }
1059 
1077 {
1078  timestamp = vpTime::measureTimeSecond();
1079  getVelocity(frame, vel);
1080 }
1081 
1124 {
1125  vpColVector vel(6);
1126  getVelocity(frame, vel);
1127 
1128  return vel;
1129 }
1130 
1144 {
1145  timestamp = vpTime::measureTimeSecond();
1146  vpColVector vel(6);
1147  getVelocity(frame, vel);
1148 
1149  return vel;
1150 }
1151 
1153 {
1154  double vel_rot_max = getMaxRotationVelocity();
1155  double velmax = fabs(q[0]);
1156  for (unsigned int i = 1; i < 6; i++) {
1157  if (velmax < fabs(q[i]))
1158  velmax = fabs(q[i]);
1159  }
1160 
1161  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1162  q = q * alpha;
1163 }
1164 
1240 {
1242  vpERROR_TRACE("Robot was not in position-based control\n"
1243  "Modification of the robot state");
1244  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1245  }
1246 
1247  vpColVector articularCoordinates = get_artCoord();
1248 
1249  vpColVector error(6);
1250  double errsqr = 0;
1251  switch (frame) {
1252  case vpRobot::CAMERA_FRAME: {
1253  unsigned int nbSol;
1254  vpColVector qdes(6);
1255 
1256  vpTranslationVector txyz;
1257  vpRxyzVector rxyz;
1258  for (unsigned int i = 0; i < 3; i++) {
1259  txyz[i] = q[i];
1260  rxyz[i] = q[i + 3];
1261  }
1262 
1263  vpRotationMatrix cRc2(rxyz);
1264  vpHomogeneousMatrix cMc2(txyz, cRc2);
1265 
1266  vpHomogeneousMatrix fMc_;
1267  vpViper850::get_fMc(articularCoordinates, fMc_);
1268 
1269  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1270 
1271  do {
1272  articularCoordinates = get_artCoord();
1273  qdes = articularCoordinates;
1274  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1276  setVelocityCalled = true;
1278  if (nbSol > 0) {
1279  error = qdes - articularCoordinates;
1280  errsqr = error.sumSquare();
1281  // findHighestPositioningSpeed(error);
1282  set_artVel(error);
1283  if (errsqr < 1e-4) {
1284  set_artCoord(qdes);
1285  error = 0;
1286  set_artVel(error);
1287  set_velocity(error);
1288  break;
1289  }
1290  } else {
1291  vpERROR_TRACE("Positioning error.");
1292  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1293  }
1294  } while (errsqr > 1e-8 && nbSol > 0);
1295 
1296  break;
1297  }
1298 
1299  case vpRobot::ARTICULAR_FRAME: {
1300  do {
1301  articularCoordinates = get_artCoord();
1302  error = q - articularCoordinates;
1303  errsqr = error.sumSquare();
1304  // findHighestPositioningSpeed(error);
1305  set_artVel(error);
1307  setVelocityCalled = true;
1309  if (errsqr < 1e-4) {
1310  set_artCoord(q);
1311  error = 0;
1312  set_artVel(error);
1313  set_velocity(error);
1314  break;
1315  }
1316  } while (errsqr > 1e-8);
1317  break;
1318  }
1319 
1320  case vpRobot::REFERENCE_FRAME: {
1321  unsigned int nbSol;
1322  vpColVector qdes(6);
1323 
1324  vpTranslationVector txyz;
1325  vpRxyzVector rxyz;
1326  for (unsigned int i = 0; i < 3; i++) {
1327  txyz[i] = q[i];
1328  rxyz[i] = q[i + 3];
1329  }
1330 
1331  vpRotationMatrix fRc(rxyz);
1332  vpHomogeneousMatrix fMc_(txyz, fRc);
1333 
1334  do {
1335  articularCoordinates = get_artCoord();
1336  qdes = articularCoordinates;
1337  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1338  if (nbSol > 0) {
1339  error = qdes - articularCoordinates;
1340  errsqr = error.sumSquare();
1341  // findHighestPositioningSpeed(error);
1342  set_artVel(error);
1344  setVelocityCalled = true;
1346  if (errsqr < 1e-4) {
1347  set_artCoord(qdes);
1348  error = 0;
1349  set_artVel(error);
1350  set_velocity(error);
1351  break;
1352  }
1353  } else
1354  vpERROR_TRACE("Positioning error. Position unreachable");
1355  } while (errsqr > 1e-8 && nbSol > 0);
1356  break;
1357  }
1358  case vpRobot::MIXT_FRAME: {
1359  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1360  "Mixt frame not implemented.");
1361  }
1363  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1364  "End-effector frame not implemented.");
1365  }
1366  }
1367 }
1368 
1431 void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1432  double pos4, double pos5, double pos6)
1433 {
1434  try {
1435  vpColVector position(6);
1436  position[0] = pos1;
1437  position[1] = pos2;
1438  position[2] = pos3;
1439  position[3] = pos4;
1440  position[4] = pos5;
1441  position[5] = pos6;
1442 
1443  setPosition(frame, position);
1444  } catch (...) {
1445  vpERROR_TRACE("Error caught");
1446  throw;
1447  }
1448 }
1449 
1484 void vpSimulatorViper850::setPosition(const char *filename)
1485 {
1486  vpColVector q;
1487  bool ret;
1488 
1489  ret = this->readPosFile(filename, q);
1490 
1491  if (ret == false) {
1492  vpERROR_TRACE("Bad position in \"%s\"", filename);
1493  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1494  }
1497 }
1498 
1559 {
1560  q.resize(6);
1561 
1562  switch (frame) {
1563  case vpRobot::CAMERA_FRAME: {
1564  q = 0;
1565  break;
1566  }
1567 
1568  case vpRobot::ARTICULAR_FRAME: {
1569  q = get_artCoord();
1570  break;
1571  }
1572 
1573  case vpRobot::REFERENCE_FRAME: {
1574  vpHomogeneousMatrix fMc_;
1575  vpViper::get_fMc(get_artCoord(), fMc_);
1576 
1577  vpRotationMatrix fRc;
1578  fMc_.extract(fRc);
1579  vpRxyzVector rxyz(fRc);
1580 
1581  vpTranslationVector txyz;
1582  fMc_.extract(txyz);
1583 
1584  for (unsigned int i = 0; i < 3; i++) {
1585  q[i] = txyz[i];
1586  q[i + 3] = rxyz[i];
1587  }
1588  break;
1589  }
1590 
1591  case vpRobot::MIXT_FRAME: {
1592  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1593  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1594  "Mixt frame not implemented.");
1595  }
1597  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1598  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1599  "End-effector frame not implemented.");
1600  }
1601  }
1602 }
1603 
1631 {
1632  timestamp = vpTime::measureTimeSecond();
1633  getPosition(frame, q);
1634 }
1635 
1648 {
1649  vpColVector posRxyz;
1650  // recupere position en Rxyz
1651  this->getPosition(frame, posRxyz);
1652 
1653  // recupere le vecteur thetaU correspondant
1654  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1655 
1656  // remplit le vpPoseVector avec translation et rotation ThetaU
1657  for (unsigned int j = 0; j < 3; j++) {
1658  position[j] = posRxyz[j];
1659  position[j + 3] = RtuVect[j];
1660  }
1661 }
1662 
1675  double &timestamp)
1676 {
1677  timestamp = vpTime::measureTimeSecond();
1678  getPosition(frame, position);
1679 }
1680 
1689 void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1690 {
1691  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1692  vpTRACE("Joint limit vector has not a size of 6 !");
1693  return;
1694  }
1695 
1696  joint_min[0] = limitMin[0];
1697  joint_min[1] = limitMin[1];
1698  joint_min[2] = limitMin[2];
1699  joint_min[3] = limitMin[3];
1700  joint_min[4] = limitMin[4];
1701  joint_min[5] = limitMin[5];
1702 
1703  joint_max[0] = limitMax[0];
1704  joint_max[1] = limitMax[1];
1705  joint_max[2] = limitMax[2];
1706  joint_max[3] = limitMax[3];
1707  joint_max[4] = limitMax[4];
1708  joint_max[5] = limitMax[5];
1709 }
1710 
1717 {
1718  double q2 = q[1];
1719  double q3 = q[2];
1720  double q5 = q[4];
1721 
1722  double c2 = cos(q2);
1723  double c3 = cos(q3);
1724  double s3 = sin(q3);
1725  double c23 = cos(q2 + q3);
1726  double s23 = sin(q2 + q3);
1727  double s5 = sin(q5);
1728 
1729  bool cond1 = fabs(s5) < 1e-1;
1730  bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1731  bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1732 
1733  if (cond1) {
1734  J[3][0] = 0;
1735  J[5][0] = 0;
1736  J[3][1] = 0;
1737  J[5][1] = 0;
1738  J[3][2] = 0;
1739  J[5][2] = 0;
1740  J[3][3] = 0;
1741  J[5][3] = 0;
1742  J[3][4] = 0;
1743  J[5][4] = 0;
1744  J[3][5] = 0;
1745  J[5][5] = 0;
1746  return true;
1747  }
1748 
1749  if (cond2) {
1750  J[1][0] = 0;
1751  J[2][0] = 0;
1752  J[3][0] = 0;
1753  J[4][0] = 0;
1754  J[5][0] = 0;
1755  J[1][1] = 0;
1756  J[2][1] = 0;
1757  J[3][1] = 0;
1758  J[4][1] = 0;
1759  J[5][1] = 0;
1760  J[1][2] = 0;
1761  J[2][2] = 0;
1762  J[3][2] = 0;
1763  J[4][2] = 0;
1764  J[5][2] = 0;
1765  return true;
1766  }
1767 
1768  if (cond3) {
1769  J[0][0] = 0;
1770  J[3][0] = 0;
1771  J[4][0] = 0;
1772  J[5][0] = 0;
1773  J[0][1] = 0;
1774  J[3][1] = 0;
1775  J[4][1] = 0;
1776  J[5][1] = 0;
1777  }
1778 
1779  return false;
1780 }
1781 
1786 {
1787  int artNumb = 0;
1788  double diff = 0;
1789  double difft = 0;
1790 
1791  vpColVector articularCoordinates = get_artCoord();
1792 
1793  for (unsigned int i = 0; i < 6; i++) {
1794  if (articularCoordinates[i] <= joint_min[i]) {
1795  difft = joint_min[i] - articularCoordinates[i];
1796  if (difft > diff) {
1797  diff = difft;
1798  artNumb = -(int)i - 1;
1799  }
1800  }
1801  }
1802 
1803  for (unsigned int i = 0; i < 6; i++) {
1804  if (articularCoordinates[i] >= joint_max[i]) {
1805  difft = articularCoordinates[i] - joint_max[i];
1806  if (difft > diff) {
1807  diff = difft;
1808  artNumb = (int)(i + 1);
1809  }
1810  }
1811  }
1812 
1813  if (artNumb != 0)
1814  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1815  << std::endl;
1816 
1817  return artNumb;
1818 }
1819 
1838 {
1839  displacement.resize(6);
1840  displacement = 0;
1841  vpColVector q_cur(6);
1842 
1843  q_cur = get_artCoord();
1844 
1845  if (!first_time_getdis) {
1846  switch (frame) {
1847  case vpRobot::CAMERA_FRAME: {
1848  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1849  return;
1850  }
1851 
1852  case vpRobot::ARTICULAR_FRAME: {
1853  displacement = q_cur - q_prev_getdis;
1854  break;
1855  }
1856 
1857  case vpRobot::REFERENCE_FRAME: {
1858  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1859  return;
1860  }
1861 
1862  case vpRobot::MIXT_FRAME: {
1863  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1864  return;
1865  }
1867  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1868  return;
1869  }
1870  }
1871  } else {
1872  first_time_getdis = false;
1873  }
1874 
1875  // Memorize the joint position for the next call
1876  q_prev_getdis = q_cur;
1877 }
1878 
1940 bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1941 {
1942  std::ifstream fd(filename.c_str(), std::ios::in);
1943 
1944  if (!fd.is_open()) {
1945  return false;
1946  }
1947 
1948  std::string line;
1949  std::string key("R:");
1950  std::string id("#Viper850 - Position");
1951  bool pos_found = false;
1952  int lineNum = 0;
1953 
1954  q.resize(njoint);
1955 
1956  while (std::getline(fd, line)) {
1957  lineNum++;
1958  if (lineNum == 1) {
1959  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1960  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1961  return false;
1962  }
1963  }
1964  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1965  continue;
1966  }
1967  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1968  // check if there are at least njoint values in the line
1969  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1970  if (chain.size() < njoint + 1) // try to split with tab separator
1971  chain = vpIoTools::splitChain(line, std::string("\t"));
1972  if (chain.size() < njoint + 1)
1973  continue;
1974 
1975  std::istringstream ss(line);
1976  std::string key_;
1977  ss >> key_;
1978  for (unsigned int i = 0; i < njoint; i++)
1979  ss >> q[i];
1980  pos_found = true;
1981  break;
1982  }
1983  }
1984 
1985  // converts rotations from degrees into radians
1986  q.deg2rad();
1987 
1988  fd.close();
1989 
1990  if (!pos_found) {
1991  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
1992  return false;
1993  }
1994 
1995  return true;
1996 }
1997 
2019 bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
2020 {
2021 
2022  FILE *fd;
2023  fd = fopen(filename.c_str(), "w");
2024  if (fd == NULL)
2025  return false;
2026 
2027  fprintf(fd, "\
2028 #Viper - Position - Version 1.0\n\
2029 #\n\
2030 # R: A B C D E F\n\
2031 # Joint position in degrees\n\
2032 #\n\
2033 #\n\n");
2034 
2035  // Save positions in mm and deg
2036  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2037  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2038 
2039  fclose(fd);
2040  return (true);
2041 }
2042 
2050 void vpSimulatorViper850::move(const char *filename)
2051 {
2052  vpColVector q;
2053 
2054  try {
2055  this->readPosFile(filename, q);
2058  } catch (...) {
2059  throw;
2060  }
2061 }
2062 
2073 
2082 {
2083  vpHomogeneousMatrix cMe;
2084  vpViper850::get_cMe(cMe);
2085 
2086  cVe.buildFrom(cMe);
2087 }
2088 
2099 {
2100  try {
2102  } catch (...) {
2103  vpERROR_TRACE("catch exception ");
2104  throw;
2105  }
2106 }
2107 
2119 {
2120  try {
2121  vpColVector articularCoordinates = get_artCoord();
2122  vpViper850::get_fJe(articularCoordinates, fJe_);
2123  } catch (...) {
2124  vpERROR_TRACE("Error caught");
2125  throw;
2126  }
2127 }
2128 
2133 {
2135  return;
2136 
2137  vpColVector stop(6);
2138  stop = 0;
2139  set_artVel(stop);
2140  set_velocity(stop);
2142 }
2143 
2144 /**********************************************************************************/
2145 /**********************************************************************************/
2146 /**********************************************************************************/
2147 /**********************************************************************************/
2148 
2158 {
2159  // set scene_dir from #define VISP_SCENE_DIR if it exists
2160  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2161  std::string scene_dir_;
2162  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2163  bool sceneDirExists = false;
2164  for (size_t i = 0; i < scene_dirs.size(); i++)
2165  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2166  scene_dir_ = scene_dirs[i];
2167  sceneDirExists = true;
2168  break;
2169  }
2170  if (!sceneDirExists) {
2171  try {
2172  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2173  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2174  } catch (...) {
2175  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2176  }
2177  }
2178 
2179  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2180  if (scene_dir_.size() > FILENAME_MAX)
2181  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2182 
2183  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2184  if (full_length > FILENAME_MAX)
2185  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2186 
2187  char *name_cam = new char[full_length];
2188 
2189  strcpy(name_cam, scene_dir_.c_str());
2190  strcat(name_cam, "/camera.bnd");
2191  set_scene(name_cam, &camera, cameraFactor);
2192 
2193  if (arm_dir.size() > FILENAME_MAX)
2194  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2195  full_length = (unsigned int)arm_dir.size() + name_length;
2196  if (full_length > FILENAME_MAX)
2197  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2198 
2199  char *name_arm = new char[full_length];
2200  strcpy(name_arm, arm_dir.c_str());
2201  strcat(name_arm, "/viper850_arm1.bnd");
2202  set_scene(name_arm, robotArms, 1.0);
2203  strcpy(name_arm, arm_dir.c_str());
2204  strcat(name_arm, "/viper850_arm2.bnd");
2205  set_scene(name_arm, robotArms + 1, 1.0);
2206  strcpy(name_arm, arm_dir.c_str());
2207  strcat(name_arm, "/viper850_arm3.bnd");
2208  set_scene(name_arm, robotArms + 2, 1.0);
2209  strcpy(name_arm, arm_dir.c_str());
2210  strcat(name_arm, "/viper850_arm4.bnd");
2211  set_scene(name_arm, robotArms + 3, 1.0);
2212  strcpy(name_arm, arm_dir.c_str());
2213  strcat(name_arm, "/viper850_arm5.bnd");
2214  set_scene(name_arm, robotArms + 4, 1.0);
2215  strcpy(name_arm, arm_dir.c_str());
2216  strcat(name_arm, "/viper850_arm6.bnd");
2217  set_scene(name_arm, robotArms + 5, 1.0);
2218 
2219  // set_scene("./arm2.bnd", robotArms+1, 1.0);
2220  // set_scene("./arm3.bnd", robotArms+2, 1.0);
2221  // set_scene("./arm4.bnd", robotArms+3, 1.0);
2222  // set_scene("./arm5.bnd", robotArms+4, 1.0);
2223  // set_scene("./arm6.bnd", robotArms+5, 1.0);
2224 
2225  add_rfstack(IS_BACK);
2226 
2227  add_vwstack("start", "depth", 0.0, 100.0);
2228  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2229  add_vwstack("start", "type", PERSPECTIVE);
2230  //
2231  // sceneInitialized = true;
2232  // displayObject = true;
2233  displayCamera = true;
2234 
2235  delete[] name_cam;
2236  delete[] name_arm;
2237 }
2238 
2240 {
2241  m_mutex_scene.lock();
2242  bool changed = false;
2243  vpHomogeneousMatrix displacement = navigation(I_, changed);
2244 
2245  // if (displacement[2][3] != 0)
2246  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2247  camMf2 = camMf2 * displacement;
2248 
2249  f2Mf = camMf2.inverse() * camMf;
2250 
2251  camMf = camMf2 * displacement * f2Mf;
2252 
2253  double u;
2254  double v;
2255  // if(px_ext != 1 && py_ext != 1)
2256  // we assume px_ext and py_ext > 0
2257  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2258  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2259  u = (double)I_.getWidth() / (2 * px_ext);
2260  v = (double)I_.getHeight() / (2 * py_ext);
2261  } else {
2262  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2263  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2264  }
2265 
2266  float w44o[4][4], w44cext[4][4], x, y, z;
2267 
2268  vp2jlc_matrix(camMf.inverse(), w44cext);
2269 
2270  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2271  x = w44cext[2][0] + w44cext[3][0];
2272  y = w44cext[2][1] + w44cext[3][1];
2273  z = w44cext[2][2] + w44cext[3][2];
2274  add_vwstack("start", "vrp", x, y, z);
2275  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2276  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2277  add_vwstack("start", "window", -u, u, -v, v);
2278 
2279  vpHomogeneousMatrix fMit[8];
2280  get_fMi(fMit);
2281 
2282  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2283  display_scene(w44o, robotArms[0], I_, curColor);
2284 
2285  vp2jlc_matrix(fMit[0], w44o);
2286  display_scene(w44o, robotArms[1], I_, curColor);
2287 
2288  vp2jlc_matrix(fMit[1], w44o);
2289  display_scene(w44o, robotArms[2], I_, curColor);
2290 
2291  vp2jlc_matrix(fMit[2], w44o);
2292  display_scene(w44o, robotArms[3], I_, curColor);
2293 
2294  vp2jlc_matrix(fMit[3], w44o);
2295  display_scene(w44o, robotArms[4], I_, curColor);
2296 
2297  vp2jlc_matrix(fMit[6], w44o);
2298  display_scene(w44o, robotArms[5], I_, curColor);
2299 
2300  if (displayCamera) {
2301  vpHomogeneousMatrix cMe;
2302  get_cMe(cMe);
2303  cMe = cMe.inverse();
2304  cMe = fMit[6] * cMe;
2305  vp2jlc_matrix(cMe, w44o);
2306  display_scene(w44o, camera, I_, camColor);
2307  }
2308 
2309  if (displayObject) {
2310  vp2jlc_matrix(fMo, w44o);
2311  display_scene(w44o, scene, I_, curColor);
2312  }
2314 }
2315 
2333 {
2334  vpColVector stop(6);
2335  bool status = true;
2336  stop = 0;
2337  set_artVel(stop);
2338  set_velocity(stop);
2339  vpHomogeneousMatrix fMc_;
2340  fMc_ = fMo * cMo_.inverse();
2341 
2342  vpColVector articularCoordinates = get_artCoord();
2343  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2344 
2345  if (nbSol == 0) {
2346  status = false;
2347  vpERROR_TRACE("Positioning error. Position unreachable");
2348  }
2349 
2350  if (verbose_)
2351  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2352 
2353  set_artCoord(articularCoordinates);
2354 
2355  compute_fMi();
2356 
2357  return status;
2358 }
2359 
2374 {
2375  vpColVector stop(6);
2376  stop = 0;
2377  m_mutex_scene.lock();
2378  set_artVel(stop);
2379  set_velocity(stop);
2380  vpHomogeneousMatrix fMit[8];
2381  get_fMi(fMit);
2382  fMo = fMit[7] * cMo_;
2384 }
2385 
2386 #elif !defined(VISP_BUILD_SHARED_LIBS)
2387 // Work around to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2388 // has no symbols
2389 void dummy_vpSimulatorViper850(){};
2390 #endif
unsigned int getRows() const
Definition: vpArray2D.h:292
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
double sumSquare() const
vpColVector & deg2rad()
Definition: vpColVector.h:238
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:357
static const vpColor none
Definition: vpColor.h:229
static const vpColor green
Definition: vpColor.h:220
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
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, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
unsigned int getWidth() const
Definition: vpImage.h:247
unsigned int getHeight() const
Definition: vpImage.h:189
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1994
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:435
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:365
static double rad(double deg)
Definition: vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:170
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:178
static double deg(double rad)
Definition: vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void unlock()
Definition: vpMutex.h:111
void lock()
Definition: vpMutex.h:95
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:194
Error that can be emited by the vpRobot class and its derivates.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
void set_velocity(const vpColVector &vel)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix getExternalCameraPosition() const
static void * launcher(void *arg)
void set_artCoord(const vpColVector &coord)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:145
vpControlFrameType
Definition: vpRobot.h:76
@ REFERENCE_FRAME
Definition: vpRobot.h:77
@ ARTICULAR_FRAME
Definition: vpRobot.h:79
@ MIXT_FRAME
Definition: vpRobot.h:87
@ CAMERA_FRAME
Definition: vpRobot.h:83
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:82
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:173
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:69
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
static bool savePosFile(const std::string &filename, const vpColVector &q)
void get_fMi(vpHomogeneousMatrix *fMit)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
void get_fJe(vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void setCameraParameters(const vpCameraParameters &cam)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getExternalImage(vpImage< vpRGBa > &I)
void get_cMe(vpHomogeneousMatrix &cMe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
bool singularityTest(const vpColVector &q, vpMatrix &J)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
static const double defaultPositioningVelocity
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
void get_eJe(vpMatrix &eJe)
double getPositioningVelocity(void)
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:104
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:170
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:131
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:130
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:129
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:132
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
vpTranslationVector etc
Definition: vpViper.h:159
double d6
for joint 6
Definition: vpViper.h:167
double a3
for joint 3
Definition: vpViper.h:165
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
double d4
for joint 4
Definition: vpViper.h:166
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
vpColVector joint_max
Definition: vpViper.h:172
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
double a1
Definition: vpViper.h:163
vpRxyzVector erc
Definition: vpViper.h:160
vpColVector joint_min
Definition: vpViper.h:173
double a2
for joint 2
Definition: vpViper.h:164
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
double d1
for joint 1
Definition: vpViper.h:163
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
#define vpTRACE
Definition: vpDebug.h:416
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()