Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpSimulatorViper850.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/robot/vpSimulatorViper850.h>
37 
38 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
39 
40 #include <cmath> // std::fabs
41 #include <limits> // numeric_limits
42 #include <string>
43 #include <visp3/core/vpImagePoint.h>
44 #include <visp3/core/vpIoTools.h>
45 #include <visp3/core/vpMeterPixelConversion.h>
46 #include <visp3/core/vpPoint.h>
47 #include <visp3/core/vpTime.h>
48 
49 #include "../wireframe-simulator/vpBound.h"
50 #include "../wireframe-simulator/vpRfstack.h"
51 #include "../wireframe-simulator/vpScene.h"
52 #include "../wireframe-simulator/vpVwstack.h"
53 
55 
60  : vpRobotWireFrameSimulator(), vpViper850(), q_prev_getdis(), first_time_getdis(true),
61  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
62 {
63  init();
64  initDisplay();
65 
67 
68  m_thread = new std::thread(&launcher, std::ref(*this));
69 
70  compute_fMi();
71 }
72 
80  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
81  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
82 {
83  init();
84  initDisplay();
85 
87 
88  m_thread = new std::thread(&launcher, std::ref(*this));
89 
90  compute_fMi();
91 }
92 
97 {
98  m_mutex_robotStop.lock();
99  robotStop = true;
100  m_mutex_robotStop.unlock();
101 
102  m_thread->join();
103 
104  if (robotArms != nullptr) {
105  // free_Bound_scene (&(camera));
106  for (int i = 0; i < 6; i++)
107  free_Bound_scene(&(robotArms[i]));
108  }
109 
110  delete[] robotArms;
111  delete[] fMi;
112  delete m_thread;
113 }
114 
124 {
125  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
126  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
127  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
128  bool armDirExists = false;
129  for (size_t i = 0; i < arm_dirs.size(); i++)
130  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
131  arm_dir = arm_dirs[i];
132  armDirExists = true;
133  break;
134  }
135  if (!armDirExists) {
136  try {
137  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
138  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
139  }
140  catch (...) {
141  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
142  }
143  }
144 
146  toolCustom = false;
147 
148  size_fMi = 8;
149  fMi = new vpHomogeneousMatrix[8];
152 
153  zeroPos.resize(njoint);
154  zeroPos = 0;
155  zeroPos[1] = -M_PI / 2;
156  zeroPos[2] = M_PI;
157  reposPos.resize(njoint);
158  reposPos = 0;
159  reposPos[1] = -M_PI / 2;
160  reposPos[2] = M_PI;
161  reposPos[4] = M_PI / 2;
162 
163  artCoord = reposPos;
164  artVel = 0;
165 
166  q_prev_getdis.resize(njoint);
167  q_prev_getdis = 0;
168  first_time_getdis = true;
169 
170  positioningVelocity = defaultPositioningVelocity;
171 
174 
175  // Software joint limits in radians
176  // joint_min.resize(njoint);
177  joint_min[0] = vpMath::rad(-50);
178  joint_min[1] = vpMath::rad(-135);
179  joint_min[2] = vpMath::rad(-40);
180  joint_min[3] = vpMath::rad(-190);
181  joint_min[4] = vpMath::rad(-110);
182  joint_min[5] = vpMath::rad(-184);
183  // joint_max.resize(njoint);
184  joint_max[0] = vpMath::rad(50);
185  joint_max[1] = vpMath::rad(-40);
186  joint_max[2] = vpMath::rad(215);
187  joint_max[3] = vpMath::rad(190);
188  joint_max[4] = vpMath::rad(110);
189  joint_max[5] = vpMath::rad(184);
190 }
191 
196 {
197  robotArms = nullptr;
198  robotArms = new Bound_scene[6];
199  initArms();
200  setExternalCameraPosition(vpHomogeneousMatrix(0.0, 0.5, 1.5, vpMath::rad(90), 0, 0));
201  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
203  vpCameraParameters tmp;
204  getCameraParameters(tmp, 640, 480);
205  px_int = tmp.get_px();
206  py_int = tmp.get_py();
207  sceneInitialized = true;
208 }
209 
226 {
227  this->projModel = proj_model;
228 
229  // Use here default values of the robot constant parameters.
230  switch (tool) {
232  erc[0] = vpMath::rad(0.07); // rx
233  erc[1] = vpMath::rad(2.76); // ry
234  erc[2] = vpMath::rad(-91.50); // rz
235  etc[0] = -0.0453; // tx
236  etc[1] = 0.0005; // ty
237  etc[2] = 0.0728; // tz
238 
239  setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
240  break;
241  }
243  erc[0] = vpMath::rad(0.1527764261); // rx
244  erc[1] = vpMath::rad(1.285092455); // ry
245  erc[2] = vpMath::rad(-90.75777848); // rz
246  etc[0] = -0.04558630174; // tx
247  etc[1] = -0.00134326752; // ty
248  etc[2] = 0.001000828017; // tz
249 
250  setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
251  break;
252  }
256  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
257  break;
258  }
259  }
260 
261  vpRotationMatrix eRc(erc);
262 
263  m_mutex_eMc.lock();
264  this->eMc.buildFrom(etc, eRc);
265  m_mutex_eMc.unlock();
266 
267  setToolType(tool);
268  return;
269 }
270 
281 void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
282  const unsigned int &image_height)
283 {
284  if (toolCustom) {
285  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
286  }
287  // Set default parameters
288  switch (getToolType()) {
290  // Set default intrinsic camera parameters for 640x480 images
291  if (image_width == 640 && image_height == 480) {
292  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
293  << std::endl;
294  cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
295  }
296  else {
297  vpTRACE("Cannot get default intrinsic camera parameters for this image "
298  "resolution");
299  }
300  break;
301  }
303  // Set default intrinsic camera parameters for 640x480 images
304  if (image_width == 640 && image_height == 480) {
305  std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
306  << std::endl;
307  cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
308  }
309  else {
310  vpTRACE("Cannot get default intrinsic camera parameters for this image "
311  "resolution");
312  }
313  break;
314  }
318  std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
319  break;
320  }
321  }
322  return;
323 }
324 
334 {
335  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
336 }
337 
347 {
348  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
349 }
350 
357 {
358  px_int = cam.get_px();
359  py_int = cam.get_py();
360  toolCustom = true;
361 }
362 
368 {
369  double tcur_1 = tcur; // temporary variable used to store the last time
370  // since the last command
371 
372  bool stop = false;
373  bool setVelocityCalled_ = false;
374  while (!stop) {
375  // Get current time
376  tprev = tcur_1;
378 
380  setVelocityCalled_ = setVelocityCalled;
381  m_mutex_setVelocityCalled.unlock();
382 
383  if (setVelocityCalled_ || !constantSamplingTimeMode) {
385  setVelocityCalled = false;
386  m_mutex_setVelocityCalled.unlock();
388 
389  double ellapsedTime = (tcur - tprev) * 1e-3;
390  if (constantSamplingTimeMode) { // if we want a constant velocity, we
391  // force the ellapsed time to the given
392  // samplingTime
393  ellapsedTime = getSamplingTime(); // in second
394  }
395 
396  vpColVector articularCoordinates = get_artCoord();
397  vpColVector articularVelocities = get_artVel();
398 
399  if (jointLimit) {
400  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
401  if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
402  if (verbose_) {
403  std::cout << "Joint " << jointLimitArt - 1
404  << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
405  << " < " << vpMath::deg(joint_max[jointLimitArt - 1]) << std::endl;
406  }
407  articularVelocities = 0.0;
408  }
409  else
410  jointLimit = false;
411  }
412 
413  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
414  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
415  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
416  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
417  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
418  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
419 
420  int jl = isInJointLimit();
421 
422  if (jl != 0 && jointLimit == false) {
423  if (jl < 0)
424  ellapsedTime = (joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
425  (articularVelocities[(unsigned int)(-jl - 1)]);
426  else
427  ellapsedTime = (joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
428  (articularVelocities[(unsigned int)(jl - 1)]);
429 
430  for (unsigned int i = 0; i < 6; i++)
431  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
432 
433  jointLimit = true;
434  jointLimitArt = (unsigned int)fabs((double)jl);
435  }
436 
437  set_artCoord(articularCoordinates);
438  set_artVel(articularVelocities);
439 
440  compute_fMi();
441 
442  if (displayAllowed) {
446  }
447 
448  if (displayType == MODEL_3D && displayAllowed) {
449  while (get_displayBusy()) {
450  vpTime::wait(2);
451  }
453  set_displayBusy(false);
454  }
455 
456  if (displayType == MODEL_DH && displayAllowed) {
457  vpHomogeneousMatrix fMit[8];
458  get_fMi(fMit);
459 
460  // vpDisplay::displayFrame(I,getExternalCameraPosition
461  // ()*fMi[6],cameraParam,0.2,vpColor::none);
462 
463  vpImagePoint iP, iP_1;
464  vpPoint pt(0, 0, 0);
465 
468  pt.track(getExternalCameraPosition() * fMit[0]);
471  for (int k = 1; k < 7; k++) {
472  pt.track(getExternalCameraPosition() * fMit[k - 1]);
474 
475  pt.track(getExternalCameraPosition() * fMit[k]);
477 
479  }
481  thickness_);
482  }
483 
485 
486  vpTime::wait(tcur, 1000 * getSamplingTime());
487  tcur_1 = tcur;
488  }
489  else {
491  }
492  m_mutex_robotStop.lock();
493  stop = robotStop;
494  m_mutex_robotStop.unlock();
495  }
496 }
497 
511 {
512  // vpColVector q = get_artCoord();
513  vpColVector q(6); //; = get_artCoord();
514  q = get_artCoord();
515 
516  vpHomogeneousMatrix fMit[8];
517 
518  double q1 = q[0];
519  double q2 = q[1];
520  double q3 = q[2];
521  double q4 = q[3];
522  double q5 = q[4];
523  double q6 = q[5];
524 
525  double c1 = cos(q1);
526  double s1 = sin(q1);
527  double c2 = cos(q2);
528  double s2 = sin(q2);
529  double c23 = cos(q2 + q3);
530  double s23 = sin(q2 + q3);
531  double c4 = cos(q4);
532  double s4 = sin(q4);
533  double c5 = cos(q5);
534  double s5 = sin(q5);
535  double c6 = cos(q6);
536  double s6 = sin(q6);
537 
538  fMit[0][0][0] = c1;
539  fMit[0][1][0] = s1;
540  fMit[0][2][0] = 0;
541  fMit[0][0][1] = 0;
542  fMit[0][1][1] = 0;
543  fMit[0][2][1] = -1;
544  fMit[0][0][2] = -s1;
545  fMit[0][1][2] = c1;
546  fMit[0][2][2] = 0;
547  fMit[0][0][3] = a1 * c1;
548  fMit[0][1][3] = a1 * s1;
549  fMit[0][2][3] = d1;
550 
551  fMit[1][0][0] = c1 * c2;
552  fMit[1][1][0] = s1 * c2;
553  fMit[1][2][0] = -s2;
554  fMit[1][0][1] = -c1 * s2;
555  fMit[1][1][1] = -s1 * s2;
556  fMit[1][2][1] = -c2;
557  fMit[1][0][2] = -s1;
558  fMit[1][1][2] = c1;
559  fMit[1][2][2] = 0;
560  fMit[1][0][3] = c1 * (a2 * c2 + a1);
561  fMit[1][1][3] = s1 * (a2 * c2 + a1);
562  fMit[1][2][3] = d1 - a2 * s2;
563 
564  double quickcomp1 = a3 * c23 - a2 * c2 - a1;
565 
566  fMit[2][0][0] = -c1 * c23;
567  fMit[2][1][0] = -s1 * c23;
568  fMit[2][2][0] = s23;
569  fMit[2][0][1] = s1;
570  fMit[2][1][1] = -c1;
571  fMit[2][2][1] = 0;
572  fMit[2][0][2] = c1 * s23;
573  fMit[2][1][2] = s1 * s23;
574  fMit[2][2][2] = c23;
575  fMit[2][0][3] = -c1 * quickcomp1;
576  fMit[2][1][3] = -s1 * quickcomp1;
577  fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
578 
579  double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
580  double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
581 
582  fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
583  fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
584  fMit[3][2][0] = s23 * c4;
585  fMit[3][0][1] = c1 * s23;
586  fMit[3][1][1] = s1 * s23;
587  fMit[3][2][1] = c23;
588  fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
589  fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
590  fMit[3][2][2] = s23 * s4;
591  fMit[3][0][3] = quickcomp2;
592  fMit[3][1][3] = quickcomp3;
593  fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
594 
595  fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
596  fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
597  fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
598  fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
599  fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
600  fMit[4][2][1] = -s23 * s4;
601  fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
602  fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
603  fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
604  fMit[4][0][3] = quickcomp2;
605  fMit[4][1][3] = quickcomp3;
606  fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
607 
608  fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
609  fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
610  fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
611  fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
612  fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
613  fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
614  fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
615  fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
616  fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
617  fMit[5][0][3] = quickcomp2;
618  fMit[5][1][3] = quickcomp3;
619  fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
620 
621  fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
622  fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
623  fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
624  fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
625  fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
626  fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
627  fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
628  fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
629  fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
630  fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
631  fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
632  fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
633 
634  // vpHomogeneousMatrix cMe;
635  // get_cMe(cMe);
636  // cMe = cMe.inverse();
637  // fMit[7] = fMit[6] * cMe;
638  m_mutex_eMc.lock();
639  vpViper::get_fMc(q, fMit[7]);
640  m_mutex_eMc.unlock();
641 
642  m_mutex_fMi.lock();
643  for (int i = 0; i < 8; i++) {
644  fMi[i] = fMit[i];
645  }
646  m_mutex_fMi.unlock();
647 }
648 
655 {
656  switch (newState) {
657  case vpRobot::STATE_STOP: {
658  // Start primitive STOP only if the current state is Velocity
660  stopMotion();
661  }
662  break;
663  }
666  std::cout << "Change the control mode from velocity to position control.\n";
667  stopMotion();
668  }
669  else {
670  // std::cout << "Change the control mode from stop to position
671  // control.\n";
672  }
673  break;
674  }
677  std::cout << "Change the control mode from stop to velocity control.\n";
678  }
679  break;
680  }
682  default:
683  break;
684  }
685 
686  return vpRobot::setRobotState(newState);
687 }
688 
763 {
765  vpERROR_TRACE("Cannot send a velocity to the robot "
766  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
768  "Cannot send a velocity to the robot "
769  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
770  }
771 
772  vpColVector vel_sat(6);
773 
774  double scale_sat = 1;
775  double vel_trans_max = getMaxTranslationVelocity();
776  double vel_rot_max = getMaxRotationVelocity();
777 
778  double vel_abs; // Absolute value
779 
780  // Velocity saturation
781  switch (frame) {
782  // saturation in cartesian space
785  if (vel.getRows() != 6) {
786  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
787  throw;
788  }
789 
790  for (unsigned int i = 0; i < 3; ++i) {
791  vel_abs = fabs(vel[i]);
792  if (vel_abs > vel_trans_max && !jointLimit) {
793  vel_trans_max = vel_abs;
794  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
795  "(axis nr. %d).",
796  vel[i], i + 1);
797  }
798 
799  vel_abs = fabs(vel[i + 3]);
800  if (vel_abs > vel_rot_max && !jointLimit) {
801  vel_rot_max = vel_abs;
802  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
803  "(axis nr. %d).",
804  vel[i + 3], i + 4);
805  }
806  }
807 
808  double scale_trans_sat = 1;
809  double scale_rot_sat = 1;
810  if (vel_trans_max > getMaxTranslationVelocity())
811  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
812 
813  if (vel_rot_max > getMaxRotationVelocity())
814  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
815 
816  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
817  if (scale_trans_sat < scale_rot_sat)
818  scale_sat = scale_trans_sat;
819  else
820  scale_sat = scale_rot_sat;
821  }
822  break;
823  }
824 
825  // saturation in joint space
827  if (vel.getRows() != 6) {
828  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
829  throw;
830  }
831  for (unsigned int i = 0; i < 6; ++i) {
832  vel_abs = fabs(vel[i]);
833  if (vel_abs > vel_rot_max && !jointLimit) {
834  vel_rot_max = vel_abs;
835  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
836  "(axis nr. %d).",
837  vel[i], i + 1);
838  }
839  }
840  double scale_rot_sat = 1;
841  if (vel_rot_max > getMaxRotationVelocity())
842  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
843  if (scale_rot_sat < 1)
844  scale_sat = scale_rot_sat;
845  break;
846  }
848  case vpRobot::MIXT_FRAME: {
849  break;
850  }
851  }
852 
853  set_velocity(vel * scale_sat);
854 
855  m_mutex_frame.lock();
856  setRobotFrame(frame);
857  m_mutex_frame.unlock();
858 
860  setVelocityCalled = true;
861  m_mutex_setVelocityCalled.unlock();
862 }
863 
868 {
870 
871  m_mutex_frame.lock();
872  frame = getRobotFrame();
873  m_mutex_frame.unlock();
874 
875  double vel_rot_max = getMaxRotationVelocity();
876 
877  vpColVector articularCoordinates = get_artCoord();
878  vpColVector velocityframe = get_velocity();
879  vpColVector articularVelocity;
880 
881  switch (frame) {
882  case vpRobot::CAMERA_FRAME: {
883  vpMatrix eJe_;
885  vpViper850::get_eJe(articularCoordinates, eJe_);
886  eJe_ = eJe_.pseudoInverse();
888  singularityTest(articularCoordinates, eJe_);
889  articularVelocity = eJe_ * eVc * velocityframe;
890  set_artVel(articularVelocity);
891  break;
892  }
894  vpMatrix fJe_;
895  vpViper850::get_fJe(articularCoordinates, fJe_);
896  fJe_ = fJe_.pseudoInverse();
898  singularityTest(articularCoordinates, fJe_);
899  articularVelocity = fJe_ * velocityframe;
900  set_artVel(articularVelocity);
901  break;
902  }
904  articularVelocity = velocityframe;
905  set_artVel(articularVelocity);
906  break;
907  }
909  case vpRobot::MIXT_FRAME: {
910  break;
911  }
912  }
913 
914  switch (frame) {
917  for (unsigned int i = 0; i < 6; ++i) {
918  double vel_abs = fabs(articularVelocity[i]);
919  if (vel_abs > vel_rot_max && !jointLimit) {
920  vel_rot_max = vel_abs;
921  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
922  "(axis nr. %d).",
923  articularVelocity[i], i + 1);
924  }
925  }
926  double scale_rot_sat = 1;
927  double scale_sat = 1;
928 
929  if (vel_rot_max > getMaxRotationVelocity())
930  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
931  if (scale_rot_sat < 1)
932  scale_sat = scale_rot_sat;
933 
934  set_artVel(articularVelocity * scale_sat);
935  break;
936  }
939  case vpRobot::MIXT_FRAME: {
940  break;
941  }
942  }
943 }
944 
991 {
992  vel.resize(6);
993 
994  vpColVector articularCoordinates = get_artCoord();
995  vpColVector articularVelocity = get_artVel();
996 
997  switch (frame) {
998  case vpRobot::CAMERA_FRAME: {
999  vpMatrix eJe_;
1001  vpViper850::get_eJe(articularCoordinates, eJe_);
1002  vel = cVe * eJe_ * articularVelocity;
1003  break;
1004  }
1005  case vpRobot::ARTICULAR_FRAME: {
1006  vel = articularVelocity;
1007  break;
1008  }
1009  case vpRobot::REFERENCE_FRAME: {
1010  vpMatrix fJe_;
1011  vpViper850::get_fJe(articularCoordinates, fJe_);
1012  vel = fJe_ * articularVelocity;
1013  break;
1014  }
1016  case vpRobot::MIXT_FRAME: {
1017  break;
1018  }
1019  default: {
1020  vpERROR_TRACE("Error in spec of vpRobot. "
1021  "Case not taken in account.");
1022  return;
1023  }
1024  }
1025 }
1026 
1044 {
1045  timestamp = vpTime::measureTimeSecond();
1046  getVelocity(frame, vel);
1047 }
1048 
1091 {
1092  vpColVector vel(6);
1093  getVelocity(frame, vel);
1094 
1095  return vel;
1096 }
1097 
1111 {
1112  timestamp = vpTime::measureTimeSecond();
1113  vpColVector vel(6);
1114  getVelocity(frame, vel);
1115 
1116  return vel;
1117 }
1118 
1120 {
1121  double vel_rot_max = getMaxRotationVelocity();
1122  double velmax = fabs(q[0]);
1123  for (unsigned int i = 1; i < 6; i++) {
1124  if (velmax < fabs(q[i]))
1125  velmax = fabs(q[i]);
1126  }
1127 
1128  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1129  q = q * alpha;
1130 }
1131 
1207 {
1209  vpERROR_TRACE("Robot was not in position-based control\n"
1210  "Modification of the robot state");
1211  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1212  }
1213 
1214  vpColVector articularCoordinates = get_artCoord();
1215 
1216  vpColVector error(6);
1217  double errsqr = 0;
1218  switch (frame) {
1219  case vpRobot::CAMERA_FRAME: {
1220  unsigned int nbSol;
1221  vpColVector qdes(6);
1222 
1223  vpTranslationVector txyz;
1224  vpRxyzVector rxyz;
1225  for (unsigned int i = 0; i < 3; i++) {
1226  txyz[i] = q[i];
1227  rxyz[i] = q[i + 3];
1228  }
1229 
1230  vpRotationMatrix cRc2(rxyz);
1231  vpHomogeneousMatrix cMc2(txyz, cRc2);
1232 
1233  vpHomogeneousMatrix fMc_;
1234  vpViper850::get_fMc(articularCoordinates, fMc_);
1235 
1236  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1237 
1238  do {
1239  articularCoordinates = get_artCoord();
1240  qdes = articularCoordinates;
1241  nbSol = getInverseKinematics(fMc2, qdes, verbose_);
1243  setVelocityCalled = true;
1244  m_mutex_setVelocityCalled.unlock();
1245  if (nbSol > 0) {
1246  error = qdes - articularCoordinates;
1247  errsqr = error.sumSquare();
1248  // findHighestPositioningSpeed(error);
1249  set_artVel(error);
1250  if (errsqr < 1e-4) {
1251  set_artCoord(qdes);
1252  error = 0;
1253  set_artVel(error);
1254  set_velocity(error);
1255  break;
1256  }
1257  }
1258  else {
1259  vpERROR_TRACE("Positioning error.");
1260  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1261  }
1262  } while (errsqr > 1e-8 && nbSol > 0);
1263 
1264  break;
1265  }
1266 
1267  case vpRobot::ARTICULAR_FRAME: {
1268  do {
1269  articularCoordinates = get_artCoord();
1270  error = q - articularCoordinates;
1271  errsqr = error.sumSquare();
1272  // findHighestPositioningSpeed(error);
1273  set_artVel(error);
1275  setVelocityCalled = true;
1276  m_mutex_setVelocityCalled.unlock();
1277  if (errsqr < 1e-4) {
1278  set_artCoord(q);
1279  error = 0;
1280  set_artVel(error);
1281  set_velocity(error);
1282  break;
1283  }
1284  } while (errsqr > 1e-8);
1285  break;
1286  }
1287 
1288  case vpRobot::REFERENCE_FRAME: {
1289  unsigned int nbSol;
1290  vpColVector qdes(6);
1291 
1292  vpTranslationVector txyz;
1293  vpRxyzVector rxyz;
1294  for (unsigned int i = 0; i < 3; i++) {
1295  txyz[i] = q[i];
1296  rxyz[i] = q[i + 3];
1297  }
1298 
1299  vpRotationMatrix fRc(rxyz);
1300  vpHomogeneousMatrix fMc_(txyz, fRc);
1301 
1302  do {
1303  articularCoordinates = get_artCoord();
1304  qdes = articularCoordinates;
1305  nbSol = getInverseKinematics(fMc_, qdes, verbose_);
1306  if (nbSol > 0) {
1307  error = qdes - articularCoordinates;
1308  errsqr = error.sumSquare();
1309  // findHighestPositioningSpeed(error);
1310  set_artVel(error);
1312  setVelocityCalled = true;
1313  m_mutex_setVelocityCalled.unlock();
1314  if (errsqr < 1e-4) {
1315  set_artCoord(qdes);
1316  error = 0;
1317  set_artVel(error);
1318  set_velocity(error);
1319  break;
1320  }
1321  }
1322  else
1323  vpERROR_TRACE("Positioning error. Position unreachable");
1324  } while (errsqr > 1e-8 && nbSol > 0);
1325  break;
1326  }
1327  case vpRobot::MIXT_FRAME: {
1328  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1329  "Mixt frame not implemented.");
1330  }
1332  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1333  "End-effector frame not implemented.");
1334  }
1335  }
1336 }
1337 
1400 void vpSimulatorViper850::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1401  double pos4, double pos5, double pos6)
1402 {
1403  try {
1404  vpColVector position(6);
1405  position[0] = pos1;
1406  position[1] = pos2;
1407  position[2] = pos3;
1408  position[3] = pos4;
1409  position[4] = pos5;
1410  position[5] = pos6;
1411 
1412  setPosition(frame, position);
1413  }
1414  catch (...) {
1415  vpERROR_TRACE("Error caught");
1416  throw;
1417  }
1418 }
1419 
1454 void vpSimulatorViper850::setPosition(const char *filename)
1455 {
1456  vpColVector q;
1457  bool ret;
1458 
1459  ret = this->readPosFile(filename, q);
1460 
1461  if (ret == false) {
1462  vpERROR_TRACE("Bad position in \"%s\"", filename);
1463  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1464  }
1467 }
1468 
1529 {
1530  q.resize(6);
1531 
1532  switch (frame) {
1533  case vpRobot::CAMERA_FRAME: {
1534  q = 0;
1535  break;
1536  }
1537 
1538  case vpRobot::ARTICULAR_FRAME: {
1539  q = get_artCoord();
1540  break;
1541  }
1542 
1543  case vpRobot::REFERENCE_FRAME: {
1544  vpHomogeneousMatrix fMc_;
1545  vpViper::get_fMc(get_artCoord(), fMc_);
1546 
1547  vpRotationMatrix fRc;
1548  fMc_.extract(fRc);
1549  vpRxyzVector rxyz(fRc);
1550 
1551  vpTranslationVector txyz;
1552  fMc_.extract(txyz);
1553 
1554  for (unsigned int i = 0; i < 3; i++) {
1555  q[i] = txyz[i];
1556  q[i + 3] = rxyz[i];
1557  }
1558  break;
1559  }
1560 
1561  case vpRobot::MIXT_FRAME: {
1562  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1563  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1564  "Mixt frame not implemented.");
1565  }
1567  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1568  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1569  "End-effector frame not implemented.");
1570  }
1571  }
1572 }
1573 
1601 {
1602  timestamp = vpTime::measureTimeSecond();
1603  getPosition(frame, q);
1604 }
1605 
1618 {
1619  vpColVector posRxyz;
1620  // recupere position en Rxyz
1621  this->getPosition(frame, posRxyz);
1622 
1623  // recupere le vecteur thetaU correspondant
1624  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1625 
1626  // remplit le vpPoseVector avec translation et rotation ThetaU
1627  for (unsigned int j = 0; j < 3; j++) {
1628  position[j] = posRxyz[j];
1629  position[j + 3] = RtuVect[j];
1630  }
1631 }
1632 
1645  double &timestamp)
1646 {
1647  timestamp = vpTime::measureTimeSecond();
1648  getPosition(frame, position);
1649 }
1650 
1659 void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1660 {
1661  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1662  vpTRACE("Joint limit vector has not a size of 6 !");
1663  return;
1664  }
1665 
1666  joint_min[0] = limitMin[0];
1667  joint_min[1] = limitMin[1];
1668  joint_min[2] = limitMin[2];
1669  joint_min[3] = limitMin[3];
1670  joint_min[4] = limitMin[4];
1671  joint_min[5] = limitMin[5];
1672 
1673  joint_max[0] = limitMax[0];
1674  joint_max[1] = limitMax[1];
1675  joint_max[2] = limitMax[2];
1676  joint_max[3] = limitMax[3];
1677  joint_max[4] = limitMax[4];
1678  joint_max[5] = limitMax[5];
1679 }
1680 
1687 {
1688  double q2 = q[1];
1689  double q3 = q[2];
1690  double q5 = q[4];
1691 
1692  double c2 = cos(q2);
1693  double c3 = cos(q3);
1694  double s3 = sin(q3);
1695  double c23 = cos(q2 + q3);
1696  double s23 = sin(q2 + q3);
1697  double s5 = sin(q5);
1698 
1699  bool cond1 = fabs(s5) < 1e-1;
1700  bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
1701  bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
1702 
1703  if (cond1) {
1704  J[3][0] = 0;
1705  J[5][0] = 0;
1706  J[3][1] = 0;
1707  J[5][1] = 0;
1708  J[3][2] = 0;
1709  J[5][2] = 0;
1710  J[3][3] = 0;
1711  J[5][3] = 0;
1712  J[3][4] = 0;
1713  J[5][4] = 0;
1714  J[3][5] = 0;
1715  J[5][5] = 0;
1716  return true;
1717  }
1718 
1719  if (cond2) {
1720  J[1][0] = 0;
1721  J[2][0] = 0;
1722  J[3][0] = 0;
1723  J[4][0] = 0;
1724  J[5][0] = 0;
1725  J[1][1] = 0;
1726  J[2][1] = 0;
1727  J[3][1] = 0;
1728  J[4][1] = 0;
1729  J[5][1] = 0;
1730  J[1][2] = 0;
1731  J[2][2] = 0;
1732  J[3][2] = 0;
1733  J[4][2] = 0;
1734  J[5][2] = 0;
1735  return true;
1736  }
1737 
1738  if (cond3) {
1739  J[0][0] = 0;
1740  J[3][0] = 0;
1741  J[4][0] = 0;
1742  J[5][0] = 0;
1743  J[0][1] = 0;
1744  J[3][1] = 0;
1745  J[4][1] = 0;
1746  J[5][1] = 0;
1747  }
1748 
1749  return false;
1750 }
1751 
1756 {
1757  int artNumb = 0;
1758  double diff = 0;
1759  double difft = 0;
1760 
1761  vpColVector articularCoordinates = get_artCoord();
1762 
1763  for (unsigned int i = 0; i < 6; i++) {
1764  if (articularCoordinates[i] <= joint_min[i]) {
1765  difft = joint_min[i] - articularCoordinates[i];
1766  if (difft > diff) {
1767  diff = difft;
1768  artNumb = -(int)i - 1;
1769  }
1770  }
1771  }
1772 
1773  for (unsigned int i = 0; i < 6; i++) {
1774  if (articularCoordinates[i] >= joint_max[i]) {
1775  difft = articularCoordinates[i] - joint_max[i];
1776  if (difft > diff) {
1777  diff = difft;
1778  artNumb = (int)(i + 1);
1779  }
1780  }
1781  }
1782 
1783  if (artNumb != 0)
1784  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1785  << std::endl;
1786 
1787  return artNumb;
1788 }
1789 
1808 {
1809  displacement.resize(6);
1810  displacement = 0;
1811  vpColVector q_cur(6);
1812 
1813  q_cur = get_artCoord();
1814 
1815  if (!first_time_getdis) {
1816  switch (frame) {
1817  case vpRobot::CAMERA_FRAME: {
1818  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1819  return;
1820  }
1821 
1822  case vpRobot::ARTICULAR_FRAME: {
1823  displacement = q_cur - q_prev_getdis;
1824  break;
1825  }
1826 
1827  case vpRobot::REFERENCE_FRAME: {
1828  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1829  return;
1830  }
1831 
1832  case vpRobot::MIXT_FRAME: {
1833  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1834  return;
1835  }
1837  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1838  return;
1839  }
1840  }
1841  }
1842  else {
1843  first_time_getdis = false;
1844  }
1845 
1846  // Memorize the joint position for the next call
1847  q_prev_getdis = q_cur;
1848 }
1849 
1911 bool vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
1912 {
1913  std::ifstream fd(filename.c_str(), std::ios::in);
1914 
1915  if (!fd.is_open()) {
1916  return false;
1917  }
1918 
1919  std::string line;
1920  std::string key("R:");
1921  std::string id("#Viper850 - Position");
1922  bool pos_found = false;
1923  int lineNum = 0;
1924 
1925  q.resize(njoint);
1926 
1927  while (std::getline(fd, line)) {
1928  lineNum++;
1929  if (lineNum == 1) {
1930  if (!(line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
1931  std::cout << "Error: this position file " << filename << " is not for Viper850 robot" << std::endl;
1932  return false;
1933  }
1934  }
1935  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1936  continue;
1937  }
1938  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1939  // check if there are at least njoint values in the line
1940  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1941  if (chain.size() < njoint + 1) // try to split with tab separator
1942  chain = vpIoTools::splitChain(line, std::string("\t"));
1943  if (chain.size() < njoint + 1)
1944  continue;
1945 
1946  std::istringstream ss(line);
1947  std::string key_;
1948  ss >> key_;
1949  for (unsigned int i = 0; i < njoint; i++)
1950  ss >> q[i];
1951  pos_found = true;
1952  break;
1953  }
1954  }
1955 
1956  // converts rotations from degrees into radians
1957  q.deg2rad();
1958 
1959  fd.close();
1960 
1961  if (!pos_found) {
1962  std::cout << "Error: unable to find a position for Viper850 robot in " << filename << std::endl;
1963  return false;
1964  }
1965 
1966  return true;
1967 }
1968 
1990 bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
1991 {
1992 
1993  FILE *fd;
1994  fd = fopen(filename.c_str(), "w");
1995  if (fd == nullptr)
1996  return false;
1997 
1998  fprintf(fd, "\
1999 #Viper - Position - Version 1.0\n\
2000 #\n\
2001 # R: A B C D E F\n\
2002 # Joint position in degrees\n\
2003 #\n\
2004 #\n\n");
2005 
2006  // Save positions in mm and deg
2007  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", vpMath::deg(q[0]), vpMath::deg(q[1]), vpMath::deg(q[2]),
2008  vpMath::deg(q[3]), vpMath::deg(q[4]), vpMath::deg(q[5]));
2009 
2010  fclose(fd);
2011  return (true);
2012 }
2013 
2021 void vpSimulatorViper850::move(const char *filename)
2022 {
2023  vpColVector q;
2024 
2025  try {
2026  this->readPosFile(filename, q);
2029  }
2030  catch (...) {
2031  throw;
2032  }
2033 }
2034 
2045 
2054 {
2055  vpHomogeneousMatrix cMe;
2056  vpViper850::get_cMe(cMe);
2057 
2058  cVe.buildFrom(cMe);
2059 }
2060 
2071 {
2072  try {
2074  }
2075  catch (...) {
2076  vpERROR_TRACE("catch exception ");
2077  throw;
2078  }
2079 }
2080 
2092 {
2093  try {
2094  vpColVector articularCoordinates = get_artCoord();
2095  vpViper850::get_fJe(articularCoordinates, fJe_);
2096  }
2097  catch (...) {
2098  vpERROR_TRACE("Error caught");
2099  throw;
2100  }
2101 }
2102 
2107 {
2109  return;
2110 
2111  vpColVector stop(6);
2112  stop = 0;
2113  set_artVel(stop);
2114  set_velocity(stop);
2116 }
2117 
2118 /**********************************************************************************/
2119 /**********************************************************************************/
2120 /**********************************************************************************/
2121 /**********************************************************************************/
2122 
2132 {
2133  // set scene_dir from #define VISP_SCENE_DIR if it exists
2134  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2135  std::string scene_dir_;
2136  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2137  bool sceneDirExists = false;
2138  for (size_t i = 0; i < scene_dirs.size(); i++)
2139  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2140  scene_dir_ = scene_dirs[i];
2141  sceneDirExists = true;
2142  break;
2143  }
2144  if (!sceneDirExists) {
2145  try {
2146  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2147  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2148  }
2149  catch (...) {
2150  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2151  }
2152  }
2153 
2154  unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
2155  if (scene_dir_.size() > FILENAME_MAX)
2156  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2157 
2158  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2159  if (full_length > FILENAME_MAX)
2160  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2161 
2162  char *name_cam = new char[full_length];
2163 
2164  strcpy(name_cam, scene_dir_.c_str());
2165  strcat(name_cam, "/camera.bnd");
2166  set_scene(name_cam, &camera, cameraFactor);
2167 
2168  if (arm_dir.size() > FILENAME_MAX)
2169  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2170  full_length = (unsigned int)arm_dir.size() + name_length;
2171  if (full_length > FILENAME_MAX)
2172  throw vpException(vpException::dimensionError, "Cannot initialize Viper850 simulator");
2173 
2174  char *name_arm = new char[full_length];
2175  strcpy(name_arm, arm_dir.c_str());
2176  strcat(name_arm, "/viper850_arm1.bnd");
2177  set_scene(name_arm, robotArms, 1.0);
2178  strcpy(name_arm, arm_dir.c_str());
2179  strcat(name_arm, "/viper850_arm2.bnd");
2180  set_scene(name_arm, robotArms + 1, 1.0);
2181  strcpy(name_arm, arm_dir.c_str());
2182  strcat(name_arm, "/viper850_arm3.bnd");
2183  set_scene(name_arm, robotArms + 2, 1.0);
2184  strcpy(name_arm, arm_dir.c_str());
2185  strcat(name_arm, "/viper850_arm4.bnd");
2186  set_scene(name_arm, robotArms + 3, 1.0);
2187  strcpy(name_arm, arm_dir.c_str());
2188  strcat(name_arm, "/viper850_arm5.bnd");
2189  set_scene(name_arm, robotArms + 4, 1.0);
2190  strcpy(name_arm, arm_dir.c_str());
2191  strcat(name_arm, "/viper850_arm6.bnd");
2192  set_scene(name_arm, robotArms + 5, 1.0);
2193 
2194  // set_scene("./arm2.bnd", robotArms+1, 1.0);
2195  // set_scene("./arm3.bnd", robotArms+2, 1.0);
2196  // set_scene("./arm4.bnd", robotArms+3, 1.0);
2197  // set_scene("./arm5.bnd", robotArms+4, 1.0);
2198  // set_scene("./arm6.bnd", robotArms+5, 1.0);
2199 
2200  add_rfstack(IS_BACK);
2201 
2202  add_vwstack("start", "depth", 0.0, 100.0);
2203  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2204  add_vwstack("start", "type", PERSPECTIVE);
2205  //
2206  // sceneInitialized = true;
2207  // displayObject = true;
2208  displayCamera = true;
2209 
2210  delete[] name_cam;
2211  delete[] name_arm;
2212 }
2213 
2215 {
2216  m_mutex_scene.lock();
2217  bool changed = false;
2218  vpHomogeneousMatrix displacement = navigation(I_, changed);
2219 
2220  // if (displacement[2][3] != 0)
2221  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2222  camMf2 = camMf2 * displacement;
2223 
2224  f2Mf = camMf2.inverse() * camMf;
2225 
2226  camMf = camMf2 * displacement * f2Mf;
2227 
2228  double u;
2229  double v;
2230  // if(px_ext != 1 && py_ext != 1)
2231  // we assume px_ext and py_ext > 0
2232  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2233  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2234  u = (double)I_.getWidth() / (2 * px_ext);
2235  v = (double)I_.getHeight() / (2 * py_ext);
2236  }
2237  else {
2238  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2239  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2240  }
2241 
2242  float w44o[4][4], w44cext[4][4], x, y, z;
2243 
2244  vp2jlc_matrix(camMf.inverse(), w44cext);
2245 
2246  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2247  x = w44cext[2][0] + w44cext[3][0];
2248  y = w44cext[2][1] + w44cext[3][1];
2249  z = w44cext[2][2] + w44cext[3][2];
2250  add_vwstack("start", "vrp", x, y, z);
2251  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2252  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2253  add_vwstack("start", "window", -u, u, -v, v);
2254 
2255  vpHomogeneousMatrix fMit[8];
2256  get_fMi(fMit);
2257 
2258  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2259  display_scene(w44o, robotArms[0], I_, curColor);
2260 
2261  vp2jlc_matrix(fMit[0], w44o);
2262  display_scene(w44o, robotArms[1], I_, curColor);
2263 
2264  vp2jlc_matrix(fMit[1], w44o);
2265  display_scene(w44o, robotArms[2], I_, curColor);
2266 
2267  vp2jlc_matrix(fMit[2], w44o);
2268  display_scene(w44o, robotArms[3], I_, curColor);
2269 
2270  vp2jlc_matrix(fMit[3], w44o);
2271  display_scene(w44o, robotArms[4], I_, curColor);
2272 
2273  vp2jlc_matrix(fMit[6], w44o);
2274  display_scene(w44o, robotArms[5], I_, curColor);
2275 
2276  if (displayCamera) {
2277  vpHomogeneousMatrix cMe;
2278  get_cMe(cMe);
2279  cMe = cMe.inverse();
2280  cMe = fMit[6] * cMe;
2281  vp2jlc_matrix(cMe, w44o);
2282  display_scene(w44o, camera, I_, camColor);
2283  }
2284 
2285  if (displayObject) {
2286  vp2jlc_matrix(fMo, w44o);
2287  display_scene(w44o, scene, I_, curColor);
2288  }
2289  m_mutex_scene.unlock();
2290 }
2291 
2309 {
2310  vpColVector stop(6);
2311  bool status = true;
2312  stop = 0;
2313  set_artVel(stop);
2314  set_velocity(stop);
2315  vpHomogeneousMatrix fMc_;
2316  fMc_ = fMo * cMo_.inverse();
2317 
2318  vpColVector articularCoordinates = get_artCoord();
2319  unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
2320 
2321  if (nbSol == 0) {
2322  status = false;
2323  vpERROR_TRACE("Positioning error. Position unreachable");
2324  }
2325 
2326  if (verbose_)
2327  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2328 
2329  set_artCoord(articularCoordinates);
2330 
2331  compute_fMi();
2332 
2333  return status;
2334 }
2335 
2350 {
2351  vpColVector stop(6);
2352  stop = 0;
2353  m_mutex_scene.lock();
2354  set_artVel(stop);
2355  set_velocity(stop);
2356  vpHomogeneousMatrix fMit[8];
2357  get_fMi(fMit);
2358  fMo = fMit[7] * cMo_;
2359  m_mutex_scene.unlock();
2360 }
2361 
2362 #elif !defined(VISP_BUILD_SHARED_LIBS)
2363 // Work around to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
2364 // has no symbols
2365 void dummy_vpSimulatorViper850() { };
2366 #endif
unsigned int getRows() const
Definition: vpArray2D.h:284
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
double sumSquare() const
vpColVector & deg2rad()
Definition: vpColVector.h:342
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
static const vpColor none
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:214
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getHeight() const
Definition: vpImage.h:184
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2372
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:818
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:748
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:260
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2287
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:77
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:461
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:459
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ positionOutOfRangeError
Position is out of range.
@ lowLevelError
Error thrown by the low level sdk.
double getSamplingTime() const
This class aims to be a basis used to create all the simulators of robots.
void set_velocity(const vpColVector &vel)
static void launcher(vpRobotWireFrameSimulator &simulator)
void set_displayBusy(const bool &status)
vpHomogeneousMatrix getExternalCameraPosition() const
void set_artCoord(const vpColVector &coord)
void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
void set_artVel(const vpColVector &vel)
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpControlFrameType getRobotFrame(void) const
Definition: vpRobot.h:181
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
double getMaxRotationVelocity(void) const
Definition: vpRobot.cpp:270
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
double getMaxTranslationVelocity(void) const
Definition: vpRobot.cpp:248
vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame)
Definition: vpRobot.cpp:204
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement) vp_override
static bool savePosFile(const std::string &filename, const vpColVector &q)
void updateArticularPosition() vp_override
int isInJointLimit() vp_override
virtual ~vpSimulatorViper850() vp_override
void setCameraParameters(const vpCameraParameters &cam)
void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width, const unsigned int &image_height)
void getExternalImage(vpImage< vpRGBa > &I)
void get_cMe(vpHomogeneousMatrix &cMe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity) vp_override
void get_cVe(vpVelocityTwistMatrix &cVe)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q)
void findHighestPositioningSpeed(vpColVector &q)
void computeArticularVelocity() vp_override
void get_eJe(vpMatrix &eJe) vp_override
void get_fJe(vpMatrix &fJe) vp_override
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) vp_override
bool singularityTest(const vpColVector &q, vpMatrix &J)
void setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
static const double defaultPositioningVelocity
void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo)
void get_fMi(vpHomogeneousMatrix *fMit) vp_override
static bool readPosFile(const std::string &filename, vpColVector &q)
void move(const char *filename)
void init() vp_override
double getPositioningVelocity(void)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
void initArms() vp_override
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Modelization of the ADEPT Viper 850 robot.
Definition: vpViper850.h:95
static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME
Definition: vpViper850.h:114
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
Definition: vpViper850.h:161
static const vpToolType defaultTool
Default tool attached to the robot end effector.
Definition: vpViper850.h:129
vpCameraParameters::vpCameraParametersProjType projModel
Definition: vpViper850.h:168
vpToolType getToolType() const
Get the current tool type.
Definition: vpViper850.h:152
vpToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpViper850.h:120
@ TOOL_SCHUNK_GRIPPER_CAMERA
Definition: vpViper850.h:123
@ TOOL_PTGREY_FLEA2_CAMERA
Definition: vpViper850.h:122
@ TOOL_MARLIN_F033C_CAMERA
Definition: vpViper850.h:121
@ TOOL_GENERIC_CAMERA
Definition: vpViper850.h:124
static const char *const CONST_MARLIN_F033C_CAMERA_NAME
Definition: vpViper850.h:113
vpTranslationVector etc
Definition: vpViper.h:156
double d6
for joint 6
Definition: vpViper.h:164
double a3
for joint 3
Definition: vpViper.h:162
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpViper.cpp:904
double d4
for joint 4
Definition: vpViper.h:163
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpViper.cpp:1141
vpColVector joint_max
Definition: vpViper.h:169
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
Definition: vpViper.cpp:555
vpHomogeneousMatrix eMc
End effector to camera transformation.
Definition: vpViper.h:154
double a1
Definition: vpViper.h:160
vpRxyzVector erc
Definition: vpViper.h:157
vpColVector joint_min
Definition: vpViper.h:170
double a2
for joint 2
Definition: vpViper.h:161
static const unsigned int njoint
Number of joint.
Definition: vpViper.h:151
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpViper.cpp:952
double d1
for joint 1
Definition: vpViper.h:160
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpViper.cpp:592
vpHomogeneousMatrix camMf2
vpHomogeneousMatrix f2Mf
void display_scene(Matrix mat, Bound_scene &sc, const vpImage< vpRGBa > &I, const vpColor &color)
vpHomogeneousMatrix fMo
vpHomogeneousMatrix camMf
void setExternalCameraParameters(const vpCameraParameters &cam)
vpHomogeneousMatrix navigation(const vpImage< vpRGBa > &I, bool &changed)
#define vpTRACE
Definition: vpDebug.h:405
#define vpERROR_TRACE
Definition: vpDebug.h:382
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()