Visual Servoing Platform  version 3.6.1 under development (2024-10-10)
vpSimulatorAfma6.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 Afma6.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 #if defined(VISP_HAVE_MODULE_GUI) && defined(VISP_HAVE_THREADS)
38 #include <cmath> // std::fabs
39 #include <limits> // numeric_limits
40 #include <string>
41 #include <visp3/core/vpDebug.h>
42 #include <visp3/core/vpImagePoint.h>
43 #include <visp3/core/vpIoTools.h>
44 #include <visp3/core/vpMeterPixelConversion.h>
45 #include <visp3/core/vpPoint.h>
46 #include <visp3/core/vpTime.h>
47 #include <visp3/robot/vpRobotException.h>
48 #include <visp3/robot/vpSimulatorAfma6.h>
49 
50 #include "../wireframe-simulator/vpBound.h"
51 #include "../wireframe-simulator/vpRfstack.h"
52 #include "../wireframe-simulator/vpScene.h"
53 #include "../wireframe-simulator/vpVwstack.h"
54 
55 BEGIN_VISP_NAMESPACE
56 const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0;
57 
61 vpSimulatorAfma6::vpSimulatorAfma6()
62  : vpRobotWireFrameSimulator(), vpAfma6(), q_prev_getdis(), first_time_getdis(true),
63  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
64 {
65  init();
66  initDisplay();
67 
68  tcur = vpTime::measureTimeMs();
69 
70  m_thread = new std::thread(&launcher, std::ref(*this));
71 
72  compute_fMi();
73 }
74 
81 vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
82  : vpRobotWireFrameSimulator(do_display), q_prev_getdis(), first_time_getdis(true),
83  positioningVelocity(defaultPositioningVelocity), zeroPos(), reposPos(), toolCustom(false), arm_dir()
84 {
85  init();
86  initDisplay();
87 
88  tcur = vpTime::measureTimeMs();
89 
90  m_thread = new std::thread(&launcher, std::ref(*this));
91 
92  compute_fMi();
93 }
94 
98 vpSimulatorAfma6::~vpSimulatorAfma6()
99 {
100  m_mutex_robotStop.lock();
101  robotStop = true;
102  m_mutex_robotStop.unlock();
103 
104  m_thread->join();
105 
106  if (robotArms != nullptr) {
107  for (int i = 0; i < 6; i++)
108  free_Bound_scene(&(robotArms[i]));
109  }
110 
111  delete[] robotArms;
112  delete[] fMi;
113  delete m_thread;
114 }
115 
124 void vpSimulatorAfma6::init()
125 {
126  // set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
127  // VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
128  std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
129  bool armDirExists = false;
130  for (size_t i = 0; i < arm_dirs.size(); i++)
131  if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
132  arm_dir = arm_dirs[i];
133  armDirExists = true;
134  break;
135  }
136  if (!armDirExists) {
137  try {
138  arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
139  std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
140  }
141  catch (...) {
142  std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
143  }
144  }
145 
146  this->init(vpAfma6::TOOL_CCMOP);
147  toolCustom = false;
148 
149  size_fMi = 8;
150  fMi = new vpHomogeneousMatrix[8];
151  artCoord.resize(njoint);
152  artVel.resize(njoint);
153 
154  zeroPos.resize(njoint);
155  zeroPos = 0;
156  reposPos.resize(njoint);
157  reposPos = 0;
158  reposPos[1] = -M_PI / 2;
159  reposPos[2] = M_PI;
160  reposPos[4] = M_PI / 2;
161 
162  artCoord = zeroPos;
163  artVel = 0;
164 
165  q_prev_getdis.resize(njoint);
166  q_prev_getdis = 0;
167  first_time_getdis = true;
168 
169  positioningVelocity = defaultPositioningVelocity;
170 
171  setRobotFrame(vpRobot::ARTICULAR_FRAME);
172  this->setRobotState(vpRobot::STATE_STOP);
173 
174  // Software joint limits in radians
175  //_joint_min.resize(njoint);
176  _joint_min[0] = -0.7501;
177  _joint_min[1] = -0.6501;
178  _joint_min[2] = -0.5001;
179  _joint_min[3] = -2.7301;
180  _joint_min[4] = -0.3001;
181  _joint_min[5] = -1.5901;
182  //_joint_max.resize(njoint);
183  _joint_max[0] = 0.6001;
184  _joint_max[1] = 0.6701;
185  _joint_max[2] = 0.4601;
186  _joint_max[3] = 2.7301;
187  _joint_max[4] = 2.4801;
188  _joint_max[5] = 1.5901;
189 }
190 
194 void vpSimulatorAfma6::initDisplay()
195 {
196  robotArms = nullptr;
197  robotArms = new Bound_scene[6];
198  initArms();
199  setExternalCameraPosition(vpHomogeneousMatrix(0, 0, 0, 0, 0, vpMath::rad(180)) *
200  vpHomogeneousMatrix(-0.1, 0, 4, vpMath::rad(90), 0, 0));
201  cameraParam.initPersProjWithoutDistortion(558.5309599, 556.055053, 320, 240);
202  setExternalCameraParameters(cameraParam);
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 
225 void vpSimulatorAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType proj_model)
226 {
227  this->projModel = proj_model;
228  unsigned int name_length = 30; // the size of this kind of string "/afma6_tool_vacuum.bnd"
229  if (arm_dir.size() > FILENAME_MAX)
230  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
231  unsigned int full_length = (unsigned int)arm_dir.size() + name_length;
232  if (full_length > FILENAME_MAX)
233  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
234 
235  // Use here default values of the robot constant parameters.
236  switch (tool) {
237  case vpAfma6::TOOL_CCMOP: {
238  _erc[0] = vpMath::rad(164.35); // rx
239  _erc[1] = vpMath::rad(89.64); // ry
240  _erc[2] = vpMath::rad(-73.05); // rz
241  _etc[0] = 0.0117; // tx
242  _etc[1] = 0.0033; // ty
243  _etc[2] = 0.2272; // tz
244 
245  setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
246 
247  if (robotArms != nullptr) {
248  while (get_displayBusy())
249  vpTime::wait(2);
250  free_Bound_scene(&(robotArms[5]));
251  char *name_arm = new char[full_length];
252  strcpy(name_arm, arm_dir.c_str());
253  strcat(name_arm, "/afma6_tool_ccmop.bnd");
254  set_scene(name_arm, robotArms + 5, 1.0);
255  set_displayBusy(false);
256  delete[] name_arm;
257  }
258  break;
259  }
260  case vpAfma6::TOOL_GRIPPER: {
261  _erc[0] = vpMath::rad(88.33); // rx
262  _erc[1] = vpMath::rad(72.07); // ry
263  _erc[2] = vpMath::rad(2.53); // rz
264  _etc[0] = 0.0783; // tx
265  _etc[1] = 0.1234; // ty
266  _etc[2] = 0.1638; // tz
267 
268  setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
269 
270  if (robotArms != nullptr) {
271  while (get_displayBusy())
272  vpTime::wait(2);
273  free_Bound_scene(&(robotArms[5]));
274  char *name_arm = new char[full_length];
275  strcpy(name_arm, arm_dir.c_str());
276  strcat(name_arm, "/afma6_tool_gripper.bnd");
277  set_scene(name_arm, robotArms + 5, 1.0);
278  set_displayBusy(false);
279  delete[] name_arm;
280  }
281  break;
282  }
283  case vpAfma6::TOOL_VACUUM: {
284  _erc[0] = vpMath::rad(90.40); // rx
285  _erc[1] = vpMath::rad(75.11); // ry
286  _erc[2] = vpMath::rad(0.18); // rz
287  _etc[0] = 0.0038; // tx
288  _etc[1] = 0.1281; // ty
289  _etc[2] = 0.1658; // tz
290 
291  setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
292 
293  if (robotArms != nullptr) {
294  while (get_displayBusy())
295  vpTime::wait(2);
296  free_Bound_scene(&(robotArms[5]));
297 
298  char *name_arm = new char[full_length];
299 
300  strcpy(name_arm, arm_dir.c_str());
301  strcat(name_arm, "/afma6_tool_vacuum.bnd");
302  set_scene(name_arm, robotArms + 5, 1.0);
303  set_displayBusy(false);
304  delete[] name_arm;
305  }
306  break;
307  }
308  case vpAfma6::TOOL_CUSTOM: {
309  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
310  break;
311  }
313  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
314  break;
315  }
317  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
318  break;
319  }
320  }
321 
322  vpRotationMatrix eRc(_erc);
323 
324  m_mutex_eMc.lock();
325  this->_eMc.buildFrom(_etc, eRc);
326  m_mutex_eMc.unlock();
327 
328  setToolType(tool);
329  return;
330 }
331 
342 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
343  const unsigned int &image_height)
344 {
345  if (toolCustom) {
346  cam.initPersProjWithoutDistortion(px_int, py_int, image_width / 2, image_height / 2);
347  }
348  // Set default parameters
349  switch (getToolType()) {
350  case vpAfma6::TOOL_CCMOP: {
351  // Set default intrinsic camera parameters for 640x480 images
352  if (image_width == 640 && image_height == 480) {
353  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
354  << std::endl;
355  cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
356  }
357  else {
358  vpTRACE("Cannot get default intrinsic camera parameters for this image "
359  "resolution");
360  }
361  break;
362  }
363  case vpAfma6::TOOL_GRIPPER: {
364  // Set default intrinsic camera parameters for 640x480 images
365  if (image_width == 640 && image_height == 480) {
366  std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
367  << std::endl;
368  cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
369  }
370  else {
371  vpTRACE("Cannot get default intrinsic camera parameters for this image "
372  "resolution");
373  }
374  break;
375  }
376  case vpAfma6::TOOL_CUSTOM: {
377  std::cout << "The generic tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
378  break;
379  }
381  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
382  break;
383  }
385  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
386  break;
387  }
388  case vpAfma6::TOOL_VACUUM: {
389  std::cout << "The vacuum tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
390  break;
391  }
392  default:
393  vpERROR_TRACE("This error should not occur!");
394  break;
395  }
396  return;
397 }
398 
407 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I_)
408 {
409  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
410 }
411 
420 void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I_)
421 {
422  getCameraParameters(cam, I_.getWidth(), I_.getHeight());
423 }
424 
430 void vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
431 {
432  px_int = cam.get_px();
433  py_int = cam.get_py();
434  toolCustom = true;
435 }
436 
441 void vpSimulatorAfma6::updateArticularPosition()
442 {
443  double tcur_1 = tcur; // temporary variable used to store the last time
444  // since the last command
445  bool stop = false;
446  bool setVelocityCalled_ = false;
447  while (!stop) {
448  // Get current time
449  tprev = tcur_1;
450  tcur = vpTime::measureTimeMs();
451 
452  m_mutex_setVelocityCalled.lock();
453  setVelocityCalled_ = setVelocityCalled;
454  m_mutex_setVelocityCalled.unlock();
455 
456  if (setVelocityCalled_ || !constantSamplingTimeMode) {
457  m_mutex_setVelocityCalled.lock();
458  setVelocityCalled = false;
459  m_mutex_setVelocityCalled.unlock();
460 
461  computeArticularVelocity();
462 
463  double ellapsedTime = (tcur - tprev) * 1e-3;
464  if (constantSamplingTimeMode) { // if we want a constant velocity, we
465  // force the ellapsed time to the given
466  // samplingTime
467  ellapsedTime = getSamplingTime(); // in second
468  }
469 
470  vpColVector articularCoordinates = get_artCoord();
471  vpColVector articularVelocities = get_artVel();
472 
473  if (jointLimit) {
474  double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
475  if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
476  if (verbose_) {
477  std::cout << "Joint " << jointLimitArt - 1
478  << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt - 1]) << " < " << vpMath::deg(art)
479  << " < " << vpMath::deg(_joint_max[jointLimitArt - 1]) << std::endl;
480  }
481 
482  articularVelocities = 0.0;
483  }
484  else
485  jointLimit = false;
486  }
487 
488  articularCoordinates[0] = articularCoordinates[0] + ellapsedTime * articularVelocities[0];
489  articularCoordinates[1] = articularCoordinates[1] + ellapsedTime * articularVelocities[1];
490  articularCoordinates[2] = articularCoordinates[2] + ellapsedTime * articularVelocities[2];
491  articularCoordinates[3] = articularCoordinates[3] + ellapsedTime * articularVelocities[3];
492  articularCoordinates[4] = articularCoordinates[4] + ellapsedTime * articularVelocities[4];
493  articularCoordinates[5] = articularCoordinates[5] + ellapsedTime * articularVelocities[5];
494 
495  int jl = isInJointLimit();
496 
497  if (jl != 0 && jointLimit == false) {
498  if (jl < 0)
499  ellapsedTime = (_joint_min[(unsigned int)(-jl - 1)] - articularCoordinates[(unsigned int)(-jl - 1)]) /
500  (articularVelocities[(unsigned int)(-jl - 1)]);
501  else
502  ellapsedTime = (_joint_max[(unsigned int)(jl - 1)] - articularCoordinates[(unsigned int)(jl - 1)]) /
503  (articularVelocities[(unsigned int)(jl - 1)]);
504 
505  for (unsigned int i = 0; i < 6; i++)
506  articularCoordinates[i] = articularCoordinates[i] + ellapsedTime * articularVelocities[i];
507 
508  jointLimit = true;
509  jointLimitArt = (unsigned int)fabs((double)jl);
510  }
511 
512  set_artCoord(articularCoordinates);
513  set_artVel(articularVelocities);
514 
515  compute_fMi();
516 
517  if (displayAllowed) {
519  vpDisplay::displayFrame(I, getExternalCameraPosition(), cameraParam, 0.2, vpColor::none, thickness_);
520  vpDisplay::displayFrame(I, getExternalCameraPosition() * fMi[7], cameraParam, 0.1, vpColor::none, thickness_);
521  }
522 
523  if (displayType == MODEL_3D && displayAllowed) {
524  while (get_displayBusy()) {
525  vpTime::wait(2);
526  }
527  vpSimulatorAfma6::getExternalImage(I);
528  set_displayBusy(false);
529  }
530 
531  if (displayType == MODEL_DH && displayAllowed) {
532  vpHomogeneousMatrix fMit[8];
533  get_fMi(fMit);
534 
535  // vpDisplay::displayFrame(I,getExternalCameraPosition
536  // ()*fMi[6],cameraParam,0.2,vpColor::none);
537 
538  vpImagePoint iP, iP_1;
539  vpPoint pt(0, 0, 0);
540 
541  pt.track(getExternalCameraPosition());
542  vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
543  pt.track(getExternalCameraPosition() * fMit[0]);
544  vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
545  vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
546  for (unsigned int k = 1; k < 7; k++) {
547  pt.track(getExternalCameraPosition() * fMit[k - 1]);
548  vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
549 
550  pt.track(getExternalCameraPosition() * fMit[k]);
551  vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
552 
553  vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
554  }
555  vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green,
556  thickness_);
557  }
558 
559  vpDisplay::flush(I);
560 
561  vpTime::wait(tcur, 1000 * getSamplingTime());
562  tcur_1 = tcur;
563  }
564  else {
566  }
567 
568  m_mutex_robotStop.lock();
569  stop = robotStop;
570  m_mutex_robotStop.unlock();
571  }
572 }
573 
587 void vpSimulatorAfma6::compute_fMi()
588 {
589  // vpColVector q = get_artCoord();
590  vpColVector q(6); //; = get_artCoord();
591  q = get_artCoord();
592 
593  vpHomogeneousMatrix fMit[8];
594 
595  double q1 = q[0];
596  double q2 = q[1];
597  double q3 = q[2];
598  double q4 = q[3];
599  double q5 = q[4];
600  double q6 = q[5];
601 
602  double c4 = cos(q4);
603  double s4 = sin(q4);
604  double c5 = cos(q5);
605  double s5 = sin(q5);
606  double c6 = cos(q6);
607  double s6 = sin(q6);
608 
609  fMit[0][0][0] = 1;
610  fMit[0][1][0] = 0;
611  fMit[0][2][0] = 0;
612  fMit[0][0][1] = 0;
613  fMit[0][1][1] = 1;
614  fMit[0][2][1] = 0;
615  fMit[0][0][2] = 0;
616  fMit[0][1][2] = 0;
617  fMit[0][2][2] = 1;
618  fMit[0][0][3] = q1;
619  fMit[0][1][3] = 0;
620  fMit[0][2][3] = 0;
621 
622  fMit[1][0][0] = 1;
623  fMit[1][1][0] = 0;
624  fMit[1][2][0] = 0;
625  fMit[1][0][1] = 0;
626  fMit[1][1][1] = 1;
627  fMit[1][2][1] = 0;
628  fMit[1][0][2] = 0;
629  fMit[1][1][2] = 0;
630  fMit[1][2][2] = 1;
631  fMit[1][0][3] = q1;
632  fMit[1][1][3] = q2;
633  fMit[1][2][3] = 0;
634 
635  fMit[2][0][0] = 1;
636  fMit[2][1][0] = 0;
637  fMit[2][2][0] = 0;
638  fMit[2][0][1] = 0;
639  fMit[2][1][1] = 1;
640  fMit[2][2][1] = 0;
641  fMit[2][0][2] = 0;
642  fMit[2][1][2] = 0;
643  fMit[2][2][2] = 1;
644  fMit[2][0][3] = q1;
645  fMit[2][1][3] = q2;
646  fMit[2][2][3] = q3;
647 
648  fMit[3][0][0] = s4;
649  fMit[3][1][0] = -c4;
650  fMit[3][2][0] = 0;
651  fMit[3][0][1] = c4;
652  fMit[3][1][1] = s4;
653  fMit[3][2][1] = 0;
654  fMit[3][0][2] = 0;
655  fMit[3][1][2] = 0;
656  fMit[3][2][2] = 1;
657  fMit[3][0][3] = q1;
658  fMit[3][1][3] = q2;
659  fMit[3][2][3] = q3;
660 
661  fMit[4][0][0] = s4 * s5;
662  fMit[4][1][0] = -c4 * s5;
663  fMit[4][2][0] = c5;
664  fMit[4][0][1] = s4 * c5;
665  fMit[4][1][1] = -c4 * c5;
666  fMit[4][2][1] = -s5;
667  fMit[4][0][2] = c4;
668  fMit[4][1][2] = s4;
669  fMit[4][2][2] = 0;
670  fMit[4][0][3] = c4 * this->_long_56 + q1;
671  fMit[4][1][3] = s4 * this->_long_56 + q2;
672  fMit[4][2][3] = q3;
673 
674  fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
675  fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
676  fMit[5][2][0] = c5 * c6;
677  fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
678  fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
679  fMit[5][2][1] = -c5 * s6;
680  fMit[5][0][2] = -s4 * c5;
681  fMit[5][1][2] = c4 * c5;
682  fMit[5][2][2] = s5;
683  fMit[5][0][3] = c4 * this->_long_56 + q1;
684  fMit[5][1][3] = s4 * this->_long_56 + q2;
685  fMit[5][2][3] = q3;
686 
687  fMit[6][0][0] = fMit[5][0][0];
688  fMit[6][1][0] = fMit[5][1][0];
689  fMit[6][2][0] = fMit[5][2][0];
690  fMit[6][0][1] = fMit[5][0][1];
691  fMit[6][1][1] = fMit[5][1][1];
692  fMit[6][2][1] = fMit[5][2][1];
693  fMit[6][0][2] = fMit[5][0][2];
694  fMit[6][1][2] = fMit[5][1][2];
695  fMit[6][2][2] = fMit[5][2][2];
696  fMit[6][0][3] = fMit[5][0][3];
697  fMit[6][1][3] = fMit[5][1][3];
698  fMit[6][2][3] = fMit[5][2][3];
699 
700  // vpHomogeneousMatrix cMe;
701  // get_cMe(cMe);
702  // cMe = cMe.inverse();
703  // fMit[7] = fMit[6] * cMe;
704  m_mutex_eMc.lock();
705  vpAfma6::get_fMc(q, fMit[7]);
706  m_mutex_eMc.unlock();
707 
708  m_mutex_fMi.lock();
709  for (int i = 0; i < 8; i++) {
710  fMi[i] = fMit[i];
711  }
712  m_mutex_fMi.unlock();
713 }
714 
720 vpRobot::vpRobotStateType vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState)
721 {
722  switch (newState) {
723  case vpRobot::STATE_STOP: {
724  // Start primitive STOP only if the current state is Velocity
725  if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
726  stopMotion();
727  }
728  break;
729  }
731  if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
732  std::cout << "Change the control mode from velocity to position control.\n";
733  stopMotion();
734  }
735  else {
736  // std::cout << "Change the control mode from stop to position
737  // control.\n";
738  }
739  break;
740  }
742  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
743  std::cout << "Change the control mode from stop to velocity control.\n";
744  }
745  break;
746  }
748  default:
749  break;
750  }
751 
752  return vpRobot::setRobotState(newState);
753 }
754 
831 void vpSimulatorAfma6::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
832 {
833  if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
834  vpERROR_TRACE("Cannot send a velocity to the robot "
835  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
837  "Cannot send a velocity to the robot "
838  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
839  }
840 
841  vpColVector vel_sat(6);
842 
843  double scale_sat = 1;
844  double vel_trans_max = getMaxTranslationVelocity();
845  double vel_rot_max = getMaxRotationVelocity();
846 
847  double vel_abs; // Absolute value
848 
849  // Velocity saturation
850  switch (frame) {
851  // saturation in cartesian space
854  if (vel.getRows() != 6) {
855  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
856  throw;
857  }
858 
859  for (unsigned int i = 0; i < 3; ++i) {
860  vel_abs = fabs(vel[i]);
861  if (vel_abs > vel_trans_max && !jointLimit) {
862  vel_trans_max = vel_abs;
863  vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
864  "(axis nr. %d).",
865  vel[i], i + 1);
866  }
867 
868  vel_abs = fabs(vel[i + 3]);
869  if (vel_abs > vel_rot_max && !jointLimit) {
870  vel_rot_max = vel_abs;
871  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
872  "(axis nr. %d).",
873  vel[i + 3], i + 4);
874  }
875  }
876 
877  double scale_trans_sat = 1;
878  double scale_rot_sat = 1;
879  if (vel_trans_max > getMaxTranslationVelocity())
880  scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
881 
882  if (vel_rot_max > getMaxRotationVelocity())
883  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
884 
885  if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
886  if (scale_trans_sat < scale_rot_sat)
887  scale_sat = scale_trans_sat;
888  else
889  scale_sat = scale_rot_sat;
890  }
891  break;
892  }
893 
894  // saturation in joint space
896  if (vel.getRows() != 6) {
897  vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
898  throw;
899  }
900  for (unsigned int i = 0; i < 6; ++i) {
901  vel_abs = fabs(vel[i]);
902  if (vel_abs > vel_rot_max && !jointLimit) {
903  vel_rot_max = vel_abs;
904  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
905  "(axis nr. %d).",
906  vel[i], i + 1);
907  }
908  }
909  double scale_rot_sat = 1;
910  if (vel_rot_max > getMaxRotationVelocity())
911  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
912  if (scale_rot_sat < 1)
913  scale_sat = scale_rot_sat;
914  break;
915  }
916  case vpRobot::MIXT_FRAME: {
917  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in MIXT_FRAME frame:"
918  "functionality not implemented");
919  }
921  throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in END_EFFECTOR_FRAME frame:"
922  "functionality not implemented");
923  }
924  }
925 
926  set_velocity(vel * scale_sat);
927 
928  m_mutex_frame.lock();
929  setRobotFrame(frame);
930  m_mutex_frame.unlock();
931 
932  m_mutex_setVelocityCalled.lock();
933  setVelocityCalled = true;
934  m_mutex_setVelocityCalled.unlock();
935 }
936 
940 void vpSimulatorAfma6::computeArticularVelocity()
941 {
943 
944  m_mutex_frame.lock();
945  frame = getRobotFrame();
946  m_mutex_frame.unlock();
947 
948  double vel_rot_max = getMaxRotationVelocity();
949 
950  vpColVector articularCoordinates = get_artCoord();
951  vpColVector velocityframe = get_velocity();
952  vpColVector articularVelocity;
953 
954  switch (frame) {
955  case vpRobot::CAMERA_FRAME: {
956  vpMatrix eJe_;
957  vpVelocityTwistMatrix eVc(_eMc);
958  vpAfma6::get_eJe(articularCoordinates, eJe_);
959  eJe_ = eJe_.pseudoInverse();
960  if (singularityManagement)
961  singularityTest(articularCoordinates, eJe_);
962  articularVelocity = eJe_ * eVc * velocityframe;
963  set_artVel(articularVelocity);
964  break;
965  }
967  vpMatrix fJe_;
968  vpAfma6::get_fJe(articularCoordinates, fJe_);
969  fJe_ = fJe_.pseudoInverse();
970  if (singularityManagement)
971  singularityTest(articularCoordinates, fJe_);
972  articularVelocity = fJe_ * velocityframe;
973  set_artVel(articularVelocity);
974  break;
975  }
977  articularVelocity = velocityframe;
978  set_artVel(articularVelocity);
979  break;
980  }
982  case vpRobot::MIXT_FRAME: {
983  break;
984  }
985  }
986 
987  switch (frame) {
990  for (unsigned int i = 0; i < 6; ++i) {
991  double vel_abs = fabs(articularVelocity[i]);
992  if (vel_abs > vel_rot_max && !jointLimit) {
993  vel_rot_max = vel_abs;
994  vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
995  "(axis nr. %d).",
996  articularVelocity[i], i + 1);
997  }
998  }
999  double scale_rot_sat = 1;
1000  double scale_sat = 1;
1001  if (vel_rot_max > getMaxRotationVelocity())
1002  scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
1003  if (scale_rot_sat < 1)
1004  scale_sat = scale_rot_sat;
1005 
1006  set_artVel(articularVelocity * scale_sat);
1007  break;
1008  }
1011  case vpRobot::MIXT_FRAME: {
1012  break;
1013  }
1014  }
1015 }
1016 
1066 void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel)
1067 {
1068  vel.resize(6);
1069 
1070  vpColVector articularCoordinates = get_artCoord();
1071  vpColVector articularVelocity = get_artVel();
1072 
1073  switch (frame) {
1074  case vpRobot::CAMERA_FRAME: {
1075  vpMatrix eJe_;
1076  vpVelocityTwistMatrix cVe(_eMc);
1077  vpAfma6::get_eJe(articularCoordinates, eJe_);
1078  vel = cVe * eJe_ * articularVelocity;
1079  break;
1080  }
1081  case vpRobot::ARTICULAR_FRAME: {
1082  vel = articularVelocity;
1083  break;
1084  }
1085  case vpRobot::REFERENCE_FRAME: {
1086  vpMatrix fJe_;
1087  vpAfma6::get_fJe(articularCoordinates, fJe_);
1088  vel = fJe_ * articularVelocity;
1089  break;
1090  }
1092  case vpRobot::MIXT_FRAME: {
1093  break;
1094  }
1095  default: {
1096  vpERROR_TRACE("Error in spec of vpRobot. "
1097  "Case not taken in account.");
1098  return;
1099  }
1100  }
1101 }
1102 
1119 void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double &timestamp)
1120 {
1121  timestamp = vpTime::measureTimeSecond();
1122  getVelocity(frame, vel);
1123 }
1124 
1170 vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame)
1171 {
1172  vpColVector vel(6);
1173  getVelocity(frame, vel);
1174 
1175  return vel;
1176 }
1177 
1190 vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame, double &timestamp)
1191 {
1192  timestamp = vpTime::measureTimeSecond();
1193  vpColVector vel(6);
1194  getVelocity(frame, vel);
1195 
1196  return vel;
1197 }
1198 
1199 void vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
1200 {
1201  double vel_rot_max = getMaxRotationVelocity();
1202  double velmax = fabs(q[0]);
1203  for (unsigned int i = 1; i < 6; i++) {
1204  if (velmax < fabs(q[i]))
1205  velmax = fabs(q[i]);
1206  }
1207 
1208  double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
1209  q = q * alpha;
1210 }
1211 
1290 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
1291 {
1292  if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
1293  vpERROR_TRACE("Robot was not in position-based control\n"
1294  "Modification of the robot state");
1295  // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
1296  }
1297 
1298  vpColVector articularCoordinates = get_artCoord();
1299 
1300  vpColVector error(6);
1301  double errsqr = 0;
1302  switch (frame) {
1303  case vpRobot::CAMERA_FRAME: {
1304  int nbSol;
1305  vpColVector qdes(6);
1306 
1307  vpTranslationVector txyz;
1308  vpRxyzVector rxyz;
1309  for (unsigned int i = 0; i < 3; i++) {
1310  txyz[i] = q[i];
1311  rxyz[i] = q[i + 3];
1312  }
1313 
1314  vpRotationMatrix cRc2(rxyz);
1315  vpHomogeneousMatrix cMc2(txyz, cRc2);
1316 
1317  vpHomogeneousMatrix fMc_;
1318  vpAfma6::get_fMc(articularCoordinates, fMc_);
1319 
1320  vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
1321 
1322  do {
1323  articularCoordinates = get_artCoord();
1324  qdes = articularCoordinates;
1325  nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
1326 
1327  m_mutex_setVelocityCalled.lock();
1328  setVelocityCalled = true;
1329  m_mutex_setVelocityCalled.unlock();
1330 
1331  if (nbSol > 0) {
1332  error = qdes - articularCoordinates;
1333  errsqr = error.sumSquare();
1334  // findHighestPositioningSpeed(error);
1335  set_artVel(error);
1336  if (errsqr < 1e-4) {
1337  set_artCoord(qdes);
1338  error = 0;
1339  set_artVel(error);
1340  set_velocity(error);
1341  break;
1342  }
1343  }
1344  else {
1345  vpERROR_TRACE("Positioning error.");
1346  throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
1347  }
1348  } while (errsqr > 1e-8 && nbSol > 0);
1349 
1350  break;
1351  }
1352 
1353  case vpRobot::ARTICULAR_FRAME: {
1354  do {
1355  articularCoordinates = get_artCoord();
1356  error = q - articularCoordinates;
1357  errsqr = error.sumSquare();
1358  // findHighestPositioningSpeed(error);
1359  set_artVel(error);
1360  m_mutex_setVelocityCalled.lock();
1361  setVelocityCalled = true;
1362  m_mutex_setVelocityCalled.unlock();
1363  if (errsqr < 1e-4) {
1364  set_artCoord(q);
1365  error = 0;
1366  set_artVel(error);
1367  set_velocity(error);
1368  break;
1369  }
1370  } while (errsqr > 1e-8);
1371  break;
1372  }
1373 
1374  case vpRobot::REFERENCE_FRAME: {
1375  int nbSol;
1376  vpColVector qdes(6);
1377 
1378  vpTranslationVector txyz;
1379  vpRxyzVector rxyz;
1380  for (unsigned int i = 0; i < 3; i++) {
1381  txyz[i] = q[i];
1382  rxyz[i] = q[i + 3];
1383  }
1384 
1385  vpRotationMatrix fRc(rxyz);
1386  vpHomogeneousMatrix fMc_(txyz, fRc);
1387 
1388  do {
1389  articularCoordinates = get_artCoord();
1390  qdes = articularCoordinates;
1391  nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
1392  m_mutex_setVelocityCalled.lock();
1393  setVelocityCalled = true;
1394  m_mutex_setVelocityCalled.unlock();
1395  if (nbSol > 0) {
1396  error = qdes - articularCoordinates;
1397  errsqr = error.sumSquare();
1398  // findHighestPositioningSpeed(error);
1399  set_artVel(error);
1400  if (errsqr < 1e-4) {
1401  set_artCoord(qdes);
1402  error = 0;
1403  set_artVel(error);
1404  set_velocity(error);
1405  break;
1406  }
1407  }
1408  else
1409  vpERROR_TRACE("Positioning error. Position unreachable");
1410  } while (errsqr > 1e-8 && nbSol > 0);
1411  break;
1412  }
1413  case vpRobot::MIXT_FRAME: {
1414  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1415  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1416  "MIXT_FRAME not implemented.");
1417  }
1419  vpERROR_TRACE("Positioning error. Mixt frame not implemented");
1420  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1421  "END_EFFECTOR_FRAME not implemented.");
1422  }
1423  }
1424 }
1425 
1492 void vpSimulatorAfma6::setPosition(const vpRobot::vpControlFrameType frame, double pos1, double pos2, double pos3,
1493  double pos4, double pos5, double pos6)
1494 {
1495  try {
1496  vpColVector position(6);
1497  position[0] = pos1;
1498  position[1] = pos2;
1499  position[2] = pos3;
1500  position[3] = pos4;
1501  position[4] = pos5;
1502  position[5] = pos6;
1503 
1504  setPosition(frame, position);
1505  }
1506  catch (...) {
1507  vpERROR_TRACE("Error caught");
1508  throw;
1509  }
1510 }
1511 
1550 void vpSimulatorAfma6::setPosition(const char *filename)
1551 {
1552  vpColVector q;
1553  bool ret;
1554 
1555  ret = this->readPosFile(filename, q);
1556 
1557  if (ret == false) {
1558  vpERROR_TRACE("Bad position in \"%s\"", filename);
1559  throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
1560  }
1561  this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
1562  this->setPosition(vpRobot::ARTICULAR_FRAME, q);
1563 }
1564 
1627 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
1628 {
1629  q.resize(6);
1630 
1631  switch (frame) {
1632  case vpRobot::CAMERA_FRAME: {
1633  q = 0;
1634  break;
1635  }
1636 
1637  case vpRobot::ARTICULAR_FRAME: {
1638  q = get_artCoord();
1639  break;
1640  }
1641 
1642  case vpRobot::REFERENCE_FRAME: {
1643  vpHomogeneousMatrix fMc_;
1644  vpAfma6::get_fMc(get_artCoord(), fMc_);
1645 
1646  vpRotationMatrix fRc;
1647  fMc_.extract(fRc);
1648  vpRxyzVector rxyz(fRc);
1649 
1650  vpTranslationVector txyz;
1651  fMc_.extract(txyz);
1652 
1653  for (unsigned int i = 0; i < 3; i++) {
1654  q[i] = txyz[i];
1655  q[i + 3] = rxyz[i];
1656  }
1657  break;
1658  }
1659 
1660  case vpRobot::MIXT_FRAME: {
1661  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1662  "Mixt frame not implemented.");
1663  }
1665  throw vpRobotException(vpRobotException::lowLevelError, "Positioning error: "
1666  "End-effector frame not implemented.");
1667  }
1668  }
1669 }
1670 
1697 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double &timestamp)
1698 {
1699  timestamp = vpTime::measureTimeSecond();
1700  getPosition(frame, q);
1701 }
1702 
1714 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
1715 {
1716  vpColVector posRxyz;
1717  // recupere position en Rxyz
1718  this->getPosition(frame, posRxyz);
1719 
1720  // recupere le vecteur thetaU correspondant
1721  vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
1722 
1723  // remplit le vpPoseVector avec translation et rotation ThetaU
1724  for (unsigned int j = 0; j < 3; j++) {
1725  position[j] = posRxyz[j];
1726  position[j + 3] = RtuVect[j];
1727  }
1728 }
1729 
1741 void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double &timestamp)
1742 {
1743  timestamp = vpTime::measureTimeSecond();
1744  getPosition(frame, position);
1745 }
1746 
1757 void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
1758 {
1759  if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
1760  vpTRACE("Joint limit vector has not a size of 6 !");
1761  return;
1762  }
1763 
1764  _joint_min[0] = limitMin[0];
1765  _joint_min[1] = limitMin[1];
1766  _joint_min[2] = limitMin[2];
1767  _joint_min[3] = limitMin[3];
1768  _joint_min[4] = limitMin[4];
1769  _joint_min[5] = limitMin[5];
1770 
1771  _joint_max[0] = limitMax[0];
1772  _joint_max[1] = limitMax[1];
1773  _joint_max[2] = limitMax[2];
1774  _joint_max[3] = limitMax[3];
1775  _joint_max[4] = limitMax[4];
1776  _joint_max[5] = limitMax[5];
1777 }
1778 
1784 bool vpSimulatorAfma6::singularityTest(const vpColVector &q, vpMatrix &J)
1785 {
1786  double q5 = q[4];
1787 
1788  bool cond = fabs(q5 - M_PI / 2) < 1e-1;
1789 
1790  if (cond) {
1791  J[0][3] = 0;
1792  J[0][4] = 0;
1793  J[1][3] = 0;
1794  J[1][4] = 0;
1795  J[3][3] = 0;
1796  J[3][4] = 0;
1797  J[5][3] = 0;
1798  J[5][4] = 0;
1799  return true;
1800  }
1801 
1802  return false;
1803 }
1804 
1808 int vpSimulatorAfma6::isInJointLimit()
1809 {
1810  int artNumb = 0;
1811  double diff = 0;
1812  double difft = 0;
1813 
1814  vpColVector articularCoordinates = get_artCoord();
1815 
1816  for (unsigned int i = 0; i < 6; i++) {
1817  if (articularCoordinates[i] <= _joint_min[i]) {
1818  difft = _joint_min[i] - articularCoordinates[i];
1819  if (difft > diff) {
1820  diff = difft;
1821  artNumb = -(int)i - 1;
1822  }
1823  }
1824  }
1825 
1826  for (unsigned int i = 0; i < 6; i++) {
1827  if (articularCoordinates[i] >= _joint_max[i]) {
1828  difft = articularCoordinates[i] - _joint_max[i];
1829  if (difft > diff) {
1830  diff = difft;
1831  artNumb = (int)(i + 1);
1832  }
1833  }
1834  }
1835 
1836  if (artNumb != 0)
1837  std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
1838  << std::endl;
1839 
1840  return artNumb;
1841 }
1842 
1860 void vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
1861 {
1862  displacement.resize(6);
1863  displacement = 0;
1864  vpColVector q_cur(6);
1865 
1866  q_cur = get_artCoord();
1867 
1868  if (!first_time_getdis) {
1869  switch (frame) {
1870  case vpRobot::CAMERA_FRAME: {
1871  std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
1872  return;
1873  }
1874  case vpRobot::ARTICULAR_FRAME: {
1875  displacement = q_cur - q_prev_getdis;
1876  break;
1877  }
1878  case vpRobot::REFERENCE_FRAME: {
1879  std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
1880  return;
1881  }
1882  case vpRobot::MIXT_FRAME: {
1883  std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
1884  return;
1885  }
1887  std::cout << "getDisplacement() END_EFFECTOR_FRAME not implemented\n";
1888  return;
1889  }
1890  }
1891  }
1892  else {
1893  first_time_getdis = false;
1894  }
1895 
1896  // Memorize the joint position for the next call
1897  q_prev_getdis = q_cur;
1898 }
1899 
1947 bool vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
1948 {
1949  std::ifstream fd(filename.c_str(), std::ios::in);
1950 
1951  if (!fd.is_open()) {
1952  return false;
1953  }
1954 
1955  std::string line;
1956  std::string key("R:");
1957  std::string id("#AFMA6 - Position");
1958  bool pos_found = false;
1959  int lineNum = 0;
1960 
1961  q.resize(njoint);
1962 
1963  while (std::getline(fd, line)) {
1964  lineNum++;
1965  if (lineNum == 1) {
1966  if (!(line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
1967  std::cout << "Error: this position file " << filename << " is not for Afma6 robot" << std::endl;
1968  return false;
1969  }
1970  }
1971  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1972  continue;
1973  }
1974  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1975  // check if there are at least njoint values in the line
1976  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1977  if (chain.size() < njoint + 1) // try to split with tab separator
1978  chain = vpIoTools::splitChain(line, std::string("\t"));
1979  if (chain.size() < njoint + 1)
1980  continue;
1981 
1982  std::istringstream ss(line);
1983  std::string key_;
1984  ss >> key_;
1985  for (unsigned int i = 0; i < njoint; i++)
1986  ss >> q[i];
1987  pos_found = true;
1988  break;
1989  }
1990  }
1991 
1992  // converts rotations from degrees into radians
1993  q[3] = vpMath::rad(q[3]);
1994  q[4] = vpMath::rad(q[4]);
1995  q[5] = vpMath::rad(q[5]);
1996 
1997  fd.close();
1998 
1999  if (!pos_found) {
2000  std::cout << "Error: unable to find a position for Afma6 robot in " << filename << std::endl;
2001  return false;
2002  }
2003 
2004  return true;
2005 }
2006 
2029 bool vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
2030 {
2031  FILE *fd;
2032  fd = fopen(filename.c_str(), "w");
2033  if (fd == nullptr)
2034  return false;
2035 
2036  fprintf(fd, "\
2037 #AFMA6 - Position - Version 2.01\n\
2038 #\n\
2039 # R: X Y Z A B C\n\
2040 # Joint position: X, Y, Z: translations in meters\n\
2041 # A, B, C: rotations in degrees\n\
2042 #\n\
2043 #\n\n");
2044 
2045  // Save positions in mm and deg
2046  fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n", q[0], q[1], q[2], vpMath::deg(q[3]), vpMath::deg(q[4]),
2047  vpMath::deg(q[5]));
2048 
2049  fclose(fd);
2050  return (true);
2051 }
2052 
2060 void vpSimulatorAfma6::move(const char *filename)
2061 {
2062  vpColVector q;
2063 
2064  try {
2065  this->readPosFile(filename, q);
2066  this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
2067  this->setPosition(vpRobot::ARTICULAR_FRAME, q);
2068  }
2069  catch (...) {
2070  throw;
2071  }
2072 }
2073 
2083 void vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe) { vpAfma6::get_cMe(cMe); }
2084 
2092 void vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
2093 {
2094  vpHomogeneousMatrix cMe;
2095  vpAfma6::get_cMe(cMe);
2096 
2097  cVe.buildFrom(cMe);
2098 }
2099 
2109 void vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
2110 {
2111  try {
2112  vpAfma6::get_eJe(get_artCoord(), eJe_);
2113  }
2114  catch (...) {
2115  vpERROR_TRACE("catch exception ");
2116  throw;
2117  }
2118 }
2119 
2130 void vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
2131 {
2132  try {
2133  vpColVector articularCoordinates = get_artCoord();
2134  vpAfma6::get_fJe(articularCoordinates, fJe_);
2135  }
2136  catch (...) {
2137  vpERROR_TRACE("Error caught");
2138  throw;
2139  }
2140 }
2141 
2145 void vpSimulatorAfma6::stopMotion()
2146 {
2147  if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
2148  return;
2149 
2150  vpColVector stop(6);
2151  stop = 0;
2152  set_artVel(stop);
2153  set_velocity(stop);
2155 }
2156 
2157 /**********************************************************************************/
2158 /**********************************************************************************/
2159 /**********************************************************************************/
2160 /**********************************************************************************/
2161 
2170 void vpSimulatorAfma6::initArms()
2171 {
2172  // set scene_dir from #define VISP_SCENE_DIR if it exists
2173  // VISP_SCENES_DIR may contain multiple locations separated by ";"
2174  std::string scene_dir_;
2175  std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
2176  bool sceneDirExists = false;
2177  for (size_t i = 0; i < scene_dirs.size(); i++)
2178  if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
2179  scene_dir_ = scene_dirs[i];
2180  sceneDirExists = true;
2181  break;
2182  }
2183  if (!sceneDirExists) {
2184  try {
2185  scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
2186  std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
2187  }
2188  catch (...) {
2189  std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
2190  }
2191  }
2192 
2193  unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
2194  if (scene_dir_.size() > FILENAME_MAX)
2195  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2196  unsigned int full_length = (unsigned int)scene_dir_.size() + name_length;
2197  if (full_length > FILENAME_MAX)
2198  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2199 
2200  char *name_cam = new char[full_length];
2201 
2202  strcpy(name_cam, scene_dir_.c_str());
2203  strcat(name_cam, "/camera.bnd");
2204  set_scene(name_cam, &camera, cameraFactor);
2205 
2206  if (arm_dir.size() > FILENAME_MAX)
2207  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2208  full_length = (unsigned int)arm_dir.size() + name_length;
2209  if (full_length > FILENAME_MAX)
2210  throw vpException(vpException::dimensionError, "Cannot initialize Afma6 simulator");
2211 
2212  char *name_arm = new char[full_length];
2213  strcpy(name_arm, arm_dir.c_str());
2214  strcat(name_arm, "/afma6_gate.bnd");
2215  std::cout << "name arm: " << name_arm << std::endl;
2216  set_scene(name_arm, robotArms, 1.0);
2217  strcpy(name_arm, arm_dir.c_str());
2218  strcat(name_arm, "/afma6_arm1.bnd");
2219  set_scene(name_arm, robotArms + 1, 1.0);
2220  strcpy(name_arm, arm_dir.c_str());
2221  strcat(name_arm, "/afma6_arm2.bnd");
2222  set_scene(name_arm, robotArms + 2, 1.0);
2223  strcpy(name_arm, arm_dir.c_str());
2224  strcat(name_arm, "/afma6_arm3.bnd");
2225  set_scene(name_arm, robotArms + 3, 1.0);
2226  strcpy(name_arm, arm_dir.c_str());
2227  strcat(name_arm, "/afma6_arm4.bnd");
2228  set_scene(name_arm, robotArms + 4, 1.0);
2229 
2231  tool = getToolType();
2232  strcpy(name_arm, arm_dir.c_str());
2233  switch (tool) {
2234  case vpAfma6::TOOL_CCMOP: {
2235  strcat(name_arm, "/afma6_tool_ccmop.bnd");
2236  break;
2237  }
2238  case vpAfma6::TOOL_GRIPPER: {
2239  strcat(name_arm, "/afma6_tool_gripper.bnd");
2240  break;
2241  }
2242  case vpAfma6::TOOL_VACUUM: {
2243  strcat(name_arm, "/afma6_tool_vacuum.bnd");
2244  break;
2245  }
2246  case vpAfma6::TOOL_CUSTOM: {
2247  std::cout << "The custom tool is not handled in vpSimulatorAfma6.cpp" << std::endl;
2248  break;
2249  }
2251  std::cout << "The Intel D435 camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2252  break;
2253  }
2255  std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
2256  break;
2257  }
2258  }
2259  set_scene(name_arm, robotArms + 5, 1.0);
2260 
2261  add_rfstack(IS_BACK);
2262 
2263  add_vwstack("start", "depth", 0.0, 100.0);
2264  add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
2265  add_vwstack("start", "type", PERSPECTIVE);
2266  //
2267  // sceneInitialized = true;
2268  // displayObject = true;
2269  displayCamera = true;
2270 
2271  delete[] name_cam;
2272  delete[] name_arm;
2273 }
2274 
2275 void vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
2276 {
2277  m_mutex_scene.lock();
2278  bool changed = false;
2279  vpHomogeneousMatrix displacement = navigation(I_, changed);
2280 
2281  // if (displacement[2][3] != 0)
2282  if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
2283  camMf2 = camMf2 * displacement;
2284 
2285  f2Mf = camMf2.inverse() * camMf;
2286 
2287  camMf = camMf2 * displacement * f2Mf;
2288 
2289  double u;
2290  double v;
2291  // if(px_ext != 1 && py_ext != 1)
2292  // we assume px_ext and py_ext > 0
2293  if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
2294  (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
2295  u = (double)I_.getWidth() / (2 * px_ext);
2296  v = (double)I_.getHeight() / (2 * py_ext);
2297  }
2298  else {
2299  u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2300  v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
2301  }
2302 
2303  float w44o[4][4], w44cext[4][4], x, y, z;
2304 
2305  vp2jlc_matrix(camMf.inverse(), w44cext);
2306 
2307  add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
2308  x = w44cext[2][0] + w44cext[3][0];
2309  y = w44cext[2][1] + w44cext[3][1];
2310  z = w44cext[2][2] + w44cext[3][2];
2311  add_vwstack("start", "vrp", x, y, z);
2312  add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
2313  add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
2314  add_vwstack("start", "window", -u, u, -v, v);
2315 
2316  vpHomogeneousMatrix fMit[8];
2317  get_fMi(fMit);
2318 
2319  vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
2320  display_scene(w44o, robotArms[0], I_, curColor);
2321 
2322  vp2jlc_matrix(fMit[0], w44o);
2323  display_scene(w44o, robotArms[1], I_, curColor);
2324 
2325  vp2jlc_matrix(fMit[2], w44o);
2326  display_scene(w44o, robotArms[2], I_, curColor);
2327 
2328  vp2jlc_matrix(fMit[3], w44o);
2329  display_scene(w44o, robotArms[3], I_, curColor);
2330 
2331  vp2jlc_matrix(fMit[4], w44o);
2332  display_scene(w44o, robotArms[4], I_, curColor);
2333 
2334  vp2jlc_matrix(fMit[5], w44o);
2335  display_scene(w44o, robotArms[5], I_, curColor);
2336 
2337  if (displayCamera) {
2338  vpHomogeneousMatrix cMe;
2339  get_cMe(cMe);
2340  cMe = cMe.inverse();
2341  cMe = fMit[6] * cMe;
2342  vp2jlc_matrix(cMe, w44o);
2343  display_scene(w44o, camera, I_, camColor);
2344  }
2345 
2346  if (displayObject) {
2347  vp2jlc_matrix(fMo, w44o);
2348  display_scene(w44o, scene, I_, curColor);
2349  }
2350  m_mutex_scene.unlock();
2351 }
2352 
2370 bool vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
2371 {
2372  vpColVector stop(6);
2373  bool status = true;
2374  stop = 0;
2375  m_mutex_scene.lock();
2376  set_artVel(stop);
2377  set_velocity(stop);
2378  vpHomogeneousMatrix fMc_;
2379  fMc_ = fMo * cMo_.inverse();
2380 
2381  vpColVector articularCoordinates = get_artCoord();
2382  int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
2383 
2384  if (nbSol == 0) {
2385  status = false;
2386  vpERROR_TRACE("Positioning error. Position unreachable");
2387  }
2388 
2389  if (verbose_)
2390  std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
2391 
2392  set_artCoord(articularCoordinates);
2393 
2394  compute_fMi();
2395  m_mutex_scene.unlock();
2396 
2397  return status;
2398 }
2399 
2413 void vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
2414 {
2415  vpColVector stop(6);
2416  stop = 0;
2417  m_mutex_scene.lock();
2418  set_artVel(stop);
2419  set_velocity(stop);
2420  vpHomogeneousMatrix fMit[8];
2421  get_fMi(fMit);
2422  fMo = fMit[7] * cMo_;
2423  m_mutex_scene.unlock();
2424 }
2425 
2437 bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
2438 {
2439  // get rid of max velocity
2440  double vMax = getMaxTranslationVelocity();
2441  double wMax = getMaxRotationVelocity();
2442  setMaxTranslationVelocity(10. * vMax);
2443  setMaxRotationVelocity(10. * wMax);
2444 
2445  vpColVector v(3), w(3), vel(6);
2446  vpHomogeneousMatrix cdMc;
2447  vpTranslationVector cdTc;
2448  vpRotationMatrix cdRc;
2449  vpThetaUVector cdTUc;
2450  vpColVector err(6);
2451  err = 1.;
2452  const double lambda = 5.;
2453 
2455 
2456  unsigned int i, iter = 0;
2457  while ((iter++ < 300) && (err.frobeniusNorm() > errMax)) {
2458  double t = vpTime::measureTimeMs();
2459 
2460  // update image
2461  if (Iint != nullptr) {
2462  vpDisplay::display(*Iint);
2463  getInternalView(*Iint);
2464  vpDisplay::flush(*Iint);
2465  }
2466 
2467  // update pose error
2468  cdMc = cdMo_ * get_cMo().inverse();
2469  cdMc.extract(cdRc);
2470  cdMc.extract(cdTc);
2471  cdTUc.buildFrom(cdRc);
2472 
2473  // compute v,w and velocity
2474  v = -lambda * cdRc.t() * cdTc;
2475  w = -lambda * cdTUc;
2476  for (i = 0; i < 3; ++i) {
2477  vel[i] = v[i];
2478  vel[i + 3] = w[i];
2479  err[i] = cdTc[i];
2480  err[i + 3] = cdTUc[i];
2481  }
2482 
2483  // update feat
2484  setVelocity(vpRobot::CAMERA_FRAME, vel);
2485 
2486  // wait for it
2487  vpTime::wait(t, 10);
2488  }
2489  vel = 0.;
2490  set_velocity(vel);
2491  set_artVel(vel);
2492  setMaxTranslationVelocity(vMax);
2493  setMaxRotationVelocity(wMax);
2494 
2495  // std::cout << "setPosition: final error " << err.t() << std::endl;
2496  return (err.frobeniusNorm() <= errMax);
2497 }
2498 END_VISP_NAMESPACE
2499 #elif !defined(VISP_BUILD_SHARED_LIBS)
2500 // Work around to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
2501 // no symbols
2502 void dummy_vpSimulatorAfma6() { };
2503 #endif
Modelization of Irisa's gantry robot named Afma6.
Definition: vpAfma6.h:78
static const char *const CONST_CCMOP_CAMERA_NAME
Definition: vpAfma6.h:101
void get_cMe(vpHomogeneousMatrix &cMe) const
Definition: vpAfma6.cpp:897
static const char *const CONST_GRIPPER_CAMERA_NAME
Definition: vpAfma6.h:106
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
Definition: vpAfma6.cpp:784
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpAfma6.cpp:941
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpAfma6.cpp:1011
vpAfma6ToolType
List of possible tools that can be attached to the robot end-effector.
Definition: vpAfma6.h:126
@ TOOL_CCMOP
Definition: vpAfma6.h:127
@ TOOL_GENERIC_CAMERA
Definition: vpAfma6.h:130
@ TOOL_CUSTOM
Definition: vpAfma6.h:132
@ TOOL_VACUUM
Definition: vpAfma6.h:129
@ TOOL_INTEL_D435_CAMERA
Definition: vpAfma6.h:131
@ TOOL_GRIPPER
Definition: vpAfma6.h:128
unsigned int getRows() const
Definition: vpArray2D.h:347
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:191
double sumSquare() const
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
static const vpColor none
Definition: vpColor.h:229
static const vpColor green
Definition: vpColor.h:220
static void display(const vpImage< unsigned char > &I)
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void flush(const vpImage< unsigned char > &I)
static void displayCamera(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)
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:242
unsigned int getHeight() const
Definition: vpImage.h:181
void init(unsigned int h, unsigned int w, Type value)
Definition: vpImage.h:376
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1661
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:396
static std::string getenv(const std::string &env)
Definition: vpIoTools.cpp:326
static double rad(double deg)
Definition: vpMath.h:129
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:254
static Type minimum(const Type &a, const Type &b)
Definition: vpMath.h:262
static double deg(double rad)
Definition: vpMath.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix pseudoInverse(double svThreshold=1e-6) const
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:79
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
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.
vpControlFrameType
Definition: vpRobot.h:77
@ REFERENCE_FRAME
Definition: vpRobot.h:78
@ ARTICULAR_FRAME
Definition: vpRobot.h:80
@ MIXT_FRAME
Definition: vpRobot.h:88
@ CAMERA_FRAME
Definition: vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:83
vpRobotStateType
Definition: vpRobot.h:65
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:68
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:67
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:69
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:202
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector & buildFrom(const vpHomogeneousMatrix &M)
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double getMinTimeForUsleepCall()
VISP_EXPORT double measureTimeSecond()
VISP_EXPORT double measureTimeMs()