Visual Servoing Platform  version 3.3.1 under development (2020-02-21)
vpRobotBiclops.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Interface for the Biclops robot.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <cmath> // std::fabs
40 #include <errno.h>
41 #include <limits> // numeric_limits
42 #include <signal.h>
43 #include <string.h>
44 
45 #include <visp3/core/vpConfig.h>
46 
47 #ifdef VISP_HAVE_BICLOPS
48 
49 #include <visp3/core/vpTime.h>
50 #include <visp3/core/vpExponentialMap.h>
51 #include <visp3/core/vpIoTools.h>
52 #include <visp3/robot/vpBiclops.h>
53 #include <visp3/robot/vpRobotBiclops.h>
54 #include <visp3/robot/vpRobotException.h>
55 
56 //#define VP_DEBUG // Activate the debug mode
57 //#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
58 #include <visp3/core/vpDebug.h>
59 
60 /* ---------------------------------------------------------------------- */
61 /* --- STATIC ------------------------------------------------------------ */
62 /* ------------------------------------------------------------------------ */
63 
64 bool vpRobotBiclops::robotAlreadyCreated = false;
66 
67 static pthread_mutex_t vpEndThread_mutex;
68 static pthread_mutex_t vpShm_mutex;
69 static pthread_mutex_t vpMeasure_mutex;
70 
71 /* ----------------------------------------------------------------------- */
72 /* --- CONSTRUCTOR ------------------------------------------------------ */
73 /* ---------------------------------------------------------------------- */
74 
113  : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
114  q_previous(), controlThreadCreated(false)
115 {
116  vpDEBUG_TRACE(12, "Begin default constructor.");
117 
118  vpRobotBiclops::robotAlreadyCreated = false;
119  setConfigFile("/usr/share/BiclopsDefault.cfg");
120 
121  // Initialize the mutex dedicated to she shm protection
122  pthread_mutex_init(&vpShm_mutex, NULL);
123  pthread_mutex_init(&vpEndThread_mutex, NULL);
124  pthread_mutex_init(&vpMeasure_mutex, NULL);
125 
126  control_thread = 0;
127 }
128 
159 vpRobotBiclops::vpRobotBiclops(const std::string &filename)
160  : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
161  q_previous(), controlThreadCreated(false)
162 {
163  vpDEBUG_TRACE(12, "Begin default constructor.");
164 
165  vpRobotBiclops::robotAlreadyCreated = false;
166  setConfigFile(filename);
167 
168  // Initialize the mutex dedicated to she shm protection
169  pthread_mutex_init(&vpShm_mutex, NULL);
170  pthread_mutex_init(&vpEndThread_mutex, NULL);
171  pthread_mutex_init(&vpMeasure_mutex, NULL);
172 
173  init();
174 
175  return;
176 }
177 
186 {
187 
188  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
190 
191  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
192  pthread_mutex_unlock(&vpEndThread_mutex);
193 
194  /* wait the end of the control thread */
195  vpDEBUG_TRACE(12, "Wait end of control thread");
196 
197  if (controlThreadCreated == true) {
198  int code = pthread_join(control_thread, NULL);
199  if (code != 0) {
200  vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno)
201  << " strCode=" << strerror(code) << std::endl;
202  }
203  }
204 
205  pthread_mutex_destroy(&vpShm_mutex);
206  pthread_mutex_destroy(&vpEndThread_mutex);
207  pthread_mutex_destroy(&vpMeasure_mutex);
208 
209  vpRobotBiclops::robotAlreadyCreated = false;
210 
211  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
212  return;
213 }
214 
215 /* -------------------------------------------------------------------------
216  */
217 /* --- INITIALISATION ------------------------------------------------------
218  */
219 /* -------------------------------------------------------------------------
220  */
221 
227 void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; }
228 
239 {
240  // test if the config file exists
241  FILE *fd = fopen(configfile.c_str(), "r");
242  if (fd == NULL) {
243  vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
244  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
245  }
246  fclose(fd);
247 
248  // Initialize the controller
249  controller.init(configfile);
250 
251  try {
253  } catch (...) {
254  vpERROR_TRACE("Error caught");
255  throw;
256  }
257 
258  vpRobotBiclops::robotAlreadyCreated = true;
259 
260  // Initialize previous articular position to manage getDisplacement()
261  q_previous.resize(vpBiclops::ndof);
262  q_previous = 0;
263 
264  controlThreadCreated = false;
265 
266  return;
267 }
268 
269 /*
270  Control loop to manage the biclops joint limits in speed control.
271 
272  This control loop is running in a seperate thread in order to detect each 5
273  ms joint limits during the speed control. If a joint limit is detected the
274  axis should be halted.
275 
276  \warning Velocity control mode is not exported from the top-level Biclops
277  API class provided by Traclabs. That means that there is no protection in
278  this mode to prevent an axis from striking its hard limit. In position mode,
279  Traclabs put soft limits in that keep any command from driving to a position
280  too close to the hard limits. In velocity mode this protection does not
281  exist in the current API.
282 
283  \warning With the understanding that hitting the hard limits at full
284  speed/power can damage the unit, damage due to velocity mode commanding is
285  under user responsibility.
286 */
288 {
289  vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
290 
291  int iter = 0;
292  // PMDAxisControl *panAxis = controller->getPanAxis();
293  // PMDAxisControl *tiltAxis = controller->getTiltAxis();
294  vpRobotBiclopsController::shmType shm;
295 
296  vpDEBUG_TRACE(10, "Start control loop");
297  vpColVector mes_q;
298  vpColVector mes_q_dot;
299  vpColVector softLimit(vpBiclops::ndof);
301  bool *new_q_dot = new bool[vpBiclops::ndof];
302  bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
303  bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
304  bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
305  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
306  double secure = vpMath::rad(2); // add a security angle before joint limit
307 
308  // Set the soft limits
309  softLimit[0] = vpBiclops::panJointLimit - secure;
310  softLimit[1] = vpBiclops::tiltJointLimit - secure;
311  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1]));
312 
313  // Initilisation
314  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
315  pthread_mutex_lock(&vpShm_mutex);
316 
317  shm = controller->readShm();
318 
319  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
320  pthread_mutex_unlock(&vpShm_mutex);
321 
322  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
323  prev_q_dot[i] = shm.q_dot[i];
324  new_q_dot[i] = false;
325  change_dir[i] = false;
326  force_halt[i] = false;
327  enable_limit[i] = true;
328  }
329 
330  // Initialize actual position and velocity
331  mes_q = controller->getActualPosition();
332  mes_q_dot = controller->getActualVelocity();
333 
334  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
335  pthread_mutex_lock(&vpShm_mutex);
336 
337  shm = controller->readShm();
338  // Updates the shm
339  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
340  shm.actual_q[i] = mes_q[i];
341  shm.actual_q_dot[i] = mes_q_dot[i];
342  }
343  // Update the actuals positions
344  controller->writeShm(shm);
345 
346  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
347  pthread_mutex_unlock(&vpShm_mutex);
348 
349  vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
350  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
351 
352  while (!controller->isStopRequested()) {
353 
354  // Get actual position and velocity
355  mes_q = controller->getActualPosition();
356  mes_q_dot = controller->getActualVelocity();
357 
358  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
359  pthread_mutex_lock(&vpShm_mutex);
360 
361  shm = controller->readShm();
362 
363  // Updates the shm
364  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
365  shm.actual_q[i] = mes_q[i];
366  shm.actual_q_dot[i] = mes_q_dot[i];
367  }
368 
369  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
370  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
371  vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
372  vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
373 
374  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
375  // test if joint limits are reached
376  if (mes_q[i] < -softLimit[i]) {
377  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
378  shm.status[i] = vpRobotBiclopsController::STOP;
379  shm.jointLimit[i] = true;
380  } else if (mes_q[i] > softLimit[i]) {
381  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
382  shm.status[i] = vpRobotBiclopsController::STOP;
383  shm.jointLimit[i] = true;
384  } else {
385  shm.status[i] = vpRobotBiclopsController::SPEED;
386  shm.jointLimit[i] = false;
387  }
388 
389  // Test if new a speed is demanded
390  // if (shm.q_dot[i] != prev_q_dot[i])
391  if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
392  std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
393  new_q_dot[i] = true;
394  else
395  new_q_dot[i] = false;
396 
397  // Test if desired speed change of sign
398  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
399  change_dir[i] = true;
400  else
401  change_dir[i] = false;
402  }
403  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
404  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
405  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
406  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
407  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
408  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
409 
410  bool updateVelocity = false;
411  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
412  // Test if a new desired speed is to apply
413  if (new_q_dot[i]) {
414  // A new desired speed is to apply
415  if (shm.status[i] == vpRobotBiclopsController::STOP) {
416  // Axis in joint limit
417  if (change_dir[i] == false) {
418  // New desired speed without change of direction
419  // We go in the joint limit
420  if (enable_limit[i] == true) { // limit detection active
421  // We have to stop this axis
422  // Test if this axis was stopped before
423  if (force_halt[i] == false) {
424  q_dot[i] = 0.;
425  force_halt[i] = true; // indicate that it will be stopped
426  updateVelocity = true; // We have to send this new speed
427  }
428  } else {
429  // We have to apply the desired speed to go away the joint
430  // Update the desired speed
431  q_dot[i] = shm.q_dot[i];
432  shm.status[i] = vpRobotBiclopsController::SPEED;
433  force_halt[i] = false;
434  updateVelocity = true; // We have to send this new speed
435  }
436  } else {
437  // New desired speed and change of direction.
438  if (enable_limit[i] == true) { // limit detection active
439  // Update the desired speed to go away the joint limit
440  q_dot[i] = shm.q_dot[i];
441  shm.status[i] = vpRobotBiclopsController::SPEED;
442  force_halt[i] = false;
443  enable_limit[i] = false; // Disable joint limit detection
444  updateVelocity = true; // We have to send this new speed
445  } else {
446  // We have to stop this axis
447  // Test if this axis was stopped before
448  if (force_halt[i] == false) {
449  q_dot[i] = 0.;
450  force_halt[i] = true; // indicate that it will be stopped
451  enable_limit[i] = true; // Joint limit detection must be active
452  updateVelocity = true; // We have to send this new speed
453  }
454  }
455  }
456  } else {
457  // Axis not in joint limit
458 
459  // Update the desired speed
460  q_dot[i] = shm.q_dot[i];
461  shm.status[i] = vpRobotBiclopsController::SPEED;
462  enable_limit[i] = true; // Joint limit detection must be active
463  updateVelocity = true; // We have to send this new speed
464  }
465  } else {
466  // No change of the desired speed. We have to stop the robot in case
467  // of joint limit
468  if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
469  if (enable_limit[i] == true) { // limit detection active
470 
471  // Test if this axis was stopped before
472  if (force_halt[i] == false) {
473  // We have to stop this axis
474  q_dot[i] = 0.;
475  force_halt[i] = true; // indicate that it will be stopped
476  updateVelocity = true; // We have to send this new speed
477  }
478  }
479  } else {
480  // No need to stop the robot
481  enable_limit[i] = true; // Normal situation, activate limit detection
482  }
483  }
484  }
485  // Update the actuals positions
486  controller->writeShm(shm);
487 
488  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
489  pthread_mutex_unlock(&vpShm_mutex);
490 
491  if (updateVelocity) {
492  vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1]));
493 
494  // Apply the velocity
495  controller->setVelocity(q_dot);
496  }
497 
498  // Update the previous speed for next iteration
499  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
500  prev_q_dot[i] = shm.q_dot[i];
501 
502  vpDEBUG_TRACE(12, "iter: %d", iter);
503 
504  // wait 5 ms
505  vpTime::wait(5.0);
506 
507  // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
508  // vpDEBUG_TRACE (12, "Calling thread will end");
509  // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
510  // std::cout << "Calling thread will end" << std::endl;
511  // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
512  //
513  // pthread_mutex_unlock(&vpEndThread_mutex);
514  // break;
515  // }
516 
517  iter++;
518  }
519  controller->stopRequest(false);
520  // Stop the robot
521  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
522  q_dot = 0;
523  controller->setVelocity(q_dot);
524 
525  delete[] new_q_dot;
526  delete[] change_dir;
527  delete[] force_halt;
528  delete[] enable_limit;
529  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
530  pthread_mutex_unlock(&vpEndThread_mutex);
531 
532  vpDEBUG_TRACE(10, "Exit control thread ");
533  // pthread_exit(0);
534 
535  return NULL;
536 }
537 
545 {
546  switch (newState) {
547  case vpRobot::STATE_STOP: {
549  stopMotion();
550  }
551  break;
552  }
555  vpDEBUG_TRACE(12, "Speed to position control.");
556  stopMotion();
557  }
558 
559  break;
560  }
562 
564  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
565  pthread_mutex_lock(&vpEndThread_mutex);
566 
567  vpDEBUG_TRACE(12, "Create speed control thread");
568  int code;
569  code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller);
570  if (code != 0) {
571  vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno)
572  << " strCode=" << strerror(code) << std::endl;
573  }
574 
575  controlThreadCreated = true;
576 
577  vpDEBUG_TRACE(12, "Speed control thread created");
578  }
579  break;
580  }
581  default:
582  break;
583  }
584 
585  return vpRobot::setRobotState(newState);
586 }
587 
594 {
596  q_dot = 0;
597  controller.setVelocity(q_dot);
598  // std::cout << "Request to stop the velocity controller thread...."<<
599  // std::endl;
600  controller.stopRequest(true);
601 }
602 
614 {
616  cMe = vpBiclops::get_cMe();
617 
618  cVe.buildFrom(cMe);
619 }
620 
631 
643 {
644  vpColVector q(2);
646 
647  try {
648  vpBiclops::get_eJe(q, _eJe);
649  } catch (...) {
650  vpERROR_TRACE("catch exception ");
651  throw;
652  }
653 }
654 
663 {
664  vpColVector q(2);
666 
667  try {
668  vpBiclops::get_fJe(q, _fJe);
669  } catch (...) {
670  vpERROR_TRACE("Error caught");
671  throw;
672  }
673 }
674 
683 {
684  if (velocity < 0 || velocity > 100) {
685  vpERROR_TRACE("Bad positionning velocity");
686  throw vpRobotException(vpRobotException::constructionError, "Bad positionning velocity");
687  }
688 
689  positioningVelocity = velocity;
690 }
698 double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; }
699 
716 {
717 
719  vpERROR_TRACE("Robot was not in position-based control\n"
720  "Modification of the robot state");
722  }
723 
724  switch (frame) {
726  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
727  "not implemented");
728  break;
730  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
731  "not implemented");
732  break;
733  case vpRobot::MIXT_FRAME:
734  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
735  "not implemented");
736  break;
738  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
739  "not implemented");
740  break;
742  break;
743  }
744 
745  // test if position reachable
746  // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
747  // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
748  // vpERROR_TRACE ("Positionning error.");
749  // throw vpRobotException (vpRobotException::wrongStateError,
750  // "Positionning error.");
751  // }
752 
753  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
754  pthread_mutex_lock(&vpEndThread_mutex);
755  controller.setPosition(q, positioningVelocity);
756  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
757  pthread_mutex_unlock(&vpEndThread_mutex);
758  return;
759 }
760 
777 void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
778 {
779  try {
780  vpColVector q(2);
781  q[0] = q1;
782  q[1] = q2;
783 
784  setPosition(frame, q);
785  } catch (...) {
786  vpERROR_TRACE("Error caught");
787  throw;
788  }
789 }
790 
804 void vpRobotBiclops::setPosition(const char *filename)
805 {
806  vpColVector q;
807  if (readPositionFile(filename, q) == false) {
808  vpERROR_TRACE("Cannot get biclops position from file");
809  throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file");
810  }
812 }
813 
830 {
831  switch (frame) {
833  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
834  "not implemented");
835  break;
837  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
838  "not implemented");
839  break;
840  case vpRobot::MIXT_FRAME:
841  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
842  "not implemented");
843  break;
845  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
846  "not implemented");
847  break;
849  break;
850  }
851 
853  state = vpRobot::getRobotState();
854 
855  switch (state) {
856  case STATE_STOP:
858  q = controller.getPosition();
859 
860  break;
863  default:
865 
866  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
867  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
868 
869  vpRobotBiclopsController::shmType shm;
870 
871  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
872  pthread_mutex_lock(&vpShm_mutex);
873 
874  shm = controller.readShm();
875 
876  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
877  pthread_mutex_unlock(&vpShm_mutex);
878 
879  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
880  q[i] = shm.actual_q[i];
881  }
882 
883  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
884 
885  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
886  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
887 
888  break;
889  }
890 }
891 
919 {
921  vpERROR_TRACE("Cannot send a velocity to the robot "
922  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
924  "Cannot send a velocity to the robot "
925  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
926  }
927 
928  switch (frame) {
929  case vpRobot::CAMERA_FRAME: {
930  vpERROR_TRACE("Cannot send a velocity to the robot "
931  "in the camera frame: "
932  "functionality not implemented");
933  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
934  "in the camera frame:"
935  "functionality not implemented");
936  }
938  if (q_dot.getRows() != 2) {
939  vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
940  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
941  "in articular frame");
942  }
943  break;
944  }
946  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
947  "in the reference frame:"
948  "functionality not implemented");
949  }
950  case vpRobot::MIXT_FRAME: {
951  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
952  "in the mixt frame:"
953  "functionality not implemented");
954  }
956  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
957  "in the end-effector frame:"
958  "functionality not implemented");
959  }
960  default: {
961  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
962  }
963  }
964 
965  vpDEBUG_TRACE(12, "Velocity limitation.");
966  bool norm = false; // Flag to indicate when velocities need to be nomalized
967 
968  // Saturate articular speed
969  double max = vpBiclops::speedLimit;
970  vpColVector q_dot_sat(vpBiclops::ndof);
971 
972  // init q_dot_saturated
973  q_dot_sat = q_dot;
974 
975  for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
976  {
977  if (fabs(q_dot[i]) > max) {
978  norm = true;
979  max = fabs(q_dot[i]);
980  vpERROR_TRACE("Excess velocity: ROTATION "
981  "(axe nr.%d).",
982  i);
983  }
984  }
985  // Rotations velocities normalisation
986  if (norm == true) {
987  max = vpBiclops::speedLimit / max;
988  q_dot_sat = q_dot * max;
989  }
990 
991  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
992 
993  vpRobotBiclopsController::shmType shm;
994 
995  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
996  pthread_mutex_lock(&vpShm_mutex);
997 
998  shm = controller.readShm();
999 
1000  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1001  shm.q_dot[i] = q_dot[i];
1002 
1003  controller.writeShm(shm);
1004 
1005  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1006  pthread_mutex_unlock(&vpShm_mutex);
1007 
1008  return;
1009 }
1010 
1011 /* -------------------------------------------------------------------------
1012  */
1013 /* --- GET -----------------------------------------------------------------
1014  */
1015 /* -------------------------------------------------------------------------
1016  */
1017 
1030 {
1031  switch (frame) {
1032  case vpRobot::CAMERA_FRAME:
1033  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
1034  "not implemented");
1035  break;
1037  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
1038  "not implemented");
1039  break;
1040  case vpRobot::MIXT_FRAME:
1041  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
1042  "not implemented");
1043  break;
1045  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
1046  "not implemented");
1047  break;
1049  break;
1050  }
1051 
1053  state = vpRobot::getRobotState();
1054 
1055  switch (state) {
1056  case STATE_STOP:
1058  q_dot = controller.getVelocity();
1059 
1060  break;
1063  default:
1064  q_dot.resize(vpBiclops::ndof);
1065 
1066  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
1067  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1068 
1069  vpRobotBiclopsController::shmType shm;
1070 
1071  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
1072  pthread_mutex_lock(&vpShm_mutex);
1073 
1074  shm = controller.readShm();
1075 
1076  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1077  pthread_mutex_unlock(&vpShm_mutex);
1078 
1079  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
1080  q_dot[i] = shm.actual_q_dot[i];
1081  }
1082 
1083  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1084 
1085  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
1086  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1087 
1088  break;
1089  }
1090 }
1091 
1104 {
1105  vpColVector q_dot;
1106  getVelocity(frame, q_dot);
1107 
1108  return q_dot;
1109 }
1110 
1130 bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
1131 {
1132  std::ifstream fd(filename.c_str(), std::ios::in);
1133 
1134  if (!fd.is_open()) {
1135  return false;
1136  }
1137 
1138  std::string line;
1139  std::string key("R:");
1140  std::string id("#PTU-EVI - Position");
1141  bool pos_found = false;
1142  int lineNum = 0;
1143 
1145 
1146  while (std::getline(fd, line)) {
1147  lineNum++;
1148  if (lineNum == 1) {
1149  if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
1150  std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
1151  return false;
1152  }
1153  }
1154  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1155  continue;
1156  }
1157  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1158  // check if there are at least njoint values in the line
1159  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1160  if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator
1161  chain = vpIoTools::splitChain(line, std::string("\t"));
1162  if (chain.size() < vpBiclops::ndof + 1)
1163  continue;
1164 
1165  std::istringstream ss(line);
1166  std::string key_;
1167  ss >> key_;
1168  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1169  ss >> q[i];
1170  pos_found = true;
1171  break;
1172  }
1173  }
1174 
1175  // converts rotations from degrees into radians
1176  q.deg2rad();
1177 
1178  fd.close();
1179 
1180  if (!pos_found) {
1181  std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
1182  return false;
1183  }
1184 
1185  return true;
1186 }
1187 
1211 {
1212  vpColVector q_current; // current position
1213 
1215 
1216  switch (frame) {
1219  d = q_current - q_previous;
1220  break;
1221 
1222  case vpRobot::CAMERA_FRAME: {
1223  d.resize(6);
1224  vpHomogeneousMatrix fMc_current;
1225  vpHomogeneousMatrix fMc_previous;
1226  fMc_current = vpBiclops::get_fMc(q_current);
1227  fMc_previous = vpBiclops::get_fMc(q_previous);
1228  vpHomogeneousMatrix c_previousMc_current;
1229  // fMc_c = fMc_p * c_pMc_c
1230  // => c_pMc_c = (fMc_p)^-1 * fMc_c
1231  c_previousMc_current = fMc_previous.inverse() * fMc_current;
1232 
1233  // Compute the instantaneous velocity from this homogeneous matrix.
1234  d = vpExponentialMap::inverse(c_previousMc_current);
1235  break;
1236  }
1237 
1239  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
1240  "functionality not implemented");
1241  break;
1242  case vpRobot::MIXT_FRAME:
1243  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
1244  "functionality not implemented");
1245  break;
1247  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
1248  "functionality not implemented");
1249  break;
1250  }
1251 
1252  q_previous = q_current; // Update for next call of this method
1253 }
1254 
1255 #elif !defined(VISP_BUILD_SHARED_LIBS)
1256 // Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no
1257 // symbols
1258 void dummy_vpRobotBiclops(){};
1259 #endif
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:77
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:157
static vpColVector inverse(const vpHomogeneousMatrix &M)
virtual ~vpRobotBiclops()
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
bool readPositionFile(const std::string &filename, vpColVector &q)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
#define vpCERROR
Definition: vpDebug.h:365
static const unsigned int ndof
Definition: vpBiclops.h:126
void get_eJe(vpMatrix &_eJe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const float tiltJointLimit
Definition: vpBiclops.h:132
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const float speedLimit
Definition: vpBiclops.h:133
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:360
void setPositioningVelocity(double velocity)
#define vpERROR_TRACE
Definition: vpDebug.h:393
Initialize the position controller.
Definition: vpRobot.h:67
void setConfigFile(const std::string &filename="/usr/share/BiclopsDefault.cfg")
unsigned int getRows() const
Definition: vpArray2D.h:289
vpHomogeneousMatrix inverse() const
vpRowVector t() const
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
void get_fJe(vpMatrix &_fJe)
vpControlFrameType
Definition: vpRobot.h:75
static const double defaultPositioningVelocity
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
void get_cVe(vpVelocityTwistMatrix &_cVe) const
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void deg2rad()
Definition: vpColVector.h:196
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
static const float panJointLimit
Definition: vpBiclops.h:131
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpRobotStateType
Definition: vpRobot.h:64
Initialize the acceleration controller.
Definition: vpRobot.h:68
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1767
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:95
static double rad(double deg)
Definition: vpMath.h:108
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static double deg(double rad)
Definition: vpMath.h:101
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d)
double getPositioningVelocity(void)
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:394
static void * vpRobotBiclopsSpeedControlLoop(void *arg)