Visual Servoing Platform  version 3.2.1 under development (2019-05-26)
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/vpTime.h>
46 
47 #include <visp3/core/vpConfig.h>
48 
49 #ifdef VISP_HAVE_BICLOPS
50 
51 #include <visp3/core/vpExponentialMap.h>
52 #include <visp3/core/vpIoTools.h>
53 #include <visp3/robot/vpBiclops.h>
54 #include <visp3/robot/vpRobotBiclops.h>
55 #include <visp3/robot/vpRobotException.h>
56 
57 //#define VP_DEBUG // Activate the debug mode
58 //#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
59 #include <visp3/core/vpDebug.h>
60 
61 /* ---------------------------------------------------------------------- */
62 /* --- STATIC ------------------------------------------------------------ */
63 /* ------------------------------------------------------------------------ */
64 
65 bool vpRobotBiclops::robotAlreadyCreated = false;
67 
68 static pthread_mutex_t vpEndThread_mutex;
69 static pthread_mutex_t vpShm_mutex;
70 static pthread_mutex_t vpMeasure_mutex;
71 
72 /* ----------------------------------------------------------------------- */
73 /* --- CONSTRUCTOR ------------------------------------------------------ */
74 /* ---------------------------------------------------------------------- */
75 
115  : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
116  q_previous(), controlThreadCreated(false)
117 {
118  vpDEBUG_TRACE(12, "Begin default constructor.");
119 
120  vpRobotBiclops::robotAlreadyCreated = false;
121  setConfigFile("/usr/share/BiclopsDefault.cfg");
122 
123  // Initialize the mutex dedicated to she shm protection
124  pthread_mutex_init(&vpShm_mutex, NULL);
125  pthread_mutex_init(&vpEndThread_mutex, NULL);
126  pthread_mutex_init(&vpMeasure_mutex, NULL);
127 
128  control_thread = 0;
129 }
130 
164 vpRobotBiclops::vpRobotBiclops(const std::string &filename)
165  : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
166  q_previous(), controlThreadCreated(false)
167 {
168  vpDEBUG_TRACE(12, "Begin default constructor.");
169 
170  vpRobotBiclops::robotAlreadyCreated = false;
171  setConfigFile(filename);
172 
173  // Initialize the mutex dedicated to she shm protection
174  pthread_mutex_init(&vpShm_mutex, NULL);
175  pthread_mutex_init(&vpEndThread_mutex, NULL);
176  pthread_mutex_init(&vpMeasure_mutex, NULL);
177 
178  init();
179 
180  return;
181 }
182 
191 {
192 
193  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
195 
196  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
197  pthread_mutex_unlock(&vpEndThread_mutex);
198 
199  /* wait the end of the control thread */
200  vpDEBUG_TRACE(12, "Wait end of control thread");
201 
202  if (controlThreadCreated == true) {
203  int code = pthread_join(control_thread, NULL);
204  if (code != 0) {
205  vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno)
206  << " strCode=" << strerror(code) << std::endl;
207  }
208  }
209 
210  pthread_mutex_destroy(&vpShm_mutex);
211  pthread_mutex_destroy(&vpEndThread_mutex);
212  pthread_mutex_destroy(&vpMeasure_mutex);
213 
214  vpRobotBiclops::robotAlreadyCreated = false;
215 
216  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
217  return;
218 }
219 
220 /* -------------------------------------------------------------------------
221  */
222 /* --- INITIALISATION ------------------------------------------------------
223  */
224 /* -------------------------------------------------------------------------
225  */
226 
232 void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; }
233 
244 {
245  // test if the config file exists
246  FILE *fd = fopen(configfile.c_str(), "r");
247  if (fd == NULL) {
248  vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
249  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
250  }
251  fclose(fd);
252 
253  // Initialize the controller
254  controller.init(configfile);
255 
256  try {
258  } catch (...) {
259  vpERROR_TRACE("Error caught");
260  throw;
261  }
262 
263  vpRobotBiclops::robotAlreadyCreated = true;
264 
265  // Initialize previous articular position to manage getDisplacement()
266  q_previous.resize(vpBiclops::ndof);
267  q_previous = 0;
268 
269  controlThreadCreated = false;
270 
271  return;
272 }
273 
274 /*
275  Control loop to manage the biclops joint limits in speed control.
276 
277  This control loop is running in a seperate thread in order to detect each 5
278  ms joint limits during the speed control. If a joint limit is detected the
279  axis should be halted.
280 
281  \warning Velocity control mode is not exported from the top-level Biclops
282  API class provided by Traclabs. That means that there is no protection in
283  this mode to prevent an axis from striking its hard limit. In position mode,
284  Traclabs put soft limits in that keep any command from driving to a position
285  too close to the hard limits. In velocity mode this protection does not
286  exist in the current API.
287 
288  \warning With the understanding that hitting the hard limits at full
289  speed/power can damage the unit, damage due to velocity mode commanding is
290  under user responsibility.
291 */
293 {
294  vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
295 
296  int iter = 0;
297  // PMDAxisControl *panAxis = controller->getPanAxis();
298  // PMDAxisControl *tiltAxis = controller->getTiltAxis();
299  vpRobotBiclopsController::shmType shm;
300 
301  vpDEBUG_TRACE(10, "Start control loop");
302  vpColVector mes_q;
303  vpColVector mes_q_dot;
304  vpColVector softLimit(vpBiclops::ndof);
306  bool *new_q_dot = new bool[vpBiclops::ndof];
307  bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
308  bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
309  bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
310  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
311  double secure = vpMath::rad(2); // add a security angle before joint limit
312 
313  // Set the soft limits
314  softLimit[0] = vpBiclops::panJointLimit - secure;
315  softLimit[1] = vpBiclops::tiltJointLimit - secure;
316  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1]));
317 
318  // Initilisation
319  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
320  pthread_mutex_lock(&vpShm_mutex);
321 
322  shm = controller->readShm();
323 
324  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
325  pthread_mutex_unlock(&vpShm_mutex);
326 
327  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
328  prev_q_dot[i] = shm.q_dot[i];
329  new_q_dot[i] = false;
330  change_dir[i] = false;
331  force_halt[i] = false;
332  enable_limit[i] = true;
333  }
334 
335  // Initialize actual position and velocity
336  mes_q = controller->getActualPosition();
337  mes_q_dot = controller->getActualVelocity();
338 
339  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
340  pthread_mutex_lock(&vpShm_mutex);
341 
342  shm = controller->readShm();
343  // Updates the shm
344  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
345  shm.actual_q[i] = mes_q[i];
346  shm.actual_q_dot[i] = mes_q_dot[i];
347  }
348  // Update the actuals positions
349  controller->writeShm(shm);
350 
351  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
352  pthread_mutex_unlock(&vpShm_mutex);
353 
354  vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
355  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
356 
357  while (!controller->isStopRequested()) {
358 
359  // Get actual position and velocity
360  mes_q = controller->getActualPosition();
361  mes_q_dot = controller->getActualVelocity();
362 
363  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
364  pthread_mutex_lock(&vpShm_mutex);
365 
366  shm = controller->readShm();
367 
368  // Updates the shm
369  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
370  shm.actual_q[i] = mes_q[i];
371  shm.actual_q_dot[i] = mes_q_dot[i];
372  }
373 
374  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
375  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
376  vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
377  vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
378 
379  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
380  // test if joint limits are reached
381  if (mes_q[i] < -softLimit[i]) {
382  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
383  shm.status[i] = vpRobotBiclopsController::STOP;
384  shm.jointLimit[i] = true;
385  } else if (mes_q[i] > softLimit[i]) {
386  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
387  shm.status[i] = vpRobotBiclopsController::STOP;
388  shm.jointLimit[i] = true;
389  } else {
390  shm.status[i] = vpRobotBiclopsController::SPEED;
391  shm.jointLimit[i] = false;
392  }
393 
394  // Test if new a speed is demanded
395  // if (shm.q_dot[i] != prev_q_dot[i])
396  if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
397  std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
398  new_q_dot[i] = true;
399  else
400  new_q_dot[i] = false;
401 
402  // Test if desired speed change of sign
403  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
404  change_dir[i] = true;
405  else
406  change_dir[i] = false;
407  }
408  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
409  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
410  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
411  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
412  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
413  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
414 
415  bool updateVelocity = false;
416  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
417  // Test if a new desired speed is to apply
418  if (new_q_dot[i]) {
419  // A new desired speed is to apply
420  if (shm.status[i] == vpRobotBiclopsController::STOP) {
421  // Axis in joint limit
422  if (change_dir[i] == false) {
423  // New desired speed without change of direction
424  // We go in the joint limit
425  if (enable_limit[i] == true) { // limit detection active
426  // We have to stop this axis
427  // Test if this axis was stopped before
428  if (force_halt[i] == false) {
429  q_dot[i] = 0.;
430  force_halt[i] = true; // indicate that it will be stopped
431  updateVelocity = true; // We have to send this new speed
432  }
433  } else {
434  // We have to apply the desired speed to go away the joint
435  // Update the desired speed
436  q_dot[i] = shm.q_dot[i];
437  shm.status[i] = vpRobotBiclopsController::SPEED;
438  force_halt[i] = false;
439  updateVelocity = true; // We have to send this new speed
440  }
441  } else {
442  // New desired speed and change of direction.
443  if (enable_limit[i] == true) { // limit detection active
444  // Update the desired speed to go away the joint limit
445  q_dot[i] = shm.q_dot[i];
446  shm.status[i] = vpRobotBiclopsController::SPEED;
447  force_halt[i] = false;
448  enable_limit[i] = false; // Disable joint limit detection
449  updateVelocity = true; // We have to send this new speed
450  } else {
451  // We have to stop this axis
452  // Test if this axis was stopped before
453  if (force_halt[i] == false) {
454  q_dot[i] = 0.;
455  force_halt[i] = true; // indicate that it will be stopped
456  enable_limit[i] = true; // Joint limit detection must be active
457  updateVelocity = true; // We have to send this new speed
458  }
459  }
460  }
461  } else {
462  // Axis not in joint limit
463 
464  // Update the desired speed
465  q_dot[i] = shm.q_dot[i];
466  shm.status[i] = vpRobotBiclopsController::SPEED;
467  enable_limit[i] = true; // Joint limit detection must be active
468  updateVelocity = true; // We have to send this new speed
469  }
470  } else {
471  // No change of the desired speed. We have to stop the robot in case
472  // of joint limit
473  if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
474  if (enable_limit[i] == true) { // limit detection active
475 
476  // Test if this axis was stopped before
477  if (force_halt[i] == false) {
478  // We have to stop this axis
479  q_dot[i] = 0.;
480  force_halt[i] = true; // indicate that it will be stopped
481  updateVelocity = true; // We have to send this new speed
482  }
483  }
484  } else {
485  // No need to stop the robot
486  enable_limit[i] = true; // Normal situation, activate limit detection
487  }
488  }
489  }
490  // Update the actuals positions
491  controller->writeShm(shm);
492 
493  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
494  pthread_mutex_unlock(&vpShm_mutex);
495 
496  if (updateVelocity) {
497  vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1]));
498 
499  // Apply the velocity
500  controller->setVelocity(q_dot);
501  }
502 
503  // Update the previous speed for next iteration
504  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
505  prev_q_dot[i] = shm.q_dot[i];
506 
507  vpDEBUG_TRACE(12, "iter: %d", iter);
508 
509  // wait 5 ms
510  vpTime::wait(5.0);
511 
512  // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
513  // vpDEBUG_TRACE (12, "Calling thread will end");
514  // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
515  // std::cout << "Calling thread will end" << std::endl;
516  // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
517  //
518  // pthread_mutex_unlock(&vpEndThread_mutex);
519  // break;
520  // }
521 
522  iter++;
523  }
524  controller->stopRequest(false);
525  // Stop the robot
526  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
527  q_dot = 0;
528  controller->setVelocity(q_dot);
529 
530  delete[] new_q_dot;
531  delete[] change_dir;
532  delete[] force_halt;
533  delete[] enable_limit;
534  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
535  pthread_mutex_unlock(&vpEndThread_mutex);
536 
537  vpDEBUG_TRACE(10, "Exit control thread ");
538  // pthread_exit(0);
539 
540  return NULL;
541 }
542 
550 {
551  switch (newState) {
552  case vpRobot::STATE_STOP: {
554  stopMotion();
555  }
556  break;
557  }
560  vpDEBUG_TRACE(12, "Speed to position control.");
561  stopMotion();
562  }
563 
564  break;
565  }
567 
569  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
570  pthread_mutex_lock(&vpEndThread_mutex);
571 
572  vpDEBUG_TRACE(12, "Create speed control thread");
573  int code;
574  code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller);
575  if (code != 0) {
576  vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno)
577  << " strCode=" << strerror(code) << std::endl;
578  }
579 
580  controlThreadCreated = true;
581 
582  vpDEBUG_TRACE(12, "Speed control thread created");
583  }
584  break;
585  }
586  default:
587  break;
588  }
589 
590  return vpRobot::setRobotState(newState);
591 }
592 
599 {
601  q_dot = 0;
602  controller.setVelocity(q_dot);
603  // std::cout << "Request to stop the velocity controller thread...."<<
604  // std::endl;
605  controller.stopRequest(true);
606 }
607 
619 {
621  cMe = vpBiclops::get_cMe();
622 
623  cVe.buildFrom(cMe);
624 }
625 
636 
648 {
649  vpColVector q(2);
651 
652  try {
653  vpBiclops::get_eJe(q, _eJe);
654  } catch (...) {
655  vpERROR_TRACE("catch exception ");
656  throw;
657  }
658 }
659 
668 {
669  vpColVector q(2);
671 
672  try {
673  vpBiclops::get_fJe(q, _fJe);
674  } catch (...) {
675  vpERROR_TRACE("Error caught");
676  throw;
677  }
678 }
679 
687 void vpRobotBiclops::setPositioningVelocity(const double velocity)
688 {
689  if (velocity < 0 || velocity > 100) {
690  vpERROR_TRACE("Bad positionning velocity");
691  throw vpRobotException(vpRobotException::constructionError, "Bad positionning velocity");
692  }
693 
694  positioningVelocity = velocity;
695 }
703 double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; }
704 
721 {
722 
724  vpERROR_TRACE("Robot was not in position-based control\n"
725  "Modification of the robot state");
727  }
728 
729  switch (frame) {
731  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
732  "not implemented");
733  break;
735  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
736  "not implemented");
737  break;
738  case vpRobot::MIXT_FRAME:
739  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
740  "not implemented");
741  break;
743  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
744  "not implemented");
745  break;
747  break;
748  }
749 
750  // test if position reachable
751  // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
752  // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
753  // vpERROR_TRACE ("Positionning error.");
754  // throw vpRobotException (vpRobotException::wrongStateError,
755  // "Positionning error.");
756  // }
757 
758  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
759  pthread_mutex_lock(&vpEndThread_mutex);
760  controller.setPosition(q, positioningVelocity);
761  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
762  pthread_mutex_unlock(&vpEndThread_mutex);
763  return;
764 }
765 
782 void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
783 {
784  try {
785  vpColVector q(2);
786  q[0] = q1;
787  q[1] = q2;
788 
789  setPosition(frame, q);
790  } catch (...) {
791  vpERROR_TRACE("Error caught");
792  throw;
793  }
794 }
795 
809 void vpRobotBiclops::setPosition(const char *filename)
810 {
811  vpColVector q;
812  if (readPositionFile(filename, q) == false) {
813  vpERROR_TRACE("Cannot get biclops position from file");
814  throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file");
815  }
817 }
818 
835 {
836  switch (frame) {
838  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
839  "not implemented");
840  break;
842  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
843  "not implemented");
844  break;
845  case vpRobot::MIXT_FRAME:
846  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
847  "not implemented");
848  break;
850  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
851  "not implemented");
852  break;
854  break;
855  }
856 
858  state = vpRobot::getRobotState();
859 
860  switch (state) {
861  case STATE_STOP:
863  q = controller.getPosition();
864 
865  break;
868  default:
870 
871  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
872  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
873 
874  vpRobotBiclopsController::shmType shm;
875 
876  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
877  pthread_mutex_lock(&vpShm_mutex);
878 
879  shm = controller.readShm();
880 
881  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
882  pthread_mutex_unlock(&vpShm_mutex);
883 
884  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
885  q[i] = shm.actual_q[i];
886  }
887 
888  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
889 
890  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
891  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
892 
893  break;
894  }
895 }
896 
924 {
926  vpERROR_TRACE("Cannot send a velocity to the robot "
927  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
929  "Cannot send a velocity to the robot "
930  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
931  }
932 
933  switch (frame) {
934  case vpRobot::CAMERA_FRAME: {
935  vpERROR_TRACE("Cannot send a velocity to the robot "
936  "in the camera frame: "
937  "functionality not implemented");
938  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
939  "in the camera frame:"
940  "functionality not implemented");
941  }
943  if (q_dot.getRows() != 2) {
944  vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
945  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
946  "in articular frame");
947  }
948  break;
949  }
951  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
952  "in the reference frame:"
953  "functionality not implemented");
954  }
955  case vpRobot::MIXT_FRAME: {
956  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
957  "in the mixt frame:"
958  "functionality not implemented");
959  }
961  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
962  "in the end-effector frame:"
963  "functionality not implemented");
964  }
965  default: {
966  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
967  }
968  }
969 
970  vpDEBUG_TRACE(12, "Velocity limitation.");
971  bool norm = false; // Flag to indicate when velocities need to be nomalized
972 
973  // Saturate articular speed
974  double max = vpBiclops::speedLimit;
975  vpColVector q_dot_sat(vpBiclops::ndof);
976 
977  // init q_dot_saturated
978  q_dot_sat = q_dot;
979 
980  for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
981  {
982  if (fabs(q_dot[i]) > max) {
983  norm = true;
984  max = fabs(q_dot[i]);
985  vpERROR_TRACE("Excess velocity: ROTATION "
986  "(axe nr.%d).",
987  i);
988  }
989  }
990  // Rotations velocities normalisation
991  if (norm == true) {
992  max = vpBiclops::speedLimit / max;
993  q_dot_sat = q_dot * max;
994  }
995 
996  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
997 
998  vpRobotBiclopsController::shmType shm;
999 
1000  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
1001  pthread_mutex_lock(&vpShm_mutex);
1002 
1003  shm = controller.readShm();
1004 
1005  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1006  shm.q_dot[i] = q_dot[i];
1007 
1008  controller.writeShm(shm);
1009 
1010  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1011  pthread_mutex_unlock(&vpShm_mutex);
1012 
1013  return;
1014 }
1015 
1016 /* -------------------------------------------------------------------------
1017  */
1018 /* --- GET -----------------------------------------------------------------
1019  */
1020 /* -------------------------------------------------------------------------
1021  */
1022 
1035 {
1036  switch (frame) {
1037  case vpRobot::CAMERA_FRAME:
1038  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
1039  "not implemented");
1040  break;
1042  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
1043  "not implemented");
1044  break;
1045  case vpRobot::MIXT_FRAME:
1046  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
1047  "not implemented");
1048  break;
1050  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
1051  "not implemented");
1052  break;
1054  break;
1055  }
1056 
1058  state = vpRobot::getRobotState();
1059 
1060  switch (state) {
1061  case STATE_STOP:
1063  q_dot = controller.getVelocity();
1064 
1065  break;
1068  default:
1069  q_dot.resize(vpBiclops::ndof);
1070 
1071  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
1072  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1073 
1074  vpRobotBiclopsController::shmType shm;
1075 
1076  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
1077  pthread_mutex_lock(&vpShm_mutex);
1078 
1079  shm = controller.readShm();
1080 
1081  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1082  pthread_mutex_unlock(&vpShm_mutex);
1083 
1084  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
1085  q_dot[i] = shm.actual_q_dot[i];
1086  }
1087 
1088  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1089 
1090  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
1091  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1092 
1093  break;
1094  }
1095 }
1096 
1109 {
1110  vpColVector q_dot;
1111  getVelocity(frame, q_dot);
1112 
1113  return q_dot;
1114 }
1115 
1135 bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
1136 {
1137  std::ifstream fd(filename.c_str(), std::ios::in);
1138 
1139  if (!fd.is_open()) {
1140  return false;
1141  }
1142 
1143  std::string line;
1144  std::string key("R:");
1145  std::string id("#PTU-EVI - Position");
1146  bool pos_found = false;
1147  int lineNum = 0;
1148 
1150 
1151  while (std::getline(fd, line)) {
1152  lineNum++;
1153  if (lineNum == 1) {
1154  if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
1155  std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
1156  return false;
1157  }
1158  }
1159  if ((line.compare(0, 1, "#") == 0)) { // skip comment
1160  continue;
1161  }
1162  if ((line.compare(0, key.size(), key) == 0)) { // decode position
1163  // check if there are at least njoint values in the line
1164  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1165  if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator
1166  chain = vpIoTools::splitChain(line, std::string("\t"));
1167  if (chain.size() < vpBiclops::ndof + 1)
1168  continue;
1169 
1170  std::istringstream ss(line);
1171  std::string key_;
1172  ss >> key_;
1173  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1174  ss >> q[i];
1175  pos_found = true;
1176  break;
1177  }
1178  }
1179 
1180  // converts rotations from degrees into radians
1181  q.deg2rad();
1182 
1183  fd.close();
1184 
1185  if (!pos_found) {
1186  std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
1187  return false;
1188  }
1189 
1190  return true;
1191 }
1192 
1216 {
1217  vpColVector q_current; // current position
1218 
1220 
1221  switch (frame) {
1224  d = q_current - q_previous;
1225  break;
1226 
1227  case vpRobot::CAMERA_FRAME: {
1228  d.resize(6);
1229  vpHomogeneousMatrix fMc_current;
1230  vpHomogeneousMatrix fMc_previous;
1231  fMc_current = vpBiclops::get_fMc(q_current);
1232  fMc_previous = vpBiclops::get_fMc(q_previous);
1233  vpHomogeneousMatrix c_previousMc_current;
1234  // fMc_c = fMc_p * c_pMc_c
1235  // => c_pMc_c = (fMc_p)^-1 * fMc_c
1236  c_previousMc_current = fMc_previous.inverse() * fMc_current;
1237 
1238  // Compute the instantaneous velocity from this homogeneous matrix.
1239  d = vpExponentialMap::inverse(c_previousMc_current);
1240  break;
1241  }
1242 
1244  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
1245  "functionality not implemented");
1246  break;
1247  case vpRobot::MIXT_FRAME:
1248  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
1249  "functionality not implemented");
1250  break;
1252  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
1253  "functionality not implemented");
1254  break;
1255  }
1256 
1257  q_previous = q_current; // Update for next call of this method
1258 }
1259 
1260 #elif !defined(VISP_BUILD_SHARED_LIBS)
1261 // Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no
1262 // symbols
1263 void dummy_vpRobotBiclops(){};
1264 #endif
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:77
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static vpColVector inverse(const vpHomogeneousMatrix &M)
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:150
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
#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")
Class that defines a generic virtual robot.
Definition: vpRobot.h:58
void get_fJe(vpMatrix &_fJe)
vpControlFrameType
Definition: vpRobot.h:75
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:95
static const double defaultPositioningVelocity
No copy constructor allowed.
virtual ~vpRobotBiclops(void)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
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
vpRobotStateType
Definition: vpRobot.h:64
Initialize the acceleration controller.
Definition: vpRobot.h:68
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRowVector t() const
#define vpCDEBUG(level)
Definition: vpDebug.h:511
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1806
unsigned int getRows() const
Definition: vpArray2D.h:289
static double rad(double deg)
Definition: vpMath.h:108
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:394
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:360
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)
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
double getPositioningVelocity(void)
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:157
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310