Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpRobotBiclops.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Interface for the Biclops robot.
32  *
33  * Authors:
34  * Fabien Spindler
35  *
36  *****************************************************************************/
37 
38 #include <signal.h>
39 #include <errno.h>
40 #include <string.h>
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 
44 #include <visp3/core/vpTime.h>
45 
46 #include <visp3/core/vpConfig.h>
47 
48 #ifdef VISP_HAVE_BICLOPS
49 
50 #include <visp3/robot/vpBiclops.h>
51 #include <visp3/robot/vpRobotBiclops.h>
52 #include <visp3/robot/vpRobotException.h>
53 #include <visp3/core/vpExponentialMap.h>
54 #include <visp3/core/vpIoTools.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 
75 
114  : vpBiclops(), vpRobot(), control_thread(), controller(),
115  positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
116 {
117  vpDEBUG_TRACE (12, "Begin default constructor.");
118 
119  vpRobotBiclops::robotAlreadyCreated = false;
120  setConfigFile("/usr/share/BiclopsDefault.cfg");
121 
122  // Initialize the mutex dedicated to she shm protection
123  pthread_mutex_init (&vpShm_mutex, NULL);
124  pthread_mutex_init (&vpEndThread_mutex, NULL);
125  pthread_mutex_init (&vpMeasure_mutex, NULL);
126 
127  control_thread = 0;
128 }
129 
163 vpRobotBiclops::vpRobotBiclops (const std::string &filename)
164  : vpBiclops(), vpRobot(), control_thread(), controller(),
165  positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
166 {
167  vpDEBUG_TRACE (12, "Begin default constructor.");
168 
169  vpRobotBiclops::robotAlreadyCreated = false;
170  setConfigFile(filename);
171 
172  // Initialize the mutex dedicated to she shm protection
173  pthread_mutex_init (&vpShm_mutex, NULL);
174  pthread_mutex_init (&vpEndThread_mutex, NULL);
175  pthread_mutex_init (&vpMeasure_mutex, NULL);
176 
177  init();
178 
179  return ;
180 }
181 
190 {
191 
192  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
194 
195  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
196  pthread_mutex_unlock(&vpEndThread_mutex);
197 
198  /* wait the end of the control thread */
199  vpDEBUG_TRACE (12, "Wait end of control thread");
200 
201  if (controlThreadCreated == true) {
202  int code = pthread_join(control_thread, NULL);
203  if (code != 0) {
204  vpCERROR << "Cannot terminate the control thread: " << code
205  << " strErr=" << strerror(errno)
206  << " strCode=" << strerror(code)
207  << std::endl;
208  }
209  }
210 
211  pthread_mutex_destroy (&vpShm_mutex);
212  pthread_mutex_destroy (&vpEndThread_mutex);
213  pthread_mutex_destroy (&vpMeasure_mutex);
214 
215  vpRobotBiclops::robotAlreadyCreated = false;
216 
217  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
218  return;
219 }
220 
221 
222 /* ------------------------------------------------------------------------- */
223 /* --- INITIALISATION ------------------------------------------------------ */
224 /* ------------------------------------------------------------------------- */
225 
231 void
232 vpRobotBiclops::setConfigFile(const std::string &filename)
233 {
234  this->configfile = filename;
235 }
236 
246 void
248 {
249  // test if the config file exists
250  FILE *fd = fopen(configfile.c_str(), "r");
251  if (fd == NULL) {
252  vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
254  "Cannot open connection with biclops");
255  }
256  fclose(fd);
257 
258  // Initialize the controller
259  controller.init(configfile);
260 
261  try
262  {
264  }
265  catch(...)
266  {
267  vpERROR_TRACE("Error caught") ;
268  throw ;
269  }
270 
271  vpRobotBiclops::robotAlreadyCreated = true;
272 
273  // Initialize previous articular position to manage getDisplacement()
274  q_previous.resize(vpBiclops::ndof);
275  q_previous = 0;
276 
277  controlThreadCreated = false;
278 
279  return ;
280 }
281 
282 /*
283  Control loop to manage the biclops joint limits in speed control.
284 
285  This control loop is running in a seperate thread in order to detect each 5
286  ms joint limits during the speed control. If a joint limit is detected the
287  axis should be halted.
288 
289  \warning Velocity control mode is not exported from the top-level Biclops API
290  class provided by Traclabs. That means that there is no protection in this
291  mode to prevent an axis from striking its hard limit. In position mode,
292  Traclabs put soft limits in that keep any command from driving to a position
293  too close to the hard limits. In velocity mode this protection does not exist
294  in the current API.
295 
296  \warning With the understanding that hitting the hard limits at full
297  speed/power can damage the unit, damage due to velocity mode commanding is
298  under user responsibility.
299 */
301 {
302  vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
303 
304  int iter = 0;
305 // PMDAxisControl *panAxis = controller->getPanAxis();
306 // PMDAxisControl *tiltAxis = controller->getTiltAxis();
307  vpRobotBiclopsController::shmType shm;
308 
309  vpDEBUG_TRACE(10, "Start control loop");
310  vpColVector mes_q;
311  vpColVector mes_q_dot;
312  vpColVector softLimit(vpBiclops::ndof);
314  bool *new_q_dot = new bool [ vpBiclops::ndof ];
315  bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
316  bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
317  bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
318  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
319  double secure = vpMath::rad(2); // add a security angle before joint limit
320 
321 
322  // Set the soft limits
323  softLimit[0] = vpBiclops::panJointLimit - secure;
324  softLimit[1] = vpBiclops::tiltJointLimit - secure;
325  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
326  vpMath::deg(softLimit[0]),
327  vpMath::deg(softLimit[1]));
328 
329  // Initilisation
330  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
331  pthread_mutex_lock(&vpShm_mutex);
332 
333  shm = controller->readShm();
334 
335  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
336  pthread_mutex_unlock(&vpShm_mutex);
337 
338  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
339  prev_q_dot [i] = shm.q_dot[i];
340  new_q_dot [i] = false;
341  change_dir [i] = false;
342  force_halt [i] = false;
343  enable_limit[i] = true;
344  }
345 
346  // Initialize actual position and velocity
347  mes_q = controller->getActualPosition();
348  mes_q_dot = controller->getActualVelocity();
349 
350  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
351  pthread_mutex_lock(&vpShm_mutex);
352 
353  shm = controller->readShm();
354  // Updates the shm
355  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
356  shm.actual_q[i] = mes_q[i];
357  shm.actual_q_dot[i] = mes_q_dot[i];
358  }
359  // Update the actuals positions
360  controller->writeShm(shm);
361 
362  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
363  pthread_mutex_unlock(&vpShm_mutex);
364 
365  vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
366  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
367 
368  while (! controller->isStopRequested()) {
369 
370  // Get actual position and velocity
371  mes_q = controller->getActualPosition();
372  mes_q_dot = controller->getActualVelocity();
373 
374  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
375  pthread_mutex_lock(&vpShm_mutex);
376 
377 
378  shm = controller->readShm();
379 
380  // Updates the shm
381  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
382  shm.actual_q[i] = mes_q[i];
383  shm.actual_q_dot[i] = mes_q_dot[i];
384  }
385 
386  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
387  vpMath::deg(mes_q[0]),
388  vpMath::deg(mes_q[1]));
389  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
390  vpMath::deg(mes_q_dot[0]),
391  vpMath::deg(mes_q_dot[1]));
392  vpDEBUG_TRACE(12, "desired q_dot : %f %f",
393  vpMath::deg(shm.q_dot[0]),
394  vpMath::deg(shm.q_dot[1]));
395  vpDEBUG_TRACE(13, "previous q_dot : %f %f",
396  vpMath::deg(prev_q_dot[0]),
397  vpMath::deg(prev_q_dot[1]));
398 
399  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
400  // test if joint limits are reached
401  if (mes_q[i] < -softLimit[i]) {
402  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
403  shm.status[i] = vpRobotBiclopsController::STOP;
404  shm.jointLimit[i] = true;
405  }
406  else if (mes_q[i] > softLimit[i]) {
407  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
408  shm.status[i] = vpRobotBiclopsController::STOP;
409  shm.jointLimit[i] = true;
410  }
411  else {
412  shm.status[i] = vpRobotBiclopsController::SPEED;
413  shm.jointLimit[i] = false;
414  }
415 
416  // Test if new a speed is demanded
417  //if (shm.q_dot[i] != prev_q_dot[i])
418  if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
419  new_q_dot[i] = true;
420  else
421  new_q_dot[i] = false;
422 
423  // Test if desired speed change of sign
424  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
425  change_dir[i] = true;
426  else
427  change_dir[i] = false;
428 
429  }
430  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
431  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
432  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
433  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
434  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
435  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
436 
437 
438  bool updateVelocity = false;
439  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
440  // Test if a new desired speed is to apply
441  if (new_q_dot[i]) {
442  // A new desired speed is to apply
443  if (shm.status[i] == vpRobotBiclopsController::STOP) {
444  // Axis in joint limit
445  if (change_dir[i] == false) {
446  // New desired speed without change of direction
447  // We go in the joint limit
448  if (enable_limit[i] == true) { // limit detection active
449  // We have to stop this axis
450  // Test if this axis was stopped before
451  if (force_halt[i] == false) {
452  q_dot[i] = 0.;
453  force_halt[i] = true; // indicate that it will be stopped
454  updateVelocity = true; // We have to send this new speed
455  }
456  }
457  else {
458  // We have to apply the desired speed to go away the joint
459  // Update the desired speed
460  q_dot[i] = shm.q_dot[i];
461  shm.status[i] = vpRobotBiclopsController::SPEED;
462  force_halt[i] = false;
463  updateVelocity = true; // We have to send this new speed
464  }
465  }
466  else {
467  // New desired speed and change of direction.
468  if (enable_limit[i] == true) { // limit detection active
469  // Update the desired speed to go away the joint limit
470  q_dot[i] = shm.q_dot[i];
471  shm.status[i] = vpRobotBiclopsController::SPEED;
472  force_halt[i] = false;
473  enable_limit[i] = false; // Disable joint limit detection
474  updateVelocity = true; // We have to send this new speed
475  }
476  else {
477  // We have to stop this axis
478  // Test if this axis was stopped before
479  if (force_halt[i] == false) {
480  q_dot[i] = 0.;
481  force_halt[i] = true; // indicate that it will be stopped
482  enable_limit[i] = true; // Joint limit detection must be active
483  updateVelocity = true; // We have to send this new speed
484  }
485  }
486  }
487  }
488  else {
489  // Axis not in joint limit
490 
491  // Update the desired speed
492  q_dot[i] = shm.q_dot[i];
493  shm.status[i] = vpRobotBiclopsController::SPEED;
494  enable_limit[i] = true; // Joint limit detection must be active
495  updateVelocity = true; // We have to send this new speed
496  }
497  }
498  else {
499  // No change of the desired speed. We have to stop the robot in case of
500  // joint limit
501  if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
502  if (enable_limit[i] == true) { // limit detection active
503 
504  // Test if this axis was stopped before
505  if (force_halt[i] == false) {
506  // We have to stop this axis
507  q_dot[i] = 0.;
508  force_halt[i] = true; // indicate that it will be stopped
509  updateVelocity = true; // We have to send this new speed
510  }
511  }
512  }
513  else {
514  // No need to stop the robot
515  enable_limit[i] = true; // Normal situation, activate limit detection
516  }
517  }
518  }
519  // Update the actuals positions
520  controller->writeShm(shm);
521 
522  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
523  pthread_mutex_unlock(&vpShm_mutex);
524 
525  if (updateVelocity) {
526  vpDEBUG_TRACE(12, "apply q_dot : %f %f",
527  vpMath::deg(q_dot[0]),
528  vpMath::deg(q_dot[1]));
529 
530  // Apply the velocity
531  controller -> setVelocity( q_dot );
532  }
533 
534 
535  // Update the previous speed for next iteration
536  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
537  prev_q_dot[i] = shm.q_dot[i];
538 
539  vpDEBUG_TRACE(12, "iter: %d", iter);
540 
541  //wait 5 ms
542  vpTime::wait(5.0);
543 
544 // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
545 // vpDEBUG_TRACE (12, "Calling thread will end");
546 // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
547 // std::cout << "Calling thread will end" << std::endl;
548 // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
549 //
550 // pthread_mutex_unlock(&vpEndThread_mutex);
551 // break;
552 // }
553 
554  iter ++;
555  }
556  controller->stopRequest(false);
557  // Stop the robot
558  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
559  q_dot = 0;
560  controller -> setVelocity( q_dot );
561 
562  delete [] new_q_dot;
563  delete [] change_dir;
564  delete [] force_halt;
565  delete [] enable_limit;
566  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
567  pthread_mutex_unlock(&vpEndThread_mutex);
568 
569  vpDEBUG_TRACE (10, "Exit control thread ");
570  // pthread_exit(0);
571 
572  return NULL;
573 }
574 
575 
584 {
585  switch (newState)
586  {
587  case vpRobot::STATE_STOP:
588  {
590  {
591  stopMotion();
592  }
593  break;
594  }
596  {
598  {
599  vpDEBUG_TRACE (12, "Speed to position control.");
600  stopMotion();
601  }
602 
603  break;
604  }
606  {
607 
609  {
610  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
611  pthread_mutex_lock(&vpEndThread_mutex);
612 
613  vpDEBUG_TRACE (12, "Create speed control thread");
614  int code;
615  code = pthread_create(&control_thread, NULL,
617  &controller);
618  if (code != 0) {
619  vpCERROR << "Cannot create speed biclops control thread: " << code
620  << " strErr=" << strerror(errno)
621  << " strCode=" << strerror(code)
622  << std::endl;
623  }
624 
625  controlThreadCreated = true;
626 
627  vpDEBUG_TRACE (12, "Speed control thread created");
628  }
629  break;
630  }
631  default:
632  break ;
633  }
634 
635  return vpRobot::setRobotState (newState);
636 }
637 
638 
639 
645 void
647 {
649  q_dot = 0;
650  controller.setVelocity(q_dot);
651  //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
652  controller.stopRequest(true);
653 }
654 
665 void
667 {
668  vpHomogeneousMatrix cMe ;
669  cMe = vpBiclops::get_cMe() ;
670 
671  cVe.buildFrom(cMe) ;
672 }
673 
683 void
685 {
686  cMe = vpBiclops::get_cMe() ;
687 }
688 
689 
700 void
702 {
703  vpColVector q(2) ;
705 
706  try
707  {
708  vpBiclops::get_eJe(q, _eJe) ;
709  }
710  catch(...)
711  {
712  vpERROR_TRACE("catch exception ") ;
713  throw ;
714  }
715 }
716 
724 void
726 {
727  vpColVector q(2) ;
729 
730  try
731  {
732  vpBiclops::get_fJe(q,_fJe) ;
733  }
734  catch(...)
735  {
736  vpERROR_TRACE("Error caught");
737  throw ;
738  }
739 
740 }
741 
749 void
751 {
752  if (velocity < 0 || velocity > 100) {
753  vpERROR_TRACE("Bad positionning velocity");
755  "Bad positionning velocity");
756  }
757 
758  positioningVelocity = velocity;
759 }
767 double
769 {
770  return positioningVelocity;
771 }
772 
773 
789 void
791  const vpColVector & q )
792 {
793 
795  {
796  vpERROR_TRACE ("Robot was not in position-based control\n"
797  "Modification of the robot state");
799  }
800 
801  switch(frame)
802  {
804  vpERROR_TRACE ("Cannot move the robot in camera frame: "
805  "not implemented");
807  "Cannot move the robot in camera frame: "
808  "not implemented");
809  break;
811  vpERROR_TRACE ("Cannot move the robot in reference frame: "
812  "not implemented");
814  "Cannot move the robot in reference frame: "
815  "not implemented");
816  break;
817  case vpRobot::MIXT_FRAME:
818  vpERROR_TRACE ("Cannot move the robot in mixt frame: "
819  "not implemented");
821  "Cannot move the robot in mixt frame: "
822  "not implemented");
823  break;
825  break ;
826  }
827 
828  // test if position reachable
829 // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
830 // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
831 // vpERROR_TRACE ("Positionning error.");
832 // throw vpRobotException (vpRobotException::wrongStateError,
833 // "Positionning error.");
834 // }
835 
836  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
837  pthread_mutex_lock(&vpEndThread_mutex);
838  controller.setPosition( q, positioningVelocity );
839  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
840  pthread_mutex_unlock(&vpEndThread_mutex);
841  return ;
842 }
843 
861  const double &q1, const double &q2)
862 {
863  try{
864  vpColVector q(2) ;
865  q[0] = q1 ;
866  q[1] = q2 ;
867 
868  setPosition(frame,q) ;
869  }
870  catch(...)
871  {
872  vpERROR_TRACE("Error caught");
873  throw ;
874  }
875 }
876 
890 void
891 vpRobotBiclops::setPosition(const char *filename)
892 {
893  vpColVector q ;
894  if (readPositionFile(filename, q) == false) {
895  vpERROR_TRACE ("Cannot get biclops position from file");
897  "Cannot get biclops position from file");
898  }
900 }
901 
917 void
919  vpColVector & q)
920 {
921  switch(frame)
922  {
923  case vpRobot::CAMERA_FRAME :
924  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
926  "Cannot get position in camera frame: "
927  "not implemented");
928  break;
930  vpERROR_TRACE ("Cannot get position in reference frame: "
931  "not implemented");
933  "Cannot get position in reference frame: "
934  "not implemented");
935  break;
936  case vpRobot::MIXT_FRAME:
937  vpERROR_TRACE ("Cannot get position in mixt frame: "
938  "not implemented");
940  "Cannot get position in mixt frame: "
941  "not implemented");
942  break;
944  break ;
945  }
946 
948  state = vpRobot::getRobotState();
949 
950  switch (state) {
951  case STATE_STOP:
953  q = controller.getPosition();
954 
955  break;
958  default:
960 
961  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
962  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
963 
964  vpRobotBiclopsController::shmType shm;
965 
966  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
967  pthread_mutex_lock(&vpShm_mutex);
968 
969  shm = controller.readShm();
970 
971  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
972  pthread_mutex_unlock(&vpShm_mutex);
973 
974  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
975  q[i] = shm.actual_q[i];
976  }
977 
978  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
979 
980  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
981  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
982 
983  break;
984 
985  }
986 }
987 
988 
1015 void
1017  const vpColVector & q_dot)
1018 {
1020  {
1021  vpERROR_TRACE ("Cannot send a velocity to the robot "
1022  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1024  "Cannot send a velocity to the robot "
1025  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1026  }
1027 
1028  switch(frame)
1029  {
1030  case vpRobot::CAMERA_FRAME :
1031  {
1032  vpERROR_TRACE ("Cannot send a velocity to the robot "
1033  "in the camera frame: "
1034  "functionality not implemented");
1036  "Cannot send a velocity to the robot "
1037  "in the camera frame:"
1038  "functionality not implemented");
1039  }
1041  {
1042  if ( q_dot.getRows() != 2) {
1043  vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
1045  "Bad dimension for speed vector "
1046  "in articular frame");
1047  }
1048  break ;
1049  }
1051  {
1052  vpERROR_TRACE ("Cannot send a velocity to the robot "
1053  "in the reference frame: "
1054  "functionality not implemented");
1056  "Cannot send a velocity to the robot "
1057  "in the reference frame:"
1058  "functionality not implemented");
1059  }
1060  case vpRobot::MIXT_FRAME :
1061  {
1062  vpERROR_TRACE ("Cannot send a velocity to the robot "
1063  "in the mixt frame: "
1064  "functionality not implemented");
1066  "Cannot send a velocity to the robot "
1067  "in the mixt frame:"
1068  "functionality not implemented");
1069  }
1070  default:
1071  {
1072  vpERROR_TRACE ("Error in spec of vpRobot. "
1073  "Case not taken in account.");
1075  "Cannot send a velocity to the robot ");
1076  }
1077  }
1078 
1079  vpDEBUG_TRACE (12, "Velocity limitation.");
1080  bool norm = false; // Flag to indicate when velocities need to be nomalized
1081 
1082  // Saturate articular speed
1083  double max = vpBiclops::speedLimit;
1084  vpColVector q_dot_sat(vpBiclops::ndof);
1085 
1086  // init q_dot_saturated
1087  q_dot_sat = q_dot;
1088 
1089  for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
1090  {
1091  if (fabs (q_dot[i]) > max)
1092  {
1093  norm = true;
1094  max = fabs (q_dot[i]);
1095  vpERROR_TRACE ("Excess velocity: ROTATION "
1096  "(axe nr.%d).", i);
1097  }
1098  }
1099  // Rotations velocities normalisation
1100  if (norm == true) {
1101  max = vpBiclops::speedLimit / max;
1102  q_dot_sat = q_dot * max;
1103  }
1104 
1105  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
1106 
1107  vpRobotBiclopsController::shmType shm;
1108 
1109  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1110  pthread_mutex_lock(&vpShm_mutex);
1111 
1112  shm = controller.readShm();
1113 
1114  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
1115  shm.q_dot[i] = q_dot[i];
1116 
1117  controller.writeShm(shm);
1118 
1119  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1120  pthread_mutex_unlock(&vpShm_mutex);
1121 
1122  return;
1123 }
1124 
1125 
1126 /* ------------------------------------------------------------------------- */
1127 /* --- GET ----------------------------------------------------------------- */
1128 /* ------------------------------------------------------------------------- */
1129 
1130 
1142 void
1144  vpColVector & q_dot)
1145 {
1146  switch(frame)
1147  {
1148  case vpRobot::CAMERA_FRAME :
1149  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
1151  "Cannot get position in camera frame: "
1152  "not implemented");
1153  break;
1155  vpERROR_TRACE ("Cannot get position in reference frame: "
1156  "not implemented");
1158  "Cannot get position in reference frame: "
1159  "not implemented");
1160  break;
1161  case vpRobot::MIXT_FRAME:
1162  vpERROR_TRACE ("Cannot get position in mixt frame: "
1163  "not implemented");
1165  "Cannot get position in mixt frame: "
1166  "not implemented");
1167  break;
1169  break ;
1170  }
1171 
1173  state = vpRobot::getRobotState();
1174 
1175  switch (state) {
1176  case STATE_STOP:
1178  q_dot = controller.getVelocity();
1179 
1180  break;
1183  default:
1184  q_dot.resize(vpBiclops::ndof);
1185 
1186  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
1187  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1188 
1189  vpRobotBiclopsController::shmType shm;
1190 
1191  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1192  pthread_mutex_lock(&vpShm_mutex);
1193 
1194  shm = controller.readShm();
1195 
1196  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1197  pthread_mutex_unlock(&vpShm_mutex);
1198 
1199  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
1200  q_dot[i] = shm.actual_q_dot[i];
1201  }
1202 
1203  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1204 
1205  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
1206  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1207 
1208  break;
1209 
1210  }
1211 }
1212 
1213 
1227 {
1228  vpColVector q_dot;
1229  getVelocity (frame, q_dot);
1230 
1231  return q_dot;
1232 }
1233 
1253 bool
1254 vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
1255 {
1256  std::ifstream fd(filename.c_str(), std::ios::in);
1257 
1258  if(! fd.is_open()) {
1259  return false;
1260  }
1261 
1262  std::string line;
1263  std::string key("R:");
1264  std::string id("#PTU-EVI - Position");
1265  bool pos_found = false;
1266  int lineNum = 0;
1267 
1269 
1270  while(std::getline(fd, line)) {
1271  lineNum ++;
1272  if (lineNum == 1) {
1273  if(! (line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
1274  std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
1275  return false;
1276  }
1277  }
1278  if((line.compare(0, 1, "#") == 0)) { // skip comment
1279  continue;
1280  }
1281  if((line.compare(0, key.size(), key) == 0)) { // decode position
1282  // check if there are at least njoint values in the line
1283  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1284  if (chain.size() < vpBiclops::ndof+1) // try to split with tab separator
1285  chain = vpIoTools::splitChain(line, std::string("\t"));
1286  if(chain.size() < vpBiclops::ndof+1)
1287  continue;
1288 
1289  std::istringstream ss(line);
1290  std::string key_;
1291  ss >> key_;
1292  for (unsigned int i=0; i< vpBiclops::ndof; i++)
1293  ss >> q[i];
1294  pos_found = true;
1295  break;
1296  }
1297  }
1298 
1299  // converts rotations from degrees into radians
1300  q.deg2rad();
1301 
1302  fd.close();
1303 
1304  if (!pos_found) {
1305  std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
1306  return false;
1307  }
1308 
1309  return true;
1310 }
1311 
1324 void
1325 vpRobotBiclops::getCameraDisplacement(vpColVector &d)
1326 {
1328 
1329 }
1341 void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
1342 {
1344 }
1345 
1370 void
1372  vpColVector &d)
1373 {
1374  vpColVector q_current; // current position
1375 
1377 
1378  switch(frame) {
1381  d = q_current - q_previous;
1382  break ;
1383 
1384  case vpRobot::CAMERA_FRAME: {
1385  d.resize(6);
1386  vpHomogeneousMatrix fMc_current;
1387  vpHomogeneousMatrix fMc_previous;
1388  fMc_current = vpBiclops::get_fMc(q_current);
1389  fMc_previous = vpBiclops::get_fMc(q_previous);
1390  vpHomogeneousMatrix c_previousMc_current;
1391  // fMc_c = fMc_p * c_pMc_c
1392  // => c_pMc_c = (fMc_p)^-1 * fMc_c
1393  c_previousMc_current = fMc_previous.inverse() * fMc_current;
1394 
1395  // Compute the instantaneous velocity from this homogeneous matrix.
1396  d = vpExponentialMap::inverse( c_previousMc_current );
1397  break ;
1398  }
1399 
1401  vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
1402  "functionality not implemented");
1404  "Cannot get a velocity in the reference frame:"
1405  "functionality not implemented");
1406  break ;
1407  case vpRobot::MIXT_FRAME:
1408  vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
1409  "functionality not implemented");
1411  "Cannot get a velocity in the mixt frame:"
1412  "functionality not implemented");
1413  break ;
1414  }
1415 
1416 
1417  q_previous = q_current; // Update for next call of this method
1418 
1419 }
1420 
1421 #elif !defined(VISP_BUILD_SHARED_LIBS)
1422 // Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no symbols
1423 void dummy_vpRobotBiclops() {};
1424 #endif
1425 
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:74
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
bool readPositionFile(const std::string &filename, vpColVector &q)
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:157
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:123
void get_eJe(vpMatrix &_eJe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const float tiltJointLimit
Definition: vpBiclops.h:129
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const float speedLimit
Definition: vpBiclops.h:130
#define vpERROR_TRACE
Definition: vpDebug.h:391
Initialize the position controller.
Definition: vpRobot.h:69
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:76
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:97
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:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:140
static const float panJointLimit
Definition: vpBiclops.h:128
Initialize the velocity controller.
Definition: vpRobot.h:68
vpRobotStateType
Definition: vpRobot.h:64
Initialize the acceleration controller.
Definition: vpRobot.h:70
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRowVector t() const
#define vpCDEBUG(level)
Definition: vpDebug.h:502
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:1596
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
static double rad(double deg)
Definition: vpMath.h:104
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:425
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:385
static double deg(double rad)
Definition: vpMath.h:97
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d)
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:141
double getPositioningVelocity(void)
#define vpDEBUG_TRACE
Definition: vpDebug.h:478
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:155
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:225