Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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 #ifdef WINRT_8_1
73  mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
74  mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
75  mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
76  mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
77  mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
78 #else
79  mutex_fMi = CreateMutex(NULL, FALSE, NULL);
80  mutex_artVel = CreateMutex(NULL, FALSE, NULL);
81  mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
82  mutex_velocity = CreateMutex(NULL, FALSE, NULL);
83  mutex_display = CreateMutex(NULL, FALSE, NULL);
84 #endif
85 
86  DWORD dwThreadIdArray;
87  hThread = CreateThread(NULL, // default security attributes
88  0, // use default stack size
89  launcher, // thread function name
90  this, // argument to thread function
91  0, // use default creation flags
92  &dwThreadIdArray); // returns the thread identifier
93 #elif defined(VISP_HAVE_PTHREAD)
94  pthread_mutex_init(&mutex_fMi, NULL);
95  pthread_mutex_init(&mutex_artVel, NULL);
96  pthread_mutex_init(&mutex_artCoord, NULL);
97  pthread_mutex_init(&mutex_velocity, NULL);
98  pthread_mutex_init(&mutex_display, NULL);
99 
100  pthread_attr_init(&attr);
101  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
102 
103  pthread_create(&thread, NULL, launcher, (void *)this);
104 #endif
105 
106  compute_fMi();
107 }
108 
116  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
117  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
118 {
119  init();
120  initDisplay();
121 
123 
124 #if defined(_WIN32)
125 #ifdef WINRT_8_1
126  mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
127  mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
128  mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
129  mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
130  mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
131 #else
132  mutex_fMi = CreateMutex(NULL, FALSE, NULL);
133  mutex_artVel = CreateMutex(NULL, FALSE, NULL);
134  mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
135  mutex_velocity = CreateMutex(NULL, FALSE, NULL);
136  mutex_display = CreateMutex(NULL, FALSE, NULL);
137 #endif
138 
139  DWORD dwThreadIdArray;
140  hThread = CreateThread(NULL, // default security attributes
141  0, // use default stack size
142  launcher, // thread function name
143  this, // argument to thread function
144  0, // use default creation flags
145  &dwThreadIdArray); // returns the thread identifier
146 #elif defined(VISP_HAVE_PTHREAD)
147  pthread_mutex_init(&mutex_fMi, NULL);
148  pthread_mutex_init(&mutex_artVel, NULL);
149  pthread_mutex_init(&mutex_artCoord, NULL);
150  pthread_mutex_init(&mutex_velocity, NULL);
151  pthread_mutex_init(&mutex_display, NULL);
152 
153  pthread_attr_init(&attr);
154  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
155 
156  pthread_create(&thread, NULL, launcher, (void *)this);
157 #endif
158 
159  compute_fMi();
160 }
161 
166 {
167  robotStop = true;
168 
169 #if defined(_WIN32)
170 #if defined(WINRT_8_1)
171  WaitForSingleObjectEx(hThread, INFINITE, FALSE);
172 #else // pure win32
173  WaitForSingleObject(hThread, INFINITE);
174 #endif
175  CloseHandle(hThread);
176  CloseHandle(mutex_fMi);
177  CloseHandle(mutex_artVel);
178  CloseHandle(mutex_artCoord);
179  CloseHandle(mutex_velocity);
180  CloseHandle(mutex_display);
181 #elif defined(VISP_HAVE_PTHREAD)
182  pthread_attr_destroy(&attr);
183  pthread_join(thread, NULL);
184  pthread_mutex_destroy(&mutex_fMi);
185  pthread_mutex_destroy(&mutex_artVel);
186  pthread_mutex_destroy(&mutex_artCoord);
187  pthread_mutex_destroy(&mutex_velocity);
188  pthread_mutex_destroy(&mutex_display);
189 #endif
190 
191  if (robotArms != NULL) {
192  // free_Bound_scene (&(camera));
193  for (int i = 0; i < 6; i++)
194  free_Bound_scene(&(robotArms[i]));
195  }
196 
197  delete[] robotArms;
198  delete[] fMi;
199 }
200 
210 {
211  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
212  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
213  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
214  bool armDirExists = false;
215  for (size_t i = 0; i < arm_dirs.size(); i++)
216  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
217  arm_dir = arm_dirs[i];
218  armDirExists = true;
219  break;
220  }
221  if (!armDirExists) {
222  try {
223  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
224  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
225  } catch (...) {
226  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
227  }
228  }
229 
231  toolCustom = false;
232 
233  size_fMi = 8;
234  fMi = new vpHomogeneousMatrix[8];
237 
238  zeroPos.resize(njoint);
239  zeroPos = 0;
240  zeroPos[1] = -M_PI / 2;
241  zeroPos[2] = M_PI;
242  reposPos.resize(njoint);
243  reposPos = 0;
244  reposPos[1] = -M_PI / 2;
245  reposPos[2] = M_PI;
246  reposPos[4] = M_PI / 2;
247 
248  artCoord = reposPos;
249  artVel = 0;
250 
251  q_prev_getdis.resize(njoint);
252  q_prev_getdis = 0;
253  first_time_getdis = true;
254 
255  positioningVelocity = defaultPositioningVelocity;
256 
259 
260  // Software joint limits in radians
261  // joint_min.resize(njoint);
262  joint_min[0] = vpMath::rad(-50);
263  joint_min[1] = vpMath::rad(-135);
264  joint_min[2] = vpMath::rad(-40);
265  joint_min[3] = vpMath::rad(-190);
266  joint_min[4] = vpMath::rad(-110);
267  joint_min[5] = vpMath::rad(-184);
268  // joint_max.resize(njoint);
269  joint_max[0] = vpMath::rad(50);
270  joint_max[1] = vpMath::rad(-40);
271  joint_max[2] = vpMath::rad(215);
272  joint_max[3] = vpMath::rad(190);
273  joint_max[4] = vpMath::rad(110);
274  joint_max[5] = vpMath::rad(184);
275 }
276 
281 {
282  robotArms = NULL;
283  robotArms = new Bound_scene[6];
284  initArms();
285  setExternalCameraPosition(vpHomogeneousMatrix(0.0, 0.5, 1.5, vpMath::rad(90), 0, 0));
286  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
288  vpCameraParameters tmp;
289  getCameraParameters(tmp, 640, 480);
290  px_int = tmp.get_px();
291  py_int = tmp.get_py();
292  sceneInitialized = true;
293 }
294 
311 {
312  this->projModel = proj_model;
313 
314  // Use here default values of the robot constant parameters.
315  switch (tool) {
317  erc[0] = vpMath::rad(0.07); // rx
318  erc[1] = vpMath::rad(2.76); // ry
319  erc[2] = vpMath::rad(-91.50); // rz
320  etc[0] = -0.0453; // tx
321  etc[1] = 0.0005; // ty
322  etc[2] = 0.0728; // tz
323 
324  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
325  break;
326  }
328  erc[0] = vpMath::rad(0.1527764261); // rx
329  erc[1] = vpMath::rad(1.285092455); // ry
330  erc[2] = vpMath::rad(-90.75777848); // rz
331  etc[0] = -0.04558630174; // tx
332  etc[1] = -0.00134326752; // ty
333  etc[2] = 0.001000828017; // tz
334 
335  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
336  break;
337  }
341  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
342  break;
343  }
344  }
345 
346  vpRotationMatrix eRc(erc);
347  this->eMc.buildFrom(etc, eRc);
348 
349  setToolType(tool);
350  return;
351 }
352 
363 void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
364  const unsigned int &image_height)
365 {
366  if (toolCustom) {
367  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
368  }
369  // Set default parameters
370  switch (getToolType()) {
372  // Set default intrinsic camera parameters for 640x480 images
373  if (image_width == 640 && image_height == 480) {
374  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
375  << std::endl;
376  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
377  } else {
378  vpTRACE("Cannot get default intrinsic camera parameters for this image "
379  "resolution");
380  }
381  break;
382  }
384  // Set default intrinsic camera parameters for 640x480 images
385  if (image_width == 640 && image_height == 480) {
386  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
387  << std::endl;
388  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
389  } else {
390  vpTRACE("Cannot get default intrinsic camera parameters for this image "
391  "resolution");
392  }
393  break;
394  }
398  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
399  break;
400  }
401  }
402  return;
403 }
404 
414 {
415  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
416 }
417 
427 {
428  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
429 }
430 
437 {
438  px_int = cam.get_px();
439  py_int = cam.get_py();
440  toolCustom = true;
441 }
442 
448 {
449  double tcur_1 = tcur; // temporary variable used to store the last time
450  // since the last command
451 
452  while (!robotStop) {
453  // Get current time
454  tprev = tcur_1;
456 
458  setVelocityCalled = false;
460 
461  double ellapsedTime = (tcur - tprev) * 1e-3;
462  if (constantSamplingTimeMode) { // if we want a constant velocity, we
463  // force the ellapsed time to the given
464  // samplingTime
465  ellapsedTime = getSamplingTime(); // in second
466  }
467 
468  vpColVector articularCoordinates = get_artCoord();
469  vpColVector articularVelocities = get_artVel();
470 
471  if (jointLimit) {
472  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
473  if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
474  if (verbose_) {
475  std::cout << "Joint " << jointLimitArt - 1
476  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
477  << " < " << vpMath::deg(joint_max[jointLimitArt - 1]) << std::endl;
478  }
479  articularVelocities = 0.0;
480  } else
481  jointLimit = false;
482  }
483 
484  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
485  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
486  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
487  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
488  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
489  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
490 
491  int jl = isInJointLimit();
492 
493  if (jl != 0 && jointLimit == false) {
494  if (jl < 0)
495  ellapsedTime = (joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
496  (articularVelocities[(unsigned int)(-jl - 1)]);
497  else
498  ellapsedTime = (joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
499  (articularVelocities[(unsigned int)(jl - 1)]);
500 
501  for (unsigned int i = 0; i < 6; i++)
502  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
503 
504  jointLimit = true;
505  jointLimitArt = (unsigned int)fabs((double)jl);
506  }
507 
508  set_artCoord(articularCoordinates);
509  set_artVel(articularVelocities);
510 
511  compute_fMi();
512 
513  if (displayAllowed) {
517  }
518 
519  if (displayType == MODEL_3D && displayAllowed) {
520  while (get_displayBusy())
521  vpTime::wait(2);
523  set_displayBusy(false);
524  }
525 
526  if (displayType == MODEL_DH && displayAllowed) {
527  vpHomogeneousMatrix fMit[8];
528  get_fMi(fMit);
529 
530  // vpDisplay::displayFrame(I,getExternalCameraPosition
531  // ()*fMi[6],cameraParam,0.2,vpColor::none);
532 
533  vpImagePoint iP, iP_1;
534  vpPoint pt(0, 0, 0);
535 
538  pt.track(getExternalCameraPosition() * fMit[0]);
541  for (int k = 1; k < 7; k++) {
542  pt.track(getExternalCameraPosition() * fMit[k - 1]);
544 
545  pt.track(getExternalCameraPosition() * fMit[k]);
547 
549  }
551  thickness_);
552  }
553 
555 
556  vpTime::wait(tcur, 1000 * getSamplingTime());
557  tcur_1 = tcur;
558  } else {
560  }
561  }
562 }
563 
577 {
578  // vpColVector q = get_artCoord();
579  vpColVector q(6); //; = get_artCoord();
580  q = get_artCoord();
581 
582  vpHomogeneousMatrix fMit[8];
583 
584  double q1 = q[0];
585  double q2 = q[1];
586  double q3 = q[2];
587  double q4 = q[3];
588  double q5 = q[4];
589  double q6 = q[5];
590 
591  double c1 = cos(q1);
592  double s1 = sin(q1);
593  double c2 = cos(q2);
594  double s2 = sin(q2);
595  double c23 = cos(q2 + q3);
596  double s23 = sin(q2 + q3);
597  double c4 = cos(q4);
598  double s4 = sin(q4);
599  double c5 = cos(q5);
600  double s5 = sin(q5);
601  double c6 = cos(q6);
602  double s6 = sin(q6);
603 
604  fMit[0][0][0] = c1;
605  fMit[0][1][0] = s1;
606  fMit[0][2][0] = 0;
607  fMit[0][0][1] = 0;
608  fMit[0][1][1] = 0;
609  fMit[0][2][1] = -1;
610  fMit[0][0][2] = -s1;
611  fMit[0][1][2] = c1;
612  fMit[0][2][2] = 0;
613  fMit[0][0][3] = a1 * c1;
614  fMit[0][1][3] = a1 * s1;
615  fMit[0][2][3] = d1;
616 
617  fMit[1][0][0] = c1 * c2;
618  fMit[1][1][0] = s1 * c2;
619  fMit[1][2][0] = -s2;
620  fMit[1][0][1] = -c1 * s2;
621  fMit[1][1][1] = -s1 * s2;
622  fMit[1][2][1] = -c2;
623  fMit[1][0][2] = -s1;
624  fMit[1][1][2] = c1;
625  fMit[1][2][2] = 0;
626  fMit[1][0][3] = c1 * (a2 * c2 + a1);
627  fMit[1][1][3] = s1 * (a2 * c2 + a1);
628  fMit[1][2][3] = d1 - a2 * s2;
629 
630  double quickcomp1 = a3 * c23 - a2 * c2 - a1;
631 
632  fMit[2][0][0] = -c1 * c23;
633  fMit[2][1][0] = -s1 * c23;
634  fMit[2][2][0] = s23;
635  fMit[2][0][1] = s1;
636  fMit[2][1][1] = -c1;
637  fMit[2][2][1] = 0;
638  fMit[2][0][2] = c1 * s23;
639  fMit[2][1][2] = s1 * s23;
640  fMit[2][2][2] = c23;
641  fMit[2][0][3] = -c1 * quickcomp1;
642  fMit[2][1][3] = -s1 * quickcomp1;
643  fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
644 
645  double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
646  double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
647 
648  fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
649  fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
650  fMit[3][2][0] = s23 * c4;
651  fMit[3][0][1] = c1 * s23;
652  fMit[3][1][1] = s1 * s23;
653  fMit[3][2][1] = c23;
654  fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
655  fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
656  fMit[3][2][2] = s23 * s4;
657  fMit[3][0][3] = quickcomp2;
658  fMit[3][1][3] = quickcomp3;
659  fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
660 
661  fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
662  fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
663  fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
664  fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
665  fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
666  fMit[4][2][1] = -s23 * s4;
667  fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
668  fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
669  fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
670  fMit[4][0][3] = quickcomp2;
671  fMit[4][1][3] = quickcomp3;
672  fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
673 
674  fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
675  fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
676  fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
677  fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
678  fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
679  fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
680  fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
681  fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
682  fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
683  fMit[5][0][3] = quickcomp2;
684  fMit[5][1][3] = quickcomp3;
685  fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
686 
687  fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
688  fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
689  fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
690  fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
691  fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
692  fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
693  fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
694  fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
695  fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
696  fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
697  fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
698  fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
699 
701  get_cMe(cMe);
702  cMe = cMe.inverse();
703  // fMit[7] = fMit[6] * cMe;
704  vpViper::get_fMc(q, fMit[7]);
705 
706 #if defined(_WIN32)
707 #if defined(WINRT_8_1)
708  WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
709 #else // pure win32
710  WaitForSingleObject(mutex_fMi, INFINITE);
711 #endif
712  for (int i = 0; i < 8; i++)
713  fMi[i] = fMit[i];
714  ReleaseMutex(mutex_fMi);
715 #elif defined(VISP_HAVE_PTHREAD)
716  pthread_mutex_lock(&mutex_fMi);
717  for (int i = 0; i < 8; i++)
718  fMi[i] = fMit[i];
719  pthread_mutex_unlock(&mutex_fMi);
720 #endif
721 }
722 
729 {
730  switch (newState) {
731  case vpRobot::STATE_STOP: {
732  // Start primitive STOP only if the current state is Velocity
734  stopMotion();
735  }
736  break;
737  }
740  std::cout << "Change the control mode from velocity to position control.\n";
741  stopMotion();
742  } else {
743  // std::cout << "Change the control mode from stop to position
744  // control.\n";
745  }
746  break;
747  }
750  std::cout << "Change the control mode from stop to velocity control.\n";
751  }
752  break;
753  }
755  default:
756  break;
757  }
758 
759  return vpRobot::setRobotState(newState);
760 }
761 
836 {
838  vpERROR_TRACE("Cannot send a velocity to the robot "
839  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
841  "Cannot send a velocity to the robot "
842  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
843  }
844 
845  vpColVector vel_sat(6);
846 
847  double scale_sat = 1;
848  double vel_trans_max = getMaxTranslationVelocity();
849  double vel_rot_max = getMaxRotationVelocity();
850 
851  double vel_abs; // Absolute value
852 
853  // Velocity saturation
854  switch (frame) {
855  // saturation in cartesian space
858  if (vel.getRows() != 6) {
859  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
860  throw;
861  }
862 
863  for (unsigned int i = 0; i < 3; ++i) {
864  vel_abs = fabs(vel[i]);
865  if (vel_abs > vel_trans_max && !jointLimit) {
866  vel_trans_max = vel_abs;
867  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
868  "(axis nr. %d).",
869  vel[i], i + 1);
870  }
871 
872  vel_abs = fabs(vel[i + 3]);
873  if (vel_abs > vel_rot_max && !jointLimit) {
874  vel_rot_max = vel_abs;
875  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
876  "(axis nr. %d).",
877  vel[i + 3], i + 4);
878  }
879  }
880 
881  double scale_trans_sat = 1;
882  double scale_rot_sat = 1;
883  if (vel_trans_max > getMaxTranslationVelocity())
884  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
885 
886  if (vel_rot_max > getMaxRotationVelocity())
887  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
888 
889  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
890  if (scale_trans_sat < scale_rot_sat)
891  scale_sat = scale_trans_sat;
892  else
893  scale_sat = scale_rot_sat;
894  }
895  break;
896  }
897 
898  // saturation in joint space
900  if (vel.getRows() != 6) {
901  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
902  throw;
903  }
904  for (unsigned int i = 0; i < 6; ++i) {
905  vel_abs = fabs(vel[i]);
906  if (vel_abs > vel_rot_max && !jointLimit) {
907  vel_rot_max = vel_abs;
908  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
909  "(axis nr. %d).",
910  vel[i], i + 1);
911  }
912  }
913  double scale_rot_sat = 1;
914  if (vel_rot_max > getMaxRotationVelocity())
915  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
916  if (scale_rot_sat < 1)
917  scale_sat = scale_rot_sat;
918  break;
919  }
921  case vpRobot::MIXT_FRAME: {
922  break;
923  }
924  }
925 
926  set_velocity(vel * scale_sat);
927  setRobotFrame(frame);
928  setVelocityCalled = true;
929 }
930 
935 {
937 
938  double vel_rot_max = getMaxRotationVelocity();
939 
940  vpColVector articularCoordinates = get_artCoord();
941  vpColVector velocityframe = get_velocity();
942  vpColVector articularVelocity;
943 
944  switch (frame) {
945  case vpRobot::CAMERA_FRAME: {
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  vpMatrix fJe_;
958  vpViper850::get_fJe(articularCoordinates, fJe_);
959  fJe_ = fJe_.pseudoInverse();
961  singularityTest(articularCoordinates, fJe_);
962  articularVelocity = fJe_ * velocityframe;
963  set_artVel(articularVelocity);
964  break;
965  }
967  articularVelocity = velocityframe;
968  set_artVel(articularVelocity);
969  break;
970  }
972  case vpRobot::MIXT_FRAME: {
973  break;
974  }
975  }
976 
977  switch (frame) {
980  for (unsigned int i = 0; i < 6; ++i) {
981  double vel_abs = fabs(articularVelocity[i]);
982  if (vel_abs > vel_rot_max && !jointLimit) {
983  vel_rot_max = vel_abs;
984  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
985  "(axis nr. %d).",
986  articularVelocity[i], i + 1);
987  }
988  }
989  double scale_rot_sat = 1;
990  double scale_sat = 1;
991 
992  if (vel_rot_max > getMaxRotationVelocity())
993  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
994  if (scale_rot_sat < 1)
995  scale_sat = scale_rot_sat;
996 
997  set_artVel(articularVelocity * scale_sat);
998  break;
999  }
1002  case vpRobot::MIXT_FRAME: {
1003  break;
1004  }
1005  }
1006 }
1007 
1054 {
1055  vel.resize(6);
1056 
1057  vpColVector articularCoordinates = get_artCoord();
1058  vpColVector articularVelocity = get_artVel();
1059 
1060  switch (frame) {
1061  case vpRobot::CAMERA_FRAME: {
1062  vpMatrix eJe_;
1064  vpViper850::get_eJe(articularCoordinates, eJe_);
1065  vel = cVe * eJe_ * articularVelocity;
1066  break;
1067  }
1068  case vpRobot::ARTICULAR_FRAME: {
1069  vel = articularVelocity;
1070  break;
1071  }
1072  case vpRobot::REFERENCE_FRAME: {
1073  vpMatrix fJe_;
1074  vpViper850::get_fJe(articularCoordinates, fJe_);
1075  vel = fJe_ * articularVelocity;
1076  break;
1077  }
1079  case vpRobot::MIXT_FRAME: {
1080  break;
1081  }
1082  default: {
1083  vpERROR_TRACE("Error in spec of vpRobot. "
1084  "Case not taken in account.");
1085  return;
1086  }
1087  }
1088 }
1089 
1107 {
1108  timestamp = vpTime::measureTimeSecond();
1109  getVelocity(frame, vel);
1110 }
1111 
1154 {
1155  vpColVector vel(6);
1156  getVelocity(frame, vel);
1157 
1158  return vel;
1159 }
1160 
1174 {
1175  timestamp = vpTime::measureTimeSecond();
1176  vpColVector vel(6);
1177  getVelocity(frame, vel);
1178 
1179  return vel;
1180 }
1181 
1183 {
1184  double vel_rot_max = getMaxRotationVelocity();
1185  double velmax = fabs(q[0]);
1186  for (unsigned int i = 1; i < 6; i++) {
1187  if (velmax < fabs(q[i]))
1188  velmax = fabs(q[i]);
1189  }
1190 
1191  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1192  q = q * alpha;
1193 }
1194 
1270 {
1272  vpERROR_TRACE("Robot was not in position-based control\n"
1273  "Modification of the robot state");
1274  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1275  }
1276 
1277  vpColVector articularCoordinates = get_artCoord();
1278 
1279  vpColVector error(6);
1280  double errsqr = 0;
1281  switch (frame) {
1282  case vpRobot::CAMERA_FRAME: {
1283  unsigned int nbSol;
1284  vpColVector qdes(6);
1285 
1286  vpTranslationVector txyz;
1287  vpRxyzVector rxyz;
1288  for (unsigned int i = 0; i < 3; i++) {
1289  txyz[i] = q[i];
1290  rxyz[i] = q[i + 3];
1291  }
1292 
1293  vpRotationMatrix cRc2(rxyz);
1294  vpHomogeneousMatrix cMc2(txyz, cRc2);
1295 
1296  vpHomogeneousMatrix fMc_;
1297  vpViper850::get_fMc(articularCoordinates, fMc_);
1298 
1299  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1300 
1301  do {
1302  articularCoordinates = get_artCoord();
1303  qdes = articularCoordinates;
1304  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1305  setVelocityCalled = true;
1306  if (nbSol > 0) {
1307  error = qdes - articularCoordinates;
1308  errsqr = error.sumSquare();
1309  // findHighestPositioningSpeed(error);
1310  set_artVel(error);
1311  if (errsqr < 1e-4) {
1312  set_artCoord(qdes);
1313  error = 0;
1314  set_artVel(error);
1315  set_velocity(error);
1316  break;
1317  }
1318  } else {
1319  vpERROR_TRACE("Positionning error.");
1320  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1321  }
1322  } while (errsqr > 1e-8 && nbSol > 0);
1323 
1324  break;
1325  }
1326 
1327  case vpRobot::ARTICULAR_FRAME: {
1328  do {
1329  articularCoordinates = get_artCoord();
1330  error = q - articularCoordinates;
1331  errsqr = error.sumSquare();
1332  // findHighestPositioningSpeed(error);
1333  set_artVel(error);
1334  setVelocityCalled = true;
1335  if (errsqr < 1e-4) {
1336  set_artCoord(q);
1337  error = 0;
1338  set_artVel(error);
1339  set_velocity(error);
1340  break;
1341  }
1342  } while (errsqr > 1e-8);
1343  break;
1344  }
1345 
1346  case vpRobot::REFERENCE_FRAME: {
1347  unsigned int nbSol;
1348  vpColVector qdes(6);
1349 
1350  vpTranslationVector txyz;
1351  vpRxyzVector rxyz;
1352  for (unsigned int i = 0; i < 3; i++) {
1353  txyz[i] = q[i];
1354  rxyz[i] = q[i + 3];
1355  }
1356 
1357  vpRotationMatrix fRc(rxyz);
1358  vpHomogeneousMatrix fMc_(txyz, fRc);
1359 
1360  do {
1361  articularCoordinates = get_artCoord();
1362  qdes = articularCoordinates;
1363  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1364  if (nbSol > 0) {
1365  error = qdes - articularCoordinates;
1366  errsqr = error.sumSquare();
1367  // findHighestPositioningSpeed(error);
1368  set_artVel(error);
1369  setVelocityCalled = true;
1370  if (errsqr < 1e-4) {
1371  set_artCoord(qdes);
1372  error = 0;
1373  set_artVel(error);
1374  set_velocity(error);
1375  break;
1376  }
1377  } else
1378  vpERROR_TRACE("Positionning error. Position unreachable");
1379  } while (errsqr > 1e-8 && nbSol > 0);
1380  break;
1381  }
1382  case vpRobot::MIXT_FRAME: {
1383  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1384  "Mixt frame not implemented.");
1385  }
1387  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1388  "End-effector frame not implemented.");
1389  }
1390  }
1391 }
1392 
1455 void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2,
1456  double pos3, double pos4, double pos5, double pos6)
1457 {
1458  try {
1459  vpColVector position(6);
1460  position[0] = pos1;
1461  position[1] = pos2;
1462  position[2] = pos3;
1463  position[3] = pos4;
1464  position[4] = pos5;
1465  position[5] = pos6;
1466 
1467  setPosition(frame, position);
1468  } catch (...) {
1469  vpERROR_TRACE("Error caught");
1470  throw;
1471  }
1472 }
1473 
1508 void vpSimulatorViper850::setPosition(const char *filename)
1509 {
1510  vpColVector q;
1511  bool ret;
1512 
1513  ret = this->readPosFile(filename, q);
1514 
1515  if (ret == false) {
1516  vpERROR_TRACE("Bad position in \"%s\"", filename);
1517  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1518  }
1521 }
1522 
1583 {
1584  q.resize(6);
1585 
1586  switch (frame) {
1587  case vpRobot::CAMERA_FRAME: {
1588  q = 0;
1589  break;
1590  }
1591 
1592  case vpRobot::ARTICULAR_FRAME: {
1593  q = get_artCoord();
1594  break;
1595  }
1596 
1597  case vpRobot::REFERENCE_FRAME: {
1598  vpHomogeneousMatrix fMc_;
1599  vpViper::get_fMc(get_artCoord(), fMc_);
1600 
1601  vpRotationMatrix fRc;
1602  fMc_.extract(fRc);
1603  vpRxyzVector rxyz(fRc);
1604 
1605  vpTranslationVector txyz;
1606  fMc_.extract(txyz);
1607 
1608  for (unsigned int i = 0; i < 3; i++) {
1609  q[i] = txyz[i];
1610  q[i + 3] = rxyz[i];
1611  }
1612  break;
1613  }
1614 
1615  case vpRobot::MIXT_FRAME: {
1616  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1617  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1618  "Mixt frame not implemented.");
1619  }
1621  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1622  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1623  "End-effector frame not implemented.");
1624  }
1625  }
1626 }
1627 
1655 {
1656  timestamp = vpTime::measureTimeSecond();
1657  getPosition(frame, q);
1658 }
1659 
1672 {
1673  vpColVector posRxyz;
1674  // recupere position en Rxyz
1675  this->getPosition(frame, posRxyz);
1676 
1677  // recupere le vecteur thetaU correspondant
1678  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1679 
1680  // remplit le vpPoseVector avec translation et rotation ThetaU
1681  for (unsigned int j = 0; j < 3; j++) {
1682  position[j] = posRxyz[j];
1683  position[j + 3] = RtuVect[j];
1684  }
1685 }
1686 
1699  double &timestamp)
1700 {
1701  timestamp = vpTime::measureTimeSecond();
1702  getPosition(frame, position);
1703 }
1704 
1713 void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1714 {
1715  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1716  vpTRACE("Joint limit vector has not a size of 6 !");
1717  return;
1718  }
1719 
1720  joint_min[0] = limitMin[0];
1721  joint_min[1] = limitMin[1];
1722  joint_min[2] = limitMin[2];
1723  joint_min[3] = limitMin[3];
1724  joint_min[4] = limitMin[4];
1725  joint_min[5] = limitMin[5];
1726 
1727  joint_max[0] = limitMax[0];
1728  joint_max[1] = limitMax[1];
1729  joint_max[2] = limitMax[2];
1730  joint_max[3] = limitMax[3];
1731  joint_max[4] = limitMax[4];
1732  joint_max[5] = limitMax[5];
1733 }
1734 
1741 {
1742  double q2 = q[1];
1743  double q3 = q[2];
1744  double q5 = q[4];
1745 
1746  double c2 = cos(q2);
1747  double c3 = cos(q3);
1748  double s3 = sin(q3);
1749  double c23 = cos(q2 + q3);
1750  double s23 = sin(q2 + q3);
1751  double s5 = sin(q5);
1752 
1753  bool cond1 = fabs(s5) < 1e-1;
1754  bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1755  bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1756 
1757  if (cond1) {
1758  J[3][0] = 0;
1759  J[5][0] = 0;
1760  J[3][1] = 0;
1761  J[5][1] = 0;
1762  J[3][2] = 0;
1763  J[5][2] = 0;
1764  J[3][3] = 0;
1765  J[5][3] = 0;
1766  J[3][4] = 0;
1767  J[5][4] = 0;
1768  J[3][5] = 0;
1769  J[5][5] = 0;
1770  return true;
1771  }
1772 
1773  if (cond2) {
1774  J[1][0] = 0;
1775  J[2][0] = 0;
1776  J[3][0] = 0;
1777  J[4][0] = 0;
1778  J[5][0] = 0;
1779  J[1][1] = 0;
1780  J[2][1] = 0;
1781  J[3][1] = 0;
1782  J[4][1] = 0;
1783  J[5][1] = 0;
1784  J[1][2] = 0;
1785  J[2][2] = 0;
1786  J[3][2] = 0;
1787  J[4][2] = 0;
1788  J[5][2] = 0;
1789  return true;
1790  }
1791 
1792  if (cond3) {
1793  J[0][0] = 0;
1794  J[3][0] = 0;
1795  J[4][0] = 0;
1796  J[5][0] = 0;
1797  J[0][1] = 0;
1798  J[3][1] = 0;
1799  J[4][1] = 0;
1800  J[5][1] = 0;
1801  }
1802 
1803  return false;
1804 }
1805 
1810 {
1811  int artNumb = 0;
1812  double diff = 0;
1813  double difft = 0;
1814 
1815  vpColVector articularCoordinates = get_artCoord();
1816 
1817  for (unsigned int i = 0; i < 6; i++) {
1818  if (articularCoordinates[i] <= joint_min[i]) {
1819  difft = joint_min[i] - articularCoordinates[i];
1820  if (difft > diff) {
1821  diff = difft;
1822  artNumb = -(int)i - 1;
1823  }
1824  }
1825  }
1826 
1827  for (unsigned int i = 0; i < 6; i++) {
1828  if (articularCoordinates[i] >= joint_max[i]) {
1829  difft = articularCoordinates[i] - joint_max[i];
1830  if (difft > diff) {
1831  diff = difft;
1832  artNumb = (int)(i + 1);
1833  }
1834  }
1835  }
1836 
1837  if (artNumb != 0)
1838  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1839  << std::endl;
1840 
1841  return artNumb;
1842 }
1843 
1862 {
1863  displacement.resize(6);
1864  displacement = 0;
1865  vpColVector q_cur(6);
1866 
1867  q_cur = get_artCoord();
1868 
1869  if (!first_time_getdis) {
1870  switch (frame) {
1871  case vpRobot::CAMERA_FRAME: {
1872  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1873  return;
1874  }
1875 
1876  case vpRobot::ARTICULAR_FRAME: {
1877  displacement = q_cur - q_prev_getdis;
1878  break;
1879  }
1880 
1881  case vpRobot::REFERENCE_FRAME: {
1882  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1883  return;
1884  }
1885 
1886  case vpRobot::MIXT_FRAME: {
1887  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1888  return;
1889  }
1891  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1892  return;
1893  }
1894  }
1895  } else {
1896  first_time_getdis = false;
1897  }
1898 
1899  // Memorize the joint position for the next call
1900  q_prev_getdis = q_cur;
1901 }
1902 
1964 bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1965 {
1966  std::ifstream fd(filename.c_str(), std::ios::in);
1967 
1968  if (!fd.is_open()) {
1969  return false;
1970  }
1971 
1972  std::string line;
1973  std::string key("R:");
1974  std::string id("#Viper850 - Position");
1975  bool pos_found = false;
1976  int lineNum = 0;
1977 
1978  q.resize(njoint);
1979 
1980  while (std::getline(fd, line)) {
1981  lineNum++;
1982  if (lineNum == 1) {
1983  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1984  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1985  return false;
1986  }
1987  }
1988  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1989  continue;
1990  }
1991  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1992  // check if there are at least njoint values in the line
1993  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1994  if (chain.size() < njoint + 1) // try to split with tab separator
1995  chain = vpIoTools::splitChain(line, std::string("\t"));
1996  if (chain.size() < njoint + 1)
1997  continue;
1998 
1999  std::istringstream ss(line);
2000  std::string key_;
2001  ss >> key_;
2002  for (unsigned int i = 0; i < njoint; i++)
2003  ss >> q[i];
2004  pos_found = true;
2005  break;
2006  }
2007  }
2008 
2009  // converts rotations from degrees into radians
2010  q.deg2rad();
2011 
2012  fd.close();
2013 
2014  if (!pos_found) {
2015  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2016  return false;
2017  }
2018 
2019  return true;
2020 }
2021 
2043 bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
2044 {
2045 
2046  FILE *fd;
2047  fd = fopen(filename.c_str(), "w");
2048  if (fd == NULL)
2049  return false;
2050 
2051  fprintf(fd, "\
2052 #Viper - Position - Version 1.0\n\
2053 #\n\
2054 # R: A B C D E F\n\
2055 # Joint position in degrees\n\
2056 #\n\
2057 #\n\n");
2058 
2059  // Save positions in mm and deg
2060  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2061  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2062 
2063  fclose(fd);
2064  return (true);
2065 }
2066 
2074 void vpSimulatorViper850::move(const char *filename)
2075 {
2076  vpColVector q;
2077 
2078  try {
2079  this->readPosFile(filename, q);
2082  } catch (...) {
2083  throw;
2084  }
2085 }
2086 
2097 
2106 {
2107  vpHomogeneousMatrix cMe;
2108  vpViper850::get_cMe(cMe);
2109 
2110  cVe.buildFrom(cMe);
2111 }
2112 
2123 {
2124  try {
2126  } catch (...) {
2127  vpERROR_TRACE("catch exception ");
2128  throw;
2129  }
2130 }
2131 
2143 {
2144  try {
2145  vpColVector articularCoordinates = get_artCoord();
2146  vpViper850::get_fJe(articularCoordinates, fJe_);
2147  } catch (...) {
2148  vpERROR_TRACE("Error caught");
2149  throw;
2150  }
2151 }
2152 
2157 {
2159  return;
2160 
2161  vpColVector stop(6);
2162  stop = 0;
2163  set_artVel(stop);
2164  set_velocity(stop);
2166 }
2167 
2168 /**********************************************************************************/
2169 /**********************************************************************************/
2170 /**********************************************************************************/
2171 /**********************************************************************************/
2172 
2182 {
2183  // set scene_dir from #define VISP_SCENE_DIR if it exists
2184  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2185  std::string scene_dir_;
2186  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2187  bool sceneDirExists = false;
2188  for (size_t i = 0; i < scene_dirs.size(); i++)
2189  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2190  scene_dir_ = scene_dirs[i];
2191  sceneDirExists = true;
2192  break;
2193  }
2194  if (!sceneDirExists) {
2195  try {
2196  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2197  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2198  } catch (...) {
2199  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2200  }
2201  }
2202 
2203  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2204  if (scene_dir_.size() > FILENAME_MAX)
2205  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2206 
2207  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2208  if (full_length > FILENAME_MAX)
2209  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2210 
2211  char *name_cam = new char[full_length];
2212 
2213  strcpy(name_cam, scene_dir_.c_str());
2214  strcat(name_cam, "/camera.bnd");
2215  set_scene(name_cam, &camera, cameraFactor);
2216 
2217  if (arm_dir.size() > FILENAME_MAX)
2218  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2219  full_length = (unsigned int)arm_dir.size() + name_length;
2220  if (full_length > FILENAME_MAX)
2221  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2222 
2223  char *name_arm = new char[full_length];
2224  strcpy(name_arm, arm_dir.c_str());
2225  strcat(name_arm, "/viper850_arm1.bnd");
2226  set_scene(name_arm, robotArms, 1.0);
2227  strcpy(name_arm, arm_dir.c_str());
2228  strcat(name_arm, "/viper850_arm2.bnd");
2229  set_scene(name_arm, robotArms + 1, 1.0);
2230  strcpy(name_arm, arm_dir.c_str());
2231  strcat(name_arm, "/viper850_arm3.bnd");
2232  set_scene(name_arm, robotArms + 2, 1.0);
2233  strcpy(name_arm, arm_dir.c_str());
2234  strcat(name_arm, "/viper850_arm4.bnd");
2235  set_scene(name_arm, robotArms + 3, 1.0);
2236  strcpy(name_arm, arm_dir.c_str());
2237  strcat(name_arm, "/viper850_arm5.bnd");
2238  set_scene(name_arm, robotArms + 4, 1.0);
2239  strcpy(name_arm, arm_dir.c_str());
2240  strcat(name_arm, "/viper850_arm6.bnd");
2241  set_scene(name_arm, robotArms + 5, 1.0);
2242 
2243  // set_scene("./arm2.bnd", robotArms+1, 1.0);
2244  // set_scene("./arm3.bnd", robotArms+2, 1.0);
2245  // set_scene("./arm4.bnd", robotArms+3, 1.0);
2246  // set_scene("./arm5.bnd", robotArms+4, 1.0);
2247  // set_scene("./arm6.bnd", robotArms+5, 1.0);
2248 
2249  add_rfstack(IS_BACK);
2250 
2251  add_vwstack("start", "depth", 0.0, 100.0);
2252  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2253  add_vwstack("start", "type", PERSPECTIVE);
2254  //
2255  // sceneInitialized = true;
2256  // displayObject = true;
2257  displayCamera = true;
2258 
2259  delete[] name_cam;
2260  delete[] name_arm;
2261 }
2262 
2264 {
2265  bool changed = false;
2266  vpHomogeneousMatrix displacement = navigation(I_, changed);
2267 
2268  // if (displacement[2][3] != 0)
2269  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2270  camMf2 = camMf2 * displacement;
2271 
2272  f2Mf = camMf2.inverse() * camMf;
2273 
2274  camMf = camMf2 * displacement * f2Mf;
2275 
2276  double u;
2277  double v;
2278  // if(px_ext != 1 && py_ext != 1)
2279  // we assume px_ext and py_ext > 0
2280  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2281  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2282  u = (double)I_.getWidth() / (2 * px_ext);
2283  v = (double)I_.getHeight() / (2 * py_ext);
2284  } else {
2285  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2286  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2287  }
2288 
2289  float w44o[4][4], w44cext[4][4], x, y, z;
2290 
2291  vp2jlc_matrix(camMf.inverse(), w44cext);
2292 
2293  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2294  x = w44cext[2][0] + w44cext[3][0];
2295  y = w44cext[2][1] + w44cext[3][1];
2296  z = w44cext[2][2] + w44cext[3][2];
2297  add_vwstack("start", "vrp", x, y, z);
2298  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2299  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2300  add_vwstack("start", "window", -u, u, -v, v);
2301 
2302  vpHomogeneousMatrix fMit[8];
2303  get_fMi(fMit);
2304 
2305  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2306  display_scene(w44o, robotArms[0], I_, curColor);
2307 
2308  vp2jlc_matrix(fMit[0], w44o);
2309  display_scene(w44o, robotArms[1], I_, curColor);
2310 
2311  vp2jlc_matrix(fMit[1], w44o);
2312  display_scene(w44o, robotArms[2], I_, curColor);
2313 
2314  vp2jlc_matrix(fMit[2], w44o);
2315  display_scene(w44o, robotArms[3], I_, curColor);
2316 
2317  vp2jlc_matrix(fMit[3], w44o);
2318  display_scene(w44o, robotArms[4], I_, curColor);
2319 
2320  vp2jlc_matrix(fMit[6], w44o);
2321  display_scene(w44o, robotArms[5], I_, curColor);
2322 
2323  if (displayCamera) {
2324  vpHomogeneousMatrix cMe;
2325  get_cMe(cMe);
2326  cMe = cMe.inverse();
2327  cMe = fMit[6] * cMe;
2328  vp2jlc_matrix(cMe, w44o);
2329  display_scene(w44o, camera, I_, camColor);
2330  }
2331 
2332  if (displayObject) {
2333  vp2jlc_matrix(fMo, w44o);
2334  display_scene(w44o, scene, I_, curColor);
2335  }
2336 }
2337 
2355 {
2356  vpColVector stop(6);
2357  bool status = true;
2358  stop = 0;
2359  set_artVel(stop);
2360  set_velocity(stop);
2361  vpHomogeneousMatrix fMc_;
2362  fMc_ = fMo * cMo_.inverse();
2363 
2364  vpColVector articularCoordinates = get_artCoord();
2365  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2366 
2367  if (nbSol == 0) {
2368  status = false;
2369  vpERROR_TRACE("Positionning error. Position unreachable");
2370  }
2371 
2372  if (verbose_)
2373  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2374 
2375  set_artCoord(articularCoordinates);
2376 
2377  compute_fMi();
2378 
2379  return status;
2380 }
2381 
2396 {
2397  vpColVector stop(6);
2398  stop = 0;
2399  set_artVel(stop);
2400  set_velocity(stop);
2401  vpHomogeneousMatrix fMit[8];
2402  get_fMi(fMit);
2403  fMo = fMit[7] * cMo_;
2404 }
2405 
2406 #elif !defined(VISP_BUILD_SHARED_LIBS)
2407 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2408 // has no symbols
2409 void dummy_vpSimulatorViper850(){};
2410 #endif
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2206
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
double a3
for joint 3
Definition: vpViper.h:165
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:157
Error that can be emited by the vpRobot class and its derivates.
vpHomogeneousMatrix getExternalCameraPosition() const
void get_fMi(vpHomogeneousMatrix *fMit)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
void get_cMe(vpHomogeneousMatrix &cMe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void * launcher(void *arg)
bool singularityTest(const vpColVector &q, vpMatrix &J)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT double measureTimeSecond()
Definition: vpTime.cpp:158
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
void getExternalImage(vpImage< vpRGBa > &I)
static const vpColor none
Definition: vpColor.h:191
Initialize the position controller.
Definition: vpRobot.h:67
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
void track(const vpHomogeneousMatrix &cMo)
vpHomogeneousMatrix inverse() const
vpRowVector t() const
void get_fJe(vpMatrix &fJe)
vpControlFrameType
Definition: vpRobot.h:75
vpRxyzVector erc
Definition: vpViper.h:160
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
void extract(vpRotationMatrix &R) const
static const vpColor green
Definition: vpColor.h:182
vpHomogeneousMatrix fMo
double a2
for joint 2
Definition: vpViper.h:164
static void flush(const vpImage< unsigned char > &I)
static bool savePosFile(const std::string &filename, const vpColVector &q)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
Class that defines what is a point.
Definition: vpPoint.h:58
void deg2rad()
Definition: vpColVector.h:196
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:422
double d1
for joint 1
Definition: vpViper.h:163
double a1
Definition: vpViper.h:163
void move(const char *filename)
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:170
vpTranslationVector etc
Definition: vpViper.h:159
Initialize the acceleration controller.
Definition: vpRobot.h:68
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpTRACE
Definition: vpDebug.h:416
vpHomogeneousMatrix f2Mf
void set_displayBusy(const bool &status)
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
static void display(const vpImage< unsigned char > &I)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
vpColVector joint_max
Definition: vpViper.h:172
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:128
Generic class defining intrinsic camera parameters.
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:103
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1767
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:151
static bool readPosFile(const std::string &filename, vpColVector &q)
double getSamplingTime() const
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:108
void setExternalCameraParameters(const vpCameraParameters &cam)
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:101
unsigned int getHeight() const
Definition: vpImage.h:186
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
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))
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:431
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:151
double sumSquare() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:433
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:172
double d4
for joint 4
Definition: vpViper.h:166
void findHighestPositioningSpeed(vpColVector &q)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_artCoord(const vpColVector &coord)
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:86
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
unsigned int getWidth() const
Definition: vpImage.h:244
static const double defaultPositioningVelocity
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double getPositioningVelocity(void)
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:154
vpHomogeneousMatrix camMf2
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void get_eJe(vpMatrix &eJe)
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:355
double d6
for joint 6
Definition: vpViper.h:167
vpColVector joint_min
Definition: vpViper.h:173