ViSP  2.10.0
vpRobotBiclops.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotBiclops.cpp 4574 2014-01-09 08:48:51Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Interface for the Biclops robot.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <signal.h>
43 #include <errno.h>
44 #include <string.h>
45 #include <cmath> // std::fabs
46 #include <limits> // numeric_limits
47 
48 #include <visp/vpTime.h>
49 
50 #include <visp/vpConfig.h>
51 
52 #ifdef VISP_HAVE_BICLOPS
53 
54 #include <visp/vpBiclops.h>
55 #include <visp/vpRobotBiclops.h>
56 #include <visp/vpRobotException.h>
57 #include <visp/vpExponentialMap.h>
58 
59 //#define VP_DEBUG // Activate the debug mode
60 //#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
61 #include <visp/vpDebug.h>
62 
63 /* ---------------------------------------------------------------------- */
64 /* --- STATIC ------------------------------------------------------------ */
65 /* ------------------------------------------------------------------------ */
66 
67 bool vpRobotBiclops::robotAlreadyCreated = false;
69 
70 static pthread_mutex_t vpEndThread_mutex;
71 static pthread_mutex_t vpShm_mutex;
72 static pthread_mutex_t vpMeasure_mutex;
73 
74 /* ----------------------------------------------------------------------- */
75 /* --- CONSTRUCTOR ------------------------------------------------------ */
76 /* ---------------------------------------------------------------------- */
77 
78 
117  :
118  vpRobot ()
119 {
120  vpDEBUG_TRACE (12, "Begin default constructor.");
121 
122  vpRobotBiclops::robotAlreadyCreated = false;
123  controlThreadCreated = false;
124  setConfigFile("/usr/share/BiclopsDefault.cfg");
125 
126  // Initialize the mutex dedicated to she shm protection
127  pthread_mutex_init (&vpShm_mutex, NULL);
128  pthread_mutex_init (&vpEndThread_mutex, NULL);
129  pthread_mutex_init (&vpMeasure_mutex, NULL);
130 
131  positioningVelocity = defaultPositioningVelocity ;
132 }
133 
167 vpRobotBiclops::vpRobotBiclops (const char * filename)
168  :
169  vpRobot ()
170 {
171  vpDEBUG_TRACE (12, "Begin default constructor.");
172 
173  vpRobotBiclops::robotAlreadyCreated = false;
174  controlThreadCreated = false;
175  setConfigFile(filename);
176 
177  // Initialize the mutex dedicated to she shm protection
178  pthread_mutex_init (&vpShm_mutex, NULL);
179  pthread_mutex_init (&vpEndThread_mutex, NULL);
180  pthread_mutex_init (&vpMeasure_mutex, NULL);
181 
182  positioningVelocity = defaultPositioningVelocity ;
183 
184  init();
185 
186  return ;
187 }
188 
197 {
198 
199  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
201 
202  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
203  pthread_mutex_unlock(&vpEndThread_mutex);
204 
205  /* wait the end of the control thread */
206  int code;
207  vpDEBUG_TRACE (12, "Wait end of control thread");
208 
209  if (controlThreadCreated == true) {
210  code = pthread_join(control_thread, NULL);
211  if (code != 0) {
212  vpCERROR << "Cannot terminate the control thread: " << code
213  << " strErr=" << strerror(errno)
214  << " strCode=" << strerror(code)
215  << std::endl;
216  }
217  }
218 
219  pthread_mutex_destroy (&vpShm_mutex);
220  pthread_mutex_destroy (&vpEndThread_mutex);
221  pthread_mutex_destroy (&vpMeasure_mutex);
222 
223  vpRobotBiclops::robotAlreadyCreated = false;
224 
225  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
226  return;
227 }
228 
229 
230 /* ------------------------------------------------------------------------- */
231 /* --- INITIALISATION ------------------------------------------------------ */
232 /* ------------------------------------------------------------------------- */
233 
239 void
240 vpRobotBiclops::setConfigFile(const char * filename)
241 {
242  sprintf(configfile, "%s", filename);
243 }
244 
254 void
256 {
257  // test if the config file exists
258  FILE *fd = fopen(configfile, "r");
259  if (fd == NULL) {
260  vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
262  "Cannot open connexion with biclops");
263  }
264  fclose(fd);
265 
266  // Initialize the controller
267  controller.init(configfile);
268 
269  try
270  {
272  }
273  catch(...)
274  {
275  vpERROR_TRACE("Error caught") ;
276  throw ;
277  }
278 
279  vpRobotBiclops::robotAlreadyCreated = true;
280 
281  // Initialize previous articular position to manage getDisplacement()
282  q_previous.resize(vpBiclops::ndof);
283  q_previous = 0;
284 
285  controlThreadCreated = false;
286 
287  return ;
288 }
289 
290 /*
291  Control loop to manage the biclops joint limits in speed control.
292 
293  This control loop is running in a seperate thread in order to detect each 5
294  ms joint limits during the speed control. If a joint limit is detected the
295  axis should be halted.
296 
297  \warning Velocity control mode is not exported from the top-level Biclops API
298  class provided by Traclabs. That means that there is no protection in this
299  mode to prevent an axis from striking its hard limit. In position mode,
300  Traclabs put soft limits in that keep any command from driving to a position
301  too close to the hard limits. In velocity mode this protection does not exist
302  in the current API.
303 
304  \warning With the understanding that hitting the hard limits at full
305  speed/power can damage the unit, damage due to velocity mode commanding is
306  under user responsibility.
307 */
309 {
310  vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
311 
312  int iter = 0;
313 // PMDAxisControl *panAxis = controller->getPanAxis();
314 // PMDAxisControl *tiltAxis = controller->getTiltAxis();
315  vpRobotBiclopsController::shmType shm;
316 
317  vpDEBUG_TRACE(10, "Start control loop");
318  vpColVector mes_q;
319  vpColVector mes_q_dot;
320  vpColVector softLimit(vpBiclops::ndof);
322  bool *new_q_dot = new bool [ vpBiclops::ndof ];
323  bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
324  bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
325  bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
326  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
327  double secure = vpMath::rad(2); // add a security angle before joint limit
328 
329 
330  // Set the soft limits
331  softLimit[0] = vpBiclops::panJointLimit - secure;
332  softLimit[1] = vpBiclops::tiltJointLimit - secure;
333  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
334  vpMath::deg(softLimit[0]),
335  vpMath::deg(softLimit[1]));
336 
337  // Initilisation
338  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
339  pthread_mutex_lock(&vpShm_mutex);
340 
341  shm = controller->readShm();
342 
343  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
344  pthread_mutex_unlock(&vpShm_mutex);
345 
346  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
347  prev_q_dot [i] = shm.q_dot[i];
348  new_q_dot [i] = false;
349  change_dir [i] = false;
350  force_halt [i] = false;
351  enable_limit[i] = true;
352  }
353 
354  // Initialize 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  // Updates the shm
363  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
364  shm.actual_q[i] = mes_q[i];
365  shm.actual_q_dot[i] = mes_q_dot[i];
366  }
367  // Update the actuals positions
368  controller->writeShm(shm);
369 
370  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
371  pthread_mutex_unlock(&vpShm_mutex);
372 
373  vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
374  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
375 
376  while (! controller->isStopRequested()) {
377 
378  // Get actual position and velocity
379  mes_q = controller->getActualPosition();
380  mes_q_dot = controller->getActualVelocity();
381 
382  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
383  pthread_mutex_lock(&vpShm_mutex);
384 
385 
386  shm = controller->readShm();
387 
388  // Updates the shm
389  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
390  shm.actual_q[i] = mes_q[i];
391  shm.actual_q_dot[i] = mes_q_dot[i];
392  }
393 
394  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
395  vpMath::deg(mes_q[0]),
396  vpMath::deg(mes_q[1]));
397  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
398  vpMath::deg(mes_q_dot[0]),
399  vpMath::deg(mes_q_dot[1]));
400  vpDEBUG_TRACE(12, "desired q_dot : %f %f",
401  vpMath::deg(shm.q_dot[0]),
402  vpMath::deg(shm.q_dot[1]));
403  vpDEBUG_TRACE(13, "previous q_dot : %f %f",
404  vpMath::deg(prev_q_dot[0]),
405  vpMath::deg(prev_q_dot[1]));
406 
407  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
408  // test if joint limits are reached
409  if (mes_q[i] < -softLimit[i]) {
410  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
411  shm.status[i] = vpRobotBiclopsController::STOP;
412  shm.jointLimit[i] = true;
413  }
414  else if (mes_q[i] > softLimit[i]) {
415  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
416  shm.status[i] = vpRobotBiclopsController::STOP;
417  shm.jointLimit[i] = true;
418  }
419  else {
420  shm.status[i] = vpRobotBiclopsController::SPEED;
421  shm.jointLimit[i] = false;
422  }
423 
424  // Test if new a speed is demanded
425  //if (shm.q_dot[i] != prev_q_dot[i])
426  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())
427  new_q_dot[i] = true;
428  else
429  new_q_dot[i] = false;
430 
431  // Test if desired speed change of sign
432  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
433  change_dir[i] = true;
434  else
435  change_dir[i] = false;
436 
437  }
438  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
439  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
440  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
441  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
442  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
443  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
444 
445 
446  bool updateVelocity = false;
447  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
448  // Test if a new desired speed is to apply
449  if (new_q_dot[i]) {
450  // A new desired speed is to apply
451  if (shm.status[i] == vpRobotBiclopsController::STOP) {
452  // Axis in joint limit
453  if (change_dir[i] == false) {
454  // New desired speed without change of direction
455  // We go in the joint limit
456  if (enable_limit[i] == true) { // limit detection active
457  // We have to stop this axis
458  // Test if this axis was stopped before
459  if (force_halt[i] == false) {
460  q_dot[i] = 0.;
461  force_halt[i] = true; // indicate that it will be stopped
462  updateVelocity = true; // We have to send this new speed
463  }
464  }
465  else {
466  // We have to apply the desired speed to go away the joint
467  // Update the desired speed
468  q_dot[i] = shm.q_dot[i];
469  shm.status[i] = vpRobotBiclopsController::SPEED;
470  force_halt[i] = false;
471  updateVelocity = true; // We have to send this new speed
472  }
473  }
474  else {
475  // New desired speed and change of direction.
476  if (enable_limit[i] == true) { // limit detection active
477  // Update the desired speed to go away the joint limit
478  q_dot[i] = shm.q_dot[i];
479  shm.status[i] = vpRobotBiclopsController::SPEED;
480  force_halt[i] = false;
481  enable_limit[i] = false; // Disable joint limit detection
482  updateVelocity = true; // We have to send this new speed
483  }
484  else {
485  // We have to stop this axis
486  // Test if this axis was stopped before
487  if (force_halt[i] == false) {
488  q_dot[i] = 0.;
489  force_halt[i] = true; // indicate that it will be stopped
490  enable_limit[i] = true; // Joint limit detection must be active
491  updateVelocity = true; // We have to send this new speed
492  }
493  }
494  }
495  }
496  else {
497  // Axis not in joint limit
498 
499  // Update the desired speed
500  q_dot[i] = shm.q_dot[i];
501  shm.status[i] = vpRobotBiclopsController::SPEED;
502  enable_limit[i] = true; // Joint limit detection must be active
503  updateVelocity = true; // We have to send this new speed
504  }
505  }
506  else {
507  // No change of the desired speed. We have to stop the robot in case of
508  // joint limit
509  if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
510  if (enable_limit[i] == true) { // limit detection active
511 
512  // Test if this axis was stopped before
513  if (force_halt[i] == false) {
514  // We have to stop this axis
515  q_dot[i] = 0.;
516  force_halt[i] = true; // indicate that it will be stopped
517  updateVelocity = true; // We have to send this new speed
518  }
519  }
520  }
521  else {
522  // No need to stop the robot
523  enable_limit[i] = true; // Normal situation, activate limit detection
524  }
525  }
526  }
527  // Update the actuals positions
528  controller->writeShm(shm);
529 
530  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
531  pthread_mutex_unlock(&vpShm_mutex);
532 
533  if (updateVelocity) {
534  vpDEBUG_TRACE(12, "apply q_dot : %f %f",
535  vpMath::deg(q_dot[0]),
536  vpMath::deg(q_dot[1]));
537 
538  // Apply the velocity
539  controller -> setVelocity( q_dot );
540  }
541 
542 
543  // Update the previous speed for next iteration
544  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
545  prev_q_dot[i] = shm.q_dot[i];
546 
547  vpDEBUG_TRACE(12, "iter: %d", iter);
548 
549  //wait 5 ms
550  vpTime::wait(5.0);
551 
552 // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
553 // vpDEBUG_TRACE (12, "Calling thread will end");
554 // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
555 // std::cout << "Calling thread will end" << std::endl;
556 // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
557 //
558 // pthread_mutex_unlock(&vpEndThread_mutex);
559 // break;
560 // }
561 
562  iter ++;
563  }
564  controller->stopRequest(false);
565  // Stop the robot
566  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
567  q_dot = 0;
568  controller -> setVelocity( q_dot );
569 
570  delete [] new_q_dot;
571  delete [] change_dir;
572  delete [] force_halt;
573  delete [] enable_limit;
574  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
575  pthread_mutex_unlock(&vpEndThread_mutex);
576 
577  vpDEBUG_TRACE (10, "Exit control thread ");
578  // pthread_exit(0);
579 
580  return NULL;
581 }
582 
583 
592 {
593  switch (newState)
594  {
595  case vpRobot::STATE_STOP:
596  {
598  {
599  stopMotion();
600  }
601  break;
602  }
604  {
606  {
607  vpDEBUG_TRACE (12, "Speed to position control.");
608  stopMotion();
609  }
610 
611  break;
612  }
614  {
615 
617  {
618  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
619  pthread_mutex_lock(&vpEndThread_mutex);
620 
621  vpDEBUG_TRACE (12, "Create speed control thread");
622  int code;
623  code = pthread_create(&control_thread, NULL,
625  &controller);
626  if (code != 0) {
627  vpCERROR << "Cannot create speed biclops control thread: " << code
628  << " strErr=" << strerror(errno)
629  << " strCode=" << strerror(code)
630  << std::endl;
631  }
632 
633  controlThreadCreated = true;
634 
635  vpDEBUG_TRACE (12, "Speed control thread created");
636  }
637  break;
638  }
639  default:
640  break ;
641  }
642 
643  return vpRobot::setRobotState (newState);
644 }
645 
646 
647 
653 void
655 {
657  q_dot = 0;
658  controller.setVelocity(q_dot);
659  //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
660  controller.stopRequest(true);
661 }
662 
673 void
675 {
676  vpHomogeneousMatrix cMe ;
677  cMe = vpBiclops::get_cMe() ;
678 
679  cVe.buildFrom(cMe) ;
680 }
681 
691 void
693 {
694  cMe = vpBiclops::get_cMe() ;
695 }
696 
697 
708 void
710 {
711  vpColVector q(2) ;
713 
714  try
715  {
716  vpBiclops::get_eJe(q,eJe) ;
717  }
718  catch(...)
719  {
720  vpERROR_TRACE("catch exception ") ;
721  throw ;
722  }
723 }
724 
732 void
734 {
735  vpColVector q(2) ;
737 
738  try
739  {
740  vpBiclops::get_fJe(q,fJe) ;
741  }
742  catch(...)
743  {
744  vpERROR_TRACE("Error caught");
745  throw ;
746  }
747 
748 }
749 
757 void
759 {
760  if (velocity < 0 || velocity > 100) {
761  vpERROR_TRACE("Bad positionning velocity");
763  "Bad positionning velocity");
764  }
765 
766  positioningVelocity = velocity;
767 }
775 double
777 {
778  return positioningVelocity;
779 }
780 
781 
797 void
799  const vpColVector & q )
800 {
801 
803  {
804  vpERROR_TRACE ("Robot was not in position-based control\n"
805  "Modification of the robot state");
807  }
808 
809  switch(frame)
810  {
812  vpERROR_TRACE ("Cannot move the robot in camera frame: "
813  "not implemented");
815  "Cannot move the robot in camera frame: "
816  "not implemented");
817  break;
819  vpERROR_TRACE ("Cannot move the robot in reference frame: "
820  "not implemented");
822  "Cannot move the robot in reference frame: "
823  "not implemented");
824  break;
825  case vpRobot::MIXT_FRAME:
826  vpERROR_TRACE ("Cannot move the robot in mixt frame: "
827  "not implemented");
829  "Cannot move the robot in mixt frame: "
830  "not implemented");
831  break;
833  break ;
834  }
835 
836  // test if position reachable
837 // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
838 // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
839 // vpERROR_TRACE ("Positionning error.");
840 // throw vpRobotException (vpRobotException::wrongStateError,
841 // "Positionning error.");
842 // }
843 
844  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
845  pthread_mutex_lock(&vpEndThread_mutex);
846  controller.setPosition( q, positioningVelocity );
847  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
848  pthread_mutex_unlock(&vpEndThread_mutex);
849  return ;
850 }
851 
869  const double &q1, const double &q2)
870 {
871  try{
872  vpColVector q(2) ;
873  q[0] = q1 ;
874  q[1] = q2 ;
875 
876  setPosition(frame,q) ;
877  }
878  catch(...)
879  {
880  vpERROR_TRACE("Error caught");
881  throw ;
882  }
883 }
884 
898 void
899 vpRobotBiclops::setPosition(const char *filename)
900 {
901  vpColVector q ;
902  if (readPositionFile(filename, q) == false) {
903  vpERROR_TRACE ("Cannot get biclops position from file");
905  "Cannot get biclops position from file");
906  }
908 }
909 
925 void
927  vpColVector & q)
928 {
929  switch(frame)
930  {
931  case vpRobot::CAMERA_FRAME :
932  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
934  "Cannot get position in camera frame: "
935  "not implemented");
936  break;
938  vpERROR_TRACE ("Cannot get position in reference frame: "
939  "not implemented");
941  "Cannot get position in reference frame: "
942  "not implemented");
943  break;
944  case vpRobot::MIXT_FRAME:
945  vpERROR_TRACE ("Cannot get position in mixt frame: "
946  "not implemented");
948  "Cannot get position in mixt frame: "
949  "not implemented");
950  break;
952  break ;
953  }
954 
956  state = vpRobot::getRobotState();
957 
958  switch (state) {
959  case STATE_STOP:
961  q = controller.getPosition();
962 
963  break;
966  default:
968 
969  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
970  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
971 
972  vpRobotBiclopsController::shmType shm;
973 
974  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
975  pthread_mutex_lock(&vpShm_mutex);
976 
977  shm = controller.readShm();
978 
979  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
980  pthread_mutex_unlock(&vpShm_mutex);
981 
982  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
983  q[i] = shm.actual_q[i];
984  }
985 
986  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
987 
988  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
989  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
990 
991  break;
992 
993  }
994 }
995 
996 
1023 void
1025  const vpColVector & q_dot)
1026 {
1028  {
1029  vpERROR_TRACE ("Cannot send a velocity to the robot "
1030  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1032  "Cannot send a velocity to the robot "
1033  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1034  }
1035 
1036  switch(frame)
1037  {
1038  case vpRobot::CAMERA_FRAME :
1039  {
1040  vpERROR_TRACE ("Cannot send a velocity to the robot "
1041  "in the camera frame: "
1042  "functionality not implemented");
1044  "Cannot send a velocity to the robot "
1045  "in the camera frame:"
1046  "functionality not implemented");
1047  break ;
1048  }
1050  {
1051  if ( q_dot.getRows() != 2) {
1052  vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
1054  "Bad dimension for speed vector "
1055  "in articular frame");
1056  }
1057  break ;
1058  }
1060  {
1061  vpERROR_TRACE ("Cannot send a velocity to the robot "
1062  "in the reference frame: "
1063  "functionality not implemented");
1065  "Cannot send a velocity to the robot "
1066  "in the reference frame:"
1067  "functionality not implemented");
1068  break ;
1069  }
1070  case vpRobot::MIXT_FRAME :
1071  {
1072  vpERROR_TRACE ("Cannot send a velocity to the robot "
1073  "in the mixt frame: "
1074  "functionality not implemented");
1076  "Cannot send a velocity to the robot "
1077  "in the mixt frame:"
1078  "functionality not implemented");
1079  break ;
1080  }
1081  default:
1082  {
1083  vpERROR_TRACE ("Error in spec of vpRobot. "
1084  "Case not taken in account.");
1086  "Cannot send a velocity to the robot ");
1087  }
1088  }
1089 
1090  vpDEBUG_TRACE (12, "Velocity limitation.");
1091  bool norm = false; // Flag to indicate when velocities need to be nomalized
1092 
1093  // Saturate articular speed
1094  double max = vpBiclops::speedLimit;
1095  vpColVector q_dot_sat(vpBiclops::ndof);
1096 
1097  // init q_dot_saturated
1098  q_dot_sat = q_dot;
1099 
1100  for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
1101  {
1102  if (fabs (q_dot[i]) > max)
1103  {
1104  norm = true;
1105  max = fabs (q_dot[i]);
1106  vpERROR_TRACE ("Excess velocity: ROTATION "
1107  "(axe nr.%d).", i);
1108  }
1109  }
1110  // Rotations velocities normalisation
1111  if (norm == true) {
1112  max = vpBiclops::speedLimit / max;
1113  q_dot_sat = q_dot * max;
1114  }
1115 
1116  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
1117 
1118  vpRobotBiclopsController::shmType shm;
1119 
1120  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1121  pthread_mutex_lock(&vpShm_mutex);
1122 
1123  shm = controller.readShm();
1124 
1125  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
1126  shm.q_dot[i] = q_dot[i];
1127 
1128  controller.writeShm(shm);
1129 
1130  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1131  pthread_mutex_unlock(&vpShm_mutex);
1132 
1133  return;
1134 }
1135 
1136 
1137 /* ------------------------------------------------------------------------- */
1138 /* --- GET ----------------------------------------------------------------- */
1139 /* ------------------------------------------------------------------------- */
1140 
1141 
1153 void
1155  vpColVector & q_dot)
1156 {
1157  switch(frame)
1158  {
1159  case vpRobot::CAMERA_FRAME :
1160  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
1162  "Cannot get position in camera frame: "
1163  "not implemented");
1164  break;
1166  vpERROR_TRACE ("Cannot get position in reference frame: "
1167  "not implemented");
1169  "Cannot get position in reference frame: "
1170  "not implemented");
1171  break;
1172  case vpRobot::MIXT_FRAME:
1173  vpERROR_TRACE ("Cannot get position in mixt frame: "
1174  "not implemented");
1176  "Cannot get position in mixt frame: "
1177  "not implemented");
1178  break;
1180  break ;
1181  }
1182 
1184  state = vpRobot::getRobotState();
1185 
1186  switch (state) {
1187  case STATE_STOP:
1189  q_dot = controller.getVelocity();
1190 
1191  break;
1194  default:
1195  q_dot.resize(vpBiclops::ndof);
1196 
1197  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
1198  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1199 
1200  vpRobotBiclopsController::shmType shm;
1201 
1202  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1203  pthread_mutex_lock(&vpShm_mutex);
1204 
1205  shm = controller.readShm();
1206 
1207  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1208  pthread_mutex_unlock(&vpShm_mutex);
1209 
1210  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
1211  q_dot[i] = shm.actual_q_dot[i];
1212  }
1213 
1214  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1215 
1216  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
1217  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1218 
1219  break;
1220 
1221  }
1222 }
1223 
1224 
1238 {
1239  vpColVector q_dot;
1240  getVelocity (frame, q_dot);
1241 
1242  return q_dot;
1243 }
1244 
1264 bool
1266 {
1267  FILE * pt_f ;
1268  pt_f = fopen(filename,"r") ;
1269 
1270  if (pt_f == NULL) {
1271  vpERROR_TRACE ("Can not open biclops position file %s", filename);
1272  return false;
1273  }
1274 
1275  char line[FILENAME_MAX];
1276  char head[] = "R:";
1277  bool end = false;
1278 
1279  do {
1280  // skip lines begining with # for comments
1281  if (fgets (line, 100, pt_f) != NULL) {
1282  if ( strncmp (line, "#", 1) != 0) {
1283  // this line is not a comment
1284  if ( fscanf (pt_f, "%s", line) != EOF) {
1285  if ( strcmp (line, head) == 0)
1286  end = true; // robot position was found
1287  }
1288  else {
1289  fclose(pt_f);
1290  return (false); // end of file without position
1291  }
1292  }
1293  }
1294  else {
1295  fclose(pt_f);
1296  return (false);// end of file
1297  }
1298 
1299  }
1300  while ( end != true );
1301 
1302  double q1,q2;
1303  // Read positions
1304  if (fscanf(pt_f, "%lf %lf", &q1, &q2) == EOF) {
1305  fclose(pt_f);
1306  std::cout << "Cannot read joint positions." << std::endl;
1307  return false;
1308  }
1309  q.resize(vpBiclops::ndof) ;
1310 
1311  q[0] = vpMath::rad(q1) ; // Rot tourelle
1312  q[1] = vpMath::rad(q2) ;
1313 
1314  fclose(pt_f) ;
1315  return (true);
1316 }
1317 
1330 void
1331 vpRobotBiclops::getCameraDisplacement(vpColVector &d)
1332 {
1334 
1335 }
1347 void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
1348 {
1350 }
1351 
1376 void
1378  vpColVector &d)
1379 {
1380  vpColVector q_current; // current position
1381 
1383 
1384  switch(frame) {
1387  d = q_current - q_previous;
1388  break ;
1389 
1390  case vpRobot::CAMERA_FRAME: {
1391  d.resize(6);
1392  vpHomogeneousMatrix fMc_current;
1393  vpHomogeneousMatrix fMc_previous;
1394  fMc_current = vpBiclops::get_fMc(q_current);
1395  fMc_previous = vpBiclops::get_fMc(q_previous);
1396  vpHomogeneousMatrix c_previousMc_current;
1397  // fMc_c = fMc_p * c_pMc_c
1398  // => c_pMc_c = (fMc_p)^-1 * fMc_c
1399  c_previousMc_current = fMc_previous.inverse() * fMc_current;
1400 
1401  // Compute the instantaneous velocity from this homogeneous matrix.
1402  d = vpExponentialMap::inverse( c_previousMc_current );
1403  break ;
1404  }
1405 
1407  vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
1408  "functionality not implemented");
1410  "Cannot get a velocity in the reference frame:"
1411  "functionality not implemented");
1412  break ;
1413  case vpRobot::MIXT_FRAME:
1414  vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
1415  "functionality not implemented");
1417  "Cannot get a velocity in the mixt frame:"
1418  "functionality not implemented");
1419  break ;
1420  }
1421 
1422 
1423  q_previous = q_current; // Update for next call of this method
1424 
1425 }
1426 
1427 
1428 
1429 /*
1430  * Local variables:
1431  * c-basic-offset: 2
1432  * End:
1433  */
1434 
1435 #endif
1436 
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static vpColVector inverse(const vpHomogeneousMatrix &M)
#define vpDEBUG_TRACE
Definition: vpDebug.h:482
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void setVelocity(const vpColVector &q_dot)
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)
static const unsigned int ndof
Definition: vpBiclops.h:127
void get_eJe(vpMatrix &_eJe)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
static const float tiltJointLimit
Definition: vpBiclops.h:133
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const float speedLimit
Definition: vpBiclops.h:134
#define vpCERROR
Definition: vpDebug.h:369
Initialize the position controller.
Definition: vpRobot.h:71
class that defines a generic virtual robot
Definition: vpRobot.h:60
void get_fJe(vpMatrix &_fJe)
vpControlFrameType
Definition: vpRobot.h:78
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:101
static const double defaultPositioningVelocity
static int wait(double t0, double t)
Definition: vpTime.cpp:149
virtual ~vpRobotBiclops(void)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:205
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
static const float panJointLimit
Definition: vpBiclops.h:132
Initialize the velocity controller.
Definition: vpRobot.h:70
vpRobotStateType
Definition: vpRobot.h:66
Initialize the acceleration controller.
Definition: vpRobot.h:72
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void init(const char *configfile)
void setPosition(const vpColVector &q, const double percentVelocity)
bool readPositionFile(const char *filename, vpColVector &q)
vpRowVector t() const
Transpose of a vector.
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
#define vpCDEBUG(level)
Definition: vpDebug.h:506
static double rad(double deg)
Definition: vpMath.h:100
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:429
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:389
static double deg(double rad)
Definition: vpMath.h:93
void setConfigFile(const char *filename="/usr/share/BiclopsDefault.cfg")
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d)
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:140
Interface to Biclops, pan, tilt, verge head for computer vision applications.
double getPositioningVelocity(void)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:156
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98