Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 
837 {
839  vpERROR_TRACE("Cannot send a velocity to the robot "
840  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
842  "Cannot send a velocity to the robot "
843  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
844  }
845 
846  vpColVector vel_sat(6);
847 
848  double scale_sat = 1;
849  double vel_trans_max = getMaxTranslationVelocity();
850  double vel_rot_max = getMaxRotationVelocity();
851 
852  double vel_abs; // Absolute value
853 
854  // Velocity saturation
855  switch (frame) {
856  // saturation in cartesian space
859  if (vel.getRows() != 6) {
860  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
861  throw;
862  }
863 
864  for (unsigned int i = 0; i < 3; ++i) {
865  vel_abs = fabs(vel[i]);
866  if (vel_abs > vel_trans_max && !jointLimit) {
867  vel_trans_max = vel_abs;
868  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
869  "(axis nr. %d).",
870  vel[i], i + 1);
871  }
872 
873  vel_abs = fabs(vel[i + 3]);
874  if (vel_abs > vel_rot_max && !jointLimit) {
875  vel_rot_max = vel_abs;
876  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
877  "(axis nr. %d).",
878  vel[i + 3], i + 4);
879  }
880  }
881 
882  double scale_trans_sat = 1;
883  double scale_rot_sat = 1;
884  if (vel_trans_max > getMaxTranslationVelocity())
885  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
886 
887  if (vel_rot_max > getMaxRotationVelocity())
888  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
889 
890  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
891  if (scale_trans_sat < scale_rot_sat)
892  scale_sat = scale_trans_sat;
893  else
894  scale_sat = scale_rot_sat;
895  }
896  break;
897  }
898 
899  // saturation in joint space
901  if (vel.getRows() != 6) {
902  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
903  throw;
904  }
905  for (unsigned int i = 0; i < 6; ++i) {
906  vel_abs = fabs(vel[i]);
907  if (vel_abs > vel_rot_max && !jointLimit) {
908  vel_rot_max = vel_abs;
909  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
910  "(axis nr. %d).",
911  vel[i], i + 1);
912  }
913  }
914  double scale_rot_sat = 1;
915  if (vel_rot_max > getMaxRotationVelocity())
916  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
917  if (scale_rot_sat < 1)
918  scale_sat = scale_rot_sat;
919  break;
920  }
922  case vpRobot::MIXT_FRAME: {
923  break;
924  }
925  }
926 
927  set_velocity(vel * scale_sat);
928  setRobotFrame(frame);
929  setVelocityCalled = true;
930 }
931 
936 {
938 
939  double vel_rot_max = getMaxRotationVelocity();
940 
941  vpColVector articularCoordinates = get_artCoord();
942  vpColVector velocityframe = get_velocity();
943  vpColVector articularVelocity;
944 
945  switch (frame) {
946  case vpRobot::CAMERA_FRAME: {
947  vpMatrix eJe_;
949  vpViper850::get_eJe(articularCoordinates, eJe_);
950  eJe_ = eJe_.pseudoInverse();
952  singularityTest(articularCoordinates, eJe_);
953  articularVelocity = eJe_ * eVc * velocityframe;
954  set_artVel(articularVelocity);
955  break;
956  }
958  vpMatrix fJe_;
959  vpViper850::get_fJe(articularCoordinates, fJe_);
960  fJe_ = fJe_.pseudoInverse();
962  singularityTest(articularCoordinates, fJe_);
963  articularVelocity = fJe_ * velocityframe;
964  set_artVel(articularVelocity);
965  break;
966  }
968  articularVelocity = velocityframe;
969  set_artVel(articularVelocity);
970  break;
971  }
973  case vpRobot::MIXT_FRAME: {
974  break;
975  }
976  }
977 
978  switch (frame) {
981  for (unsigned int i = 0; i < 6; ++i) {
982  double vel_abs = fabs(articularVelocity[i]);
983  if (vel_abs > vel_rot_max && !jointLimit) {
984  vel_rot_max = vel_abs;
985  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
986  "(axis nr. %d).",
987  articularVelocity[i], i + 1);
988  }
989  }
990  double scale_rot_sat = 1;
991  double scale_sat = 1;
992 
993  if (vel_rot_max > getMaxRotationVelocity())
994  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
995  if (scale_rot_sat < 1)
996  scale_sat = scale_rot_sat;
997 
998  set_artVel(articularVelocity * scale_sat);
999  break;
1000  }
1003  case vpRobot::MIXT_FRAME: {
1004  break;
1005  }
1006  }
1007 }
1008 
1056 {
1057  vel.resize(6);
1058 
1059  vpColVector articularCoordinates = get_artCoord();
1060  vpColVector articularVelocity = get_artVel();
1061 
1062  switch (frame) {
1063  case vpRobot::CAMERA_FRAME: {
1064  vpMatrix eJe_;
1066  vpViper850::get_eJe(articularCoordinates, eJe_);
1067  vel = cVe * eJe_ * articularVelocity;
1068  break;
1069  }
1070  case vpRobot::ARTICULAR_FRAME: {
1071  vel = articularVelocity;
1072  break;
1073  }
1074  case vpRobot::REFERENCE_FRAME: {
1075  vpMatrix fJe_;
1076  vpViper850::get_fJe(articularCoordinates, fJe_);
1077  vel = fJe_ * articularVelocity;
1078  break;
1079  }
1081  case vpRobot::MIXT_FRAME: {
1082  break;
1083  }
1084  default: {
1085  vpERROR_TRACE("Error in spec of vpRobot. "
1086  "Case not taken in account.");
1087  return;
1088  }
1089  }
1090 }
1091 
1109 {
1110  timestamp = vpTime::measureTimeSecond();
1111  getVelocity(frame, vel);
1112 }
1113 
1157 {
1158  vpColVector vel(6);
1159  getVelocity(frame, vel);
1160 
1161  return vel;
1162 }
1163 
1177 {
1178  timestamp = vpTime::measureTimeSecond();
1179  vpColVector vel(6);
1180  getVelocity(frame, vel);
1181 
1182  return vel;
1183 }
1184 
1186 {
1187  double vel_rot_max = getMaxRotationVelocity();
1188  double velmax = fabs(q[0]);
1189  for (unsigned int i = 1; i < 6; i++) {
1190  if (velmax < fabs(q[i]))
1191  velmax = fabs(q[i]);
1192  }
1193 
1194  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1195  q = q * alpha;
1196 }
1197 
1274 {
1276  vpERROR_TRACE("Robot was not in position-based control\n"
1277  "Modification of the robot state");
1278  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1279  }
1280 
1281  vpColVector articularCoordinates = get_artCoord();
1282 
1283  vpColVector error(6);
1284  double errsqr = 0;
1285  switch (frame) {
1286  case vpRobot::CAMERA_FRAME: {
1287  unsigned int nbSol;
1288  vpColVector qdes(6);
1289 
1290  vpTranslationVector txyz;
1291  vpRxyzVector rxyz;
1292  for (unsigned int i = 0; i < 3; i++) {
1293  txyz[i] = q[i];
1294  rxyz[i] = q[i + 3];
1295  }
1296 
1297  vpRotationMatrix cRc2(rxyz);
1298  vpHomogeneousMatrix cMc2(txyz, cRc2);
1299 
1300  vpHomogeneousMatrix fMc_;
1301  vpViper850::get_fMc(articularCoordinates, fMc_);
1302 
1303  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1304 
1305  do {
1306  articularCoordinates = get_artCoord();
1307  qdes = articularCoordinates;
1308  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1309  setVelocityCalled = true;
1310  if (nbSol > 0) {
1311  error = qdes - articularCoordinates;
1312  errsqr = error.sumSquare();
1313  // findHighestPositioningSpeed(error);
1314  set_artVel(error);
1315  if (errsqr < 1e-4) {
1316  set_artCoord(qdes);
1317  error = 0;
1318  set_artVel(error);
1319  set_velocity(error);
1320  break;
1321  }
1322  } else {
1323  vpERROR_TRACE("Positionning error.");
1324  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1325  }
1326  } while (errsqr > 1e-8 && nbSol > 0);
1327 
1328  break;
1329  }
1330 
1331  case vpRobot::ARTICULAR_FRAME: {
1332  do {
1333  articularCoordinates = get_artCoord();
1334  error = q - articularCoordinates;
1335  errsqr = error.sumSquare();
1336  // findHighestPositioningSpeed(error);
1337  set_artVel(error);
1338  setVelocityCalled = true;
1339  if (errsqr < 1e-4) {
1340  set_artCoord(q);
1341  error = 0;
1342  set_artVel(error);
1343  set_velocity(error);
1344  break;
1345  }
1346  } while (errsqr > 1e-8);
1347  break;
1348  }
1349 
1350  case vpRobot::REFERENCE_FRAME: {
1351  unsigned int nbSol;
1352  vpColVector qdes(6);
1353 
1354  vpTranslationVector txyz;
1355  vpRxyzVector rxyz;
1356  for (unsigned int i = 0; i < 3; i++) {
1357  txyz[i] = q[i];
1358  rxyz[i] = q[i + 3];
1359  }
1360 
1361  vpRotationMatrix fRc(rxyz);
1362  vpHomogeneousMatrix fMc_(txyz, fRc);
1363 
1364  do {
1365  articularCoordinates = get_artCoord();
1366  qdes = articularCoordinates;
1367  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1368  if (nbSol > 0) {
1369  error = qdes - articularCoordinates;
1370  errsqr = error.sumSquare();
1371  // findHighestPositioningSpeed(error);
1372  set_artVel(error);
1373  setVelocityCalled = true;
1374  if (errsqr < 1e-4) {
1375  set_artCoord(qdes);
1376  error = 0;
1377  set_artVel(error);
1378  set_velocity(error);
1379  break;
1380  }
1381  } else
1382  vpERROR_TRACE("Positionning error. Position unreachable");
1383  } while (errsqr > 1e-8 && nbSol > 0);
1384  break;
1385  }
1386  case vpRobot::MIXT_FRAME: {
1387  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1388  "Mixt frame not implemented.");
1389  }
1391  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1392  "End-effector frame not implemented.");
1393  }
1394  }
1395 }
1396 
1460 void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, const double pos1, const double pos2,
1461  const double pos3, const double pos4, const double pos5, const double pos6)
1462 {
1463  try {
1464  vpColVector position(6);
1465  position[0] = pos1;
1466  position[1] = pos2;
1467  position[2] = pos3;
1468  position[3] = pos4;
1469  position[4] = pos5;
1470  position[5] = pos6;
1471 
1472  setPosition(frame, position);
1473  } catch (...) {
1474  vpERROR_TRACE("Error caught");
1475  throw;
1476  }
1477 }
1478 
1514 void vpSimulatorViper850::setPosition(const char *filename)
1515 {
1516  vpColVector q;
1517  bool ret;
1518 
1519  ret = this->readPosFile(filename, q);
1520 
1521  if (ret == false) {
1522  vpERROR_TRACE("Bad position in \"%s\"", filename);
1523  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1524  }
1527 }
1528 
1590 {
1591  q.resize(6);
1592 
1593  switch (frame) {
1594  case vpRobot::CAMERA_FRAME: {
1595  q = 0;
1596  break;
1597  }
1598 
1599  case vpRobot::ARTICULAR_FRAME: {
1600  q = get_artCoord();
1601  break;
1602  }
1603 
1604  case vpRobot::REFERENCE_FRAME: {
1605  vpHomogeneousMatrix fMc_;
1606  vpViper::get_fMc(get_artCoord(), fMc_);
1607 
1608  vpRotationMatrix fRc;
1609  fMc_.extract(fRc);
1610  vpRxyzVector rxyz(fRc);
1611 
1612  vpTranslationVector txyz;
1613  fMc_.extract(txyz);
1614 
1615  for (unsigned int i = 0; i < 3; i++) {
1616  q[i] = txyz[i];
1617  q[i + 3] = rxyz[i];
1618  }
1619  break;
1620  }
1621 
1622  case vpRobot::MIXT_FRAME: {
1623  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1624  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1625  "Mixt frame not implemented.");
1626  }
1628  vpERROR_TRACE("Positionning error. Mixt frame not implemented");
1629  throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
1630  "End-effector frame not implemented.");
1631  }
1632  }
1633 }
1634 
1662 {
1663  timestamp = vpTime::measureTimeSecond();
1664  getPosition(frame, q);
1665 }
1666 
1679 {
1680  vpColVector posRxyz;
1681  // recupere position en Rxyz
1682  this->getPosition(frame, posRxyz);
1683 
1684  // recupere le vecteur thetaU correspondant
1685  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1686 
1687  // remplit le vpPoseVector avec translation et rotation ThetaU
1688  for (unsigned int j = 0; j < 3; j++) {
1689  position[j] = posRxyz[j];
1690  position[j + 3] = RtuVect[j];
1691  }
1692 }
1693 
1706  double &timestamp)
1707 {
1708  timestamp = vpTime::measureTimeSecond();
1709  getPosition(frame, position);
1710 }
1711 
1720 void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1721 {
1722  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1723  vpTRACE("Joint limit vector has not a size of 6 !");
1724  return;
1725  }
1726 
1727  joint_min[0] = limitMin[0];
1728  joint_min[1] = limitMin[1];
1729  joint_min[2] = limitMin[2];
1730  joint_min[3] = limitMin[3];
1731  joint_min[4] = limitMin[4];
1732  joint_min[5] = limitMin[5];
1733 
1734  joint_max[0] = limitMax[0];
1735  joint_max[1] = limitMax[1];
1736  joint_max[2] = limitMax[2];
1737  joint_max[3] = limitMax[3];
1738  joint_max[4] = limitMax[4];
1739  joint_max[5] = limitMax[5];
1740 }
1741 
1748 {
1749  double q2 = q[1];
1750  double q3 = q[2];
1751  double q5 = q[4];
1752 
1753  double c2 = cos(q2);
1754  double c3 = cos(q3);
1755  double s3 = sin(q3);
1756  double c23 = cos(q2 + q3);
1757  double s23 = sin(q2 + q3);
1758  double s5 = sin(q5);
1759 
1760  bool cond1 = fabs(s5) < 1e-1;
1761  bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1762  bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1763 
1764  if (cond1) {
1765  J[3][0] = 0;
1766  J[5][0] = 0;
1767  J[3][1] = 0;
1768  J[5][1] = 0;
1769  J[3][2] = 0;
1770  J[5][2] = 0;
1771  J[3][3] = 0;
1772  J[5][3] = 0;
1773  J[3][4] = 0;
1774  J[5][4] = 0;
1775  J[3][5] = 0;
1776  J[5][5] = 0;
1777  return true;
1778  }
1779 
1780  if (cond2) {
1781  J[1][0] = 0;
1782  J[2][0] = 0;
1783  J[3][0] = 0;
1784  J[4][0] = 0;
1785  J[5][0] = 0;
1786  J[1][1] = 0;
1787  J[2][1] = 0;
1788  J[3][1] = 0;
1789  J[4][1] = 0;
1790  J[5][1] = 0;
1791  J[1][2] = 0;
1792  J[2][2] = 0;
1793  J[3][2] = 0;
1794  J[4][2] = 0;
1795  J[5][2] = 0;
1796  return true;
1797  }
1798 
1799  if (cond3) {
1800  J[0][0] = 0;
1801  J[3][0] = 0;
1802  J[4][0] = 0;
1803  J[5][0] = 0;
1804  J[0][1] = 0;
1805  J[3][1] = 0;
1806  J[4][1] = 0;
1807  J[5][1] = 0;
1808  }
1809 
1810  return false;
1811 }
1812 
1817 {
1818  int artNumb = 0;
1819  double diff = 0;
1820  double difft = 0;
1821 
1822  vpColVector articularCoordinates = get_artCoord();
1823 
1824  for (unsigned int i = 0; i < 6; i++) {
1825  if (articularCoordinates[i] <= joint_min[i]) {
1826  difft = joint_min[i] - articularCoordinates[i];
1827  if (difft > diff) {
1828  diff = difft;
1829  artNumb = -(int)i - 1;
1830  }
1831  }
1832  }
1833 
1834  for (unsigned int i = 0; i < 6; i++) {
1835  if (articularCoordinates[i] >= joint_max[i]) {
1836  difft = articularCoordinates[i] - joint_max[i];
1837  if (difft > diff) {
1838  diff = difft;
1839  artNumb = (int)(i + 1);
1840  }
1841  }
1842  }
1843 
1844  if (artNumb != 0)
1845  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1846  << std::endl;
1847 
1848  return artNumb;
1849 }
1850 
1869 {
1870  displacement.resize(6);
1871  displacement = 0;
1872  vpColVector q_cur(6);
1873 
1874  q_cur = get_artCoord();
1875 
1876  if (!first_time_getdis) {
1877  switch (frame) {
1878  case vpRobot::CAMERA_FRAME: {
1879  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1880  return;
1881  }
1882 
1883  case vpRobot::ARTICULAR_FRAME: {
1884  displacement = q_cur - q_prev_getdis;
1885  break;
1886  }
1887 
1888  case vpRobot::REFERENCE_FRAME: {
1889  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1890  return;
1891  }
1892 
1893  case vpRobot::MIXT_FRAME: {
1894  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1895  return;
1896  }
1898  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1899  return;
1900  }
1901  }
1902  } else {
1903  first_time_getdis = false;
1904  }
1905 
1906  // Memorize the joint position for the next call
1907  q_prev_getdis = q_cur;
1908 }
1909 
1972 bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1973 {
1974  std::ifstream fd(filename.c_str(), std::ios::in);
1975 
1976  if (!fd.is_open()) {
1977  return false;
1978  }
1979 
1980  std::string line;
1981  std::string key("R:");
1982  std::string id("#Viper850 - Position");
1983  bool pos_found = false;
1984  int lineNum = 0;
1985 
1986  q.resize(njoint);
1987 
1988  while (std::getline(fd, line)) {
1989  lineNum++;
1990  if (lineNum == 1) {
1991  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1992  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1993  return false;
1994  }
1995  }
1996  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1997  continue;
1998  }
1999  if ((line.compare(0, key.size(), key) == 0)) { // decode position
2000  // check if there are at least njoint values in the line
2001  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
2002  if (chain.size() < njoint + 1) // try to split with tab separator
2003  chain = vpIoTools::splitChain(line, std::string("\t"));
2004  if (chain.size() < njoint + 1)
2005  continue;
2006 
2007  std::istringstream ss(line);
2008  std::string key_;
2009  ss >> key_;
2010  for (unsigned int i = 0; i < njoint; i++)
2011  ss >> q[i];
2012  pos_found = true;
2013  break;
2014  }
2015  }
2016 
2017  // converts rotations from degrees into radians
2018  q.deg2rad();
2019 
2020  fd.close();
2021 
2022  if (!pos_found) {
2023  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
2024  return false;
2025  }
2026 
2027  return true;
2028 }
2029 
2051 bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
2052 {
2053 
2054  FILE *fd;
2055  fd = fopen(filename.c_str(), "w");
2056  if (fd == NULL)
2057  return false;
2058 
2059  fprintf(fd, "\
2060 #Viper - Position - Version 1.0\n\
2061 #\n\
2062 # R: A B C D E F\n\
2063 # Joint position in degrees\n\
2064 #\n\
2065 #\n\n");
2066 
2067  // Save positions in mm and deg
2068  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2069  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2070 
2071  fclose(fd);
2072  return (true);
2073 }
2074 
2082 void vpSimulatorViper850::move(const char *filename)
2083 {
2084  vpColVector q;
2085 
2086  try {
2087  this->readPosFile(filename, q);
2090  } catch (...) {
2091  throw;
2092  }
2093 }
2094 
2105 
2114 {
2115  vpHomogeneousMatrix cMe;
2116  vpViper850::get_cMe(cMe);
2117 
2118  cVe.buildFrom(cMe);
2119 }
2120 
2131 {
2132  try {
2134  } catch (...) {
2135  vpERROR_TRACE("catch exception ");
2136  throw;
2137  }
2138 }
2139 
2151 {
2152  try {
2153  vpColVector articularCoordinates = get_artCoord();
2154  vpViper850::get_fJe(articularCoordinates, fJe_);
2155  } catch (...) {
2156  vpERROR_TRACE("Error caught");
2157  throw;
2158  }
2159 }
2160 
2165 {
2167  return;
2168 
2169  vpColVector stop(6);
2170  stop = 0;
2171  set_artVel(stop);
2172  set_velocity(stop);
2174 }
2175 
2176 /**********************************************************************************/
2177 /**********************************************************************************/
2178 /**********************************************************************************/
2179 /**********************************************************************************/
2180 
2190 {
2191  // set scene_dir from #define VISP_SCENE_DIR if it exists
2192  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2193  std::string scene_dir_;
2194  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2195  bool sceneDirExists = false;
2196  for (size_t i = 0; i < scene_dirs.size(); i++)
2197  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2198  scene_dir_ = scene_dirs[i];
2199  sceneDirExists = true;
2200  break;
2201  }
2202  if (!sceneDirExists) {
2203  try {
2204  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2205  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2206  } catch (...) {
2207  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2208  }
2209  }
2210 
2211  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2212  if (scene_dir_.size() > FILENAME_MAX)
2213  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2214 
2215  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2216  if (full_length > FILENAME_MAX)
2217  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2218 
2219  char *name_cam = new char[full_length];
2220 
2221  strcpy(name_cam, scene_dir_.c_str());
2222  strcat(name_cam, "/camera.bnd");
2223  set_scene(name_cam, &camera, cameraFactor);
2224 
2225  if (arm_dir.size() > FILENAME_MAX)
2226  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2227  full_length = (unsigned int)arm_dir.size() + name_length;
2228  if (full_length > FILENAME_MAX)
2229  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2230 
2231  char *name_arm = new char[full_length];
2232  strcpy(name_arm, arm_dir.c_str());
2233  strcat(name_arm, "/viper850_arm1.bnd");
2234  set_scene(name_arm, robotArms, 1.0);
2235  strcpy(name_arm, arm_dir.c_str());
2236  strcat(name_arm, "/viper850_arm2.bnd");
2237  set_scene(name_arm, robotArms + 1, 1.0);
2238  strcpy(name_arm, arm_dir.c_str());
2239  strcat(name_arm, "/viper850_arm3.bnd");
2240  set_scene(name_arm, robotArms + 2, 1.0);
2241  strcpy(name_arm, arm_dir.c_str());
2242  strcat(name_arm, "/viper850_arm4.bnd");
2243  set_scene(name_arm, robotArms + 3, 1.0);
2244  strcpy(name_arm, arm_dir.c_str());
2245  strcat(name_arm, "/viper850_arm5.bnd");
2246  set_scene(name_arm, robotArms + 4, 1.0);
2247  strcpy(name_arm, arm_dir.c_str());
2248  strcat(name_arm, "/viper850_arm6.bnd");
2249  set_scene(name_arm, robotArms + 5, 1.0);
2250 
2251  // set_scene("./arm2.bnd", robotArms+1, 1.0);
2252  // set_scene("./arm3.bnd", robotArms+2, 1.0);
2253  // set_scene("./arm4.bnd", robotArms+3, 1.0);
2254  // set_scene("./arm5.bnd", robotArms+4, 1.0);
2255  // set_scene("./arm6.bnd", robotArms+5, 1.0);
2256 
2257  add_rfstack(IS_BACK);
2258 
2259  add_vwstack("start", "depth", 0.0, 100.0);
2260  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2261  add_vwstack("start", "type", PERSPECTIVE);
2262  //
2263  // sceneInitialized = true;
2264  // displayObject = true;
2265  displayCamera = true;
2266 
2267  delete[] name_cam;
2268  delete[] name_arm;
2269 }
2270 
2272 {
2273  bool changed = false;
2274  vpHomogeneousMatrix displacement = navigation(I_, changed);
2275 
2276  // if (displacement[2][3] != 0)
2277  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2278  camMf2 = camMf2 * displacement;
2279 
2280  f2Mf = camMf2.inverse() * camMf;
2281 
2282  camMf = camMf2 * displacement * f2Mf;
2283 
2284  double u;
2285  double v;
2286  // if(px_ext != 1 && py_ext != 1)
2287  // we assume px_ext and py_ext > 0
2288  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2289  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2290  u = (double)I_.getWidth() / (2 * px_ext);
2291  v = (double)I_.getHeight() / (2 * py_ext);
2292  } else {
2293  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2294  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2295  }
2296 
2297  float w44o[4][4], w44cext[4][4], x, y, z;
2298 
2299  vp2jlc_matrix(camMf.inverse(), w44cext);
2300 
2301  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2302  x = w44cext[2][0] + w44cext[3][0];
2303  y = w44cext[2][1] + w44cext[3][1];
2304  z = w44cext[2][2] + w44cext[3][2];
2305  add_vwstack("start", "vrp", x, y, z);
2306  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2307  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2308  add_vwstack("start", "window", -u, u, -v, v);
2309 
2310  vpHomogeneousMatrix fMit[8];
2311  get_fMi(fMit);
2312 
2313  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2314  display_scene(w44o, robotArms[0], I_, curColor);
2315 
2316  vp2jlc_matrix(fMit[0], w44o);
2317  display_scene(w44o, robotArms[1], I_, curColor);
2318 
2319  vp2jlc_matrix(fMit[1], w44o);
2320  display_scene(w44o, robotArms[2], I_, curColor);
2321 
2322  vp2jlc_matrix(fMit[2], w44o);
2323  display_scene(w44o, robotArms[3], I_, curColor);
2324 
2325  vp2jlc_matrix(fMit[3], w44o);
2326  display_scene(w44o, robotArms[4], I_, curColor);
2327 
2328  vp2jlc_matrix(fMit[6], w44o);
2329  display_scene(w44o, robotArms[5], I_, curColor);
2330 
2331  if (displayCamera) {
2332  vpHomogeneousMatrix cMe;
2333  get_cMe(cMe);
2334  cMe = cMe.inverse();
2335  cMe = fMit[6] * cMe;
2336  vp2jlc_matrix(cMe, w44o);
2337  display_scene(w44o, camera, I_, camColor);
2338  }
2339 
2340  if (displayObject) {
2341  vp2jlc_matrix(fMo, w44o);
2342  display_scene(w44o, scene, I_, curColor);
2343  }
2344 }
2345 
2363 {
2364  vpColVector stop(6);
2365  bool status = true;
2366  stop = 0;
2367  set_artVel(stop);
2368  set_velocity(stop);
2369  vpHomogeneousMatrix fMc_;
2370  fMc_ = fMo * cMo_.inverse();
2371 
2372  vpColVector articularCoordinates = get_artCoord();
2373  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2374 
2375  if (nbSol == 0) {
2376  status = false;
2377  vpERROR_TRACE("Positionning error. Position unreachable");
2378  }
2379 
2380  if (verbose_)
2381  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2382 
2383  set_artCoord(articularCoordinates);
2384 
2385  compute_fMi();
2386 
2387  return status;
2388 }
2389 
2404 {
2405  vpColVector stop(6);
2406  stop = 0;
2407  set_artVel(stop);
2408  set_velocity(stop);
2409  vpHomogeneousMatrix fMit[8];
2410  get_fMi(fMit);
2411  fMo = fMit[7] * cMo_;
2412 }
2413 
2414 #elif !defined(VISP_BUILD_SHARED_LIBS)
2415 // Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2416 // has no symbols
2417 void dummy_vpSimulatorViper850(){};
2418 #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:104
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:970
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
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.
static bool checkDirectory(const char *dirname)
Definition: vpIoTools.cpp:467
void get_fMi(vpHomogeneousMatrix *fMit)
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:137
unsigned int getWidth() const
Definition: vpImage.h:239
void get_cMe(vpHomogeneousMatrix &cMe)
double getSamplingTime() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void * launcher(void *arg)
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:250
void getExternalImage(vpImage< vpRGBa > &I)
static std::string getenv(const char *env)
Definition: vpIoTools.cpp:362
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:251
static const vpColor none
Definition: vpColor.h:192
Initialize the position controller.
Definition: vpRobot.h:68
error that can be emited by ViSP classes.
Definition: vpException.h:71
void track(const vpHomogeneousMatrix &cMo)
void get_fJe(vpMatrix &fJe)
vpControlFrameType
Definition: vpRobot.h:75
vpRxyzVector erc
Definition: vpViper.h:160
double get_py() const
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:431
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:122
static const vpColor green
Definition: vpColor.h:183
vpHomogeneousMatrix fMo
double a2
for joint 2
Definition: vpViper.h:164
static void flush(const vpImage< unsigned char > &I)
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:273
static bool savePosFile(const std::string &filename, const vpColVector &q)
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
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:134
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
Implementation of a rotation matrix and operations on such kind of matrices.
double d1
for joint 1
Definition: vpViper.h:163
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
double a1
Definition: vpViper.h:163
void move(const char *filename)
Initialize the velocity controller.
Definition: vpRobot.h:67
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:69
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_)
vpRowVector t() const
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.
vpHomogeneousMatrix getExternalCameraPosition() const
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement)
Modelisation of the ADEPT Viper 850 robot.
Definition: vpViper850.h:103
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:600
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:429
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:161
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1771
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:145
static bool readPosFile(const std::string &filename, vpColVector &q)
unsigned int getRows() const
Definition: vpArray2D.h:156
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:563
void set_velocity(const vpColVector &vel)
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
double get_px() const
static double rad(double deg)
Definition: vpMath.h:102
void setExternalCameraParameters(const vpCameraParameters &cam)
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:172
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1159
This class aims to be a basis used to create all the simulators of robots.
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
double sumSquare() const
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:922
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:95
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
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))
void setCameraParameters(const vpCameraParameters &cam)
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
unsigned int getHeight() const
Definition: vpImage.h:178
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:156
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:207
double d4
for joint 4
Definition: vpViper.h:166
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:1932
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)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)
void get_cVe(vpVelocityTwistMatrix &cVe)
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:177
VISP_EXPORT double getMinTimeForUsleepCall()
Definition: vpTime.cpp:80
void set_artVel(const vpColVector &vel)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
static const double defaultPositioningVelocity
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
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:123
void get_eJe(vpMatrix &eJe)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244
double d6
for joint 6
Definition: vpViper.h:167
vpColVector joint_min
Definition: vpViper.h:173