ViSP  2.8.0
vpRobotBiclops.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobotBiclops.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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 
165 vpRobotBiclops::vpRobotBiclops (const char * filename)
166  :
167  vpRobot ()
168 {
169  vpDEBUG_TRACE (12, "Begin default constructor.");
170 
171  vpRobotBiclops::robotAlreadyCreated = false;
172  controlThreadCreated = false;
173  setConfigFile(filename);
174 
175  // Initialize the mutex dedicated to she shm protection
176  pthread_mutex_init (&vpShm_mutex, NULL);
177  pthread_mutex_init (&vpEndThread_mutex, NULL);
178  pthread_mutex_init (&vpMeasure_mutex
179 , NULL);
180 
181  init();
182 
183  return ;
184 }
185 
194 {
195 
196  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
198 
199  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
200  pthread_mutex_unlock(&vpEndThread_mutex);
201 
202  /* wait the end of the control thread */
203  int code;
204  vpDEBUG_TRACE (12, "Wait end of control thread");
205 
206  if (controlThreadCreated == true) {
207  code = pthread_join(control_thread, NULL);
208  if (code != 0) {
209  vpCERROR << "Cannot terminate the control thread: " << code
210  << " strErr=" << strerror(errno)
211  << " strCode=" << strerror(code)
212  << std::endl;
213  }
214  }
215 
216  pthread_mutex_destroy (&vpShm_mutex);
217  pthread_mutex_destroy (&vpEndThread_mutex);
218  pthread_mutex_destroy (&vpMeasure_mutex);
219 
220  vpRobotBiclops::robotAlreadyCreated = false;
221 
222  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
223  return;
224 }
225 
226 
227 /* ------------------------------------------------------------------------- */
228 /* --- INITIALISATION ------------------------------------------------------ */
229 /* ------------------------------------------------------------------------- */
230 
236 void
237 vpRobotBiclops::setConfigFile(const char * filename)
238 {
239  sprintf(configfile, "%s", filename);
240 }
241 
251 void
253 {
254  // test if the config file exists
255  FILE *fd = fopen(configfile, "r");
256  if (fd == NULL) {
257  vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
259  "Cannot open connexion with biclops");
260  }
261  fclose(fd);
262 
263  // Initialize the controller
264  controller.init(configfile);
265 
266  try
267  {
269  }
270  catch(...)
271  {
272  vpERROR_TRACE("Error caught") ;
273  throw ;
274  }
275 
276  vpRobotBiclops::robotAlreadyCreated = true;
277 
278  positioningVelocity = defaultPositioningVelocity ;
279 
280  // Initialize previous articular position to manage getDisplacement()
281  q_previous.resize(vpBiclops::ndof);
282  q_previous = 0;
283 
284  controlThreadCreated = false;
285 
286  return ;
287 }
288 
289 /*
290  Control loop to manage the biclops joint limits in speed control.
291 
292  This control loop is running in a seperate thread in order to detect each 5
293  ms joint limits during the speed control. If a joint limit is detected the
294  axis should be halted.
295 
296  \warning Velocity control mode is not exported from the top-level Biclops API
297  class provided by Traclabs. That means that there is no protection in this
298  mode to prevent an axis from striking its hard limit. In position mode,
299  Traclabs put soft limits in that keep any command from driving to a position
300  too close to the hard limits. In velocity mode this protection does not exist
301  in the current API.
302 
303  \warning With the understanding that hitting the hard limits at full
304  speed/power can damage the unit, damage due to velocity mode commanding is
305  under user responsibility.
306 */
308 {
309  vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
310 
311  int iter = 0;
312 // PMDAxisControl *panAxis = controller->getPanAxis();
313 // PMDAxisControl *tiltAxis = controller->getTiltAxis();
314  vpRobotBiclopsController::shmType shm;
315 
316  vpDEBUG_TRACE(10, "Start control loop");
317  vpColVector mes_q;
318  vpColVector mes_q_dot;
319  vpColVector softLimit(vpBiclops::ndof);
321  bool *new_q_dot = new bool [ vpBiclops::ndof ];
322  bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
323  bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
324  bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
325  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
326  double secure = vpMath::rad(2); // add a security angle before joint limit
327 
328 
329  // Set the soft limits
330  softLimit[0] = vpBiclops::panJointLimit - secure;
331  softLimit[1] = vpBiclops::tiltJointLimit - secure;
332  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
333  vpMath::deg(softLimit[0]),
334  vpMath::deg(softLimit[1]));
335 
336  // Initilisation
337  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
338  pthread_mutex_lock(&vpShm_mutex);
339 
340  shm = controller->readShm();
341 
342  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
343  pthread_mutex_unlock(&vpShm_mutex);
344 
345  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
346  prev_q_dot [i] = shm.q_dot[i];
347  new_q_dot [i] = false;
348  change_dir [i] = false;
349  force_halt [i] = false;
350  enable_limit[i] = true;
351  }
352 
353  // Initialize actual position and velocity
354  mes_q = controller->getActualPosition();
355  mes_q_dot = controller->getActualVelocity();
356 
357  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
358  pthread_mutex_lock(&vpShm_mutex);
359 
360  shm = controller->readShm();
361  // Updates the shm
362  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
363  shm.actual_q[i] = mes_q[i];
364  shm.actual_q_dot[i] = mes_q_dot[i];
365  }
366  // Update the actuals positions
367  controller->writeShm(shm);
368 
369  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
370  pthread_mutex_unlock(&vpShm_mutex);
371 
372  vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
373  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
374 
375  while (! controller->isStopRequested()) {
376 
377  // Get actual position and velocity
378  mes_q = controller->getActualPosition();
379  mes_q_dot = controller->getActualVelocity();
380 
381  vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
382  pthread_mutex_lock(&vpShm_mutex);
383 
384 
385  shm = controller->readShm();
386 
387  // Updates the shm
388  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
389  shm.actual_q[i] = mes_q[i];
390  shm.actual_q_dot[i] = mes_q_dot[i];
391  }
392 
393  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
394  vpMath::deg(mes_q[0]),
395  vpMath::deg(mes_q[1]));
396  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
397  vpMath::deg(mes_q_dot[0]),
398  vpMath::deg(mes_q_dot[1]));
399  vpDEBUG_TRACE(12, "desired q_dot : %f %f",
400  vpMath::deg(shm.q_dot[0]),
401  vpMath::deg(shm.q_dot[1]));
402  vpDEBUG_TRACE(13, "previous q_dot : %f %f",
403  vpMath::deg(prev_q_dot[0]),
404  vpMath::deg(prev_q_dot[1]));
405 
406  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
407  // test if joint limits are reached
408  if (mes_q[i] < -softLimit[i]) {
409  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
410  shm.status[i] = vpRobotBiclopsController::STOP;
411  shm.jointLimit[i] = true;
412  }
413  else if (mes_q[i] > softLimit[i]) {
414  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
415  shm.status[i] = vpRobotBiclopsController::STOP;
416  shm.jointLimit[i] = true;
417  }
418  else {
419  shm.status[i] = vpRobotBiclopsController::SPEED;
420  shm.jointLimit[i] = false;
421  }
422 
423  // Test if new a speed is demanded
424  //if (shm.q_dot[i] != prev_q_dot[i])
425  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())
426  new_q_dot[i] = true;
427  else
428  new_q_dot[i] = false;
429 
430  // Test if desired speed change of sign
431  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
432  change_dir[i] = true;
433  else
434  change_dir[i] = false;
435 
436  }
437  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
438  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
439  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
440  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
441  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
442  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
443 
444 
445  bool updateVelocity = false;
446  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
447  // Test if a new desired speed is to apply
448  if (new_q_dot[i]) {
449  // A new desired speed is to apply
450  if (shm.status[i] == vpRobotBiclopsController::STOP) {
451  // Axis in joint limit
452  if (change_dir[i] == false) {
453  // New desired speed without change of direction
454  // We go in the joint limit
455  if (enable_limit[i] == true) { // limit detection active
456  // We have to stop this axis
457  // Test if this axis was stopped before
458  if (force_halt[i] == false) {
459  q_dot[i] = 0.;
460  force_halt[i] = true; // indicate that it will be stopped
461  updateVelocity = true; // We have to send this new speed
462  }
463  }
464  else {
465  // We have to apply the desired speed to go away the joint
466  // Update the desired speed
467  q_dot[i] = shm.q_dot[i];
468  shm.status[i] = vpRobotBiclopsController::SPEED;
469  force_halt[i] = false;
470  updateVelocity = true; // We have to send this new speed
471  }
472  }
473  else {
474  // New desired speed and change of direction.
475  if (enable_limit[i] == true) { // limit detection active
476  // Update the desired speed to go away the joint limit
477  q_dot[i] = shm.q_dot[i];
478  shm.status[i] = vpRobotBiclopsController::SPEED;
479  force_halt[i] = false;
480  enable_limit[i] = false; // Disable joint limit detection
481  updateVelocity = true; // We have to send this new speed
482  }
483  else {
484  // We have to stop this axis
485  // Test if this axis was stopped before
486  if (force_halt[i] == false) {
487  q_dot[i] = 0.;
488  force_halt[i] = true; // indicate that it will be stopped
489  enable_limit[i] = true; // Joint limit detection must be active
490  updateVelocity = true; // We have to send this new speed
491  }
492  }
493  }
494  }
495  else {
496  // Axis not in joint limit
497 
498  // Update the desired speed
499  q_dot[i] = shm.q_dot[i];
500  shm.status[i] = vpRobotBiclopsController::SPEED;
501  enable_limit[i] = true; // Joint limit detection must be active
502  updateVelocity = true; // We have to send this new speed
503  }
504  }
505  else {
506  // No change of the desired speed. We have to stop the robot in case of
507  // joint limit
508  if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
509  if (enable_limit[i] == true) { // limit detection active
510 
511  // Test if this axis was stopped before
512  if (force_halt[i] == false) {
513  // We have to stop this axis
514  q_dot[i] = 0.;
515  force_halt[i] = true; // indicate that it will be stopped
516  updateVelocity = true; // We have to send this new speed
517  }
518  }
519  }
520  else {
521  // No need to stop the robot
522  enable_limit[i] = true; // Normal situation, activate limit detection
523  }
524  }
525  }
526  // Update the actuals positions
527  controller->writeShm(shm);
528 
529  vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
530  pthread_mutex_unlock(&vpShm_mutex);
531 
532  if (updateVelocity) {
533  vpDEBUG_TRACE(12, "apply q_dot : %f %f",
534  vpMath::deg(q_dot[0]),
535  vpMath::deg(q_dot[1]));
536 
537  // Apply the velocity
538  controller -> setVelocity( q_dot );
539  }
540 
541 
542  // Update the previous speed for next iteration
543  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
544  prev_q_dot[i] = shm.q_dot[i];
545 
546  vpDEBUG_TRACE(12, "iter: %d", iter);
547 
548  //wait 5 ms
549  vpTime::wait(5.0);
550 
551 // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
552 // vpDEBUG_TRACE (12, "Calling thread will end");
553 // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
554 // std::cout << "Calling thread will end" << std::endl;
555 // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
556 //
557 // pthread_mutex_unlock(&vpEndThread_mutex);
558 // break;
559 // }
560 
561  iter ++;
562  }
563  controller->stopRequest(false);
564  // Stop the robot
565  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
566  q_dot = 0;
567  controller -> setVelocity( q_dot );
568 
569  delete [] new_q_dot;
570  delete [] change_dir;
571  delete [] force_halt;
572  delete [] enable_limit;
573  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
574  pthread_mutex_unlock(&vpEndThread_mutex);
575 
576  vpDEBUG_TRACE (10, "Exit control thread ");
577  // pthread_exit(0);
578 
579  return NULL;
580 }
581 
582 
591 {
592  switch (newState)
593  {
594  case vpRobot::STATE_STOP:
595  {
597  {
598  stopMotion();
599  }
600  break;
601  }
603  {
605  {
606  vpDEBUG_TRACE (12, "Speed to position control.");
607  stopMotion();
608  }
609 
610  break;
611  }
613  {
614 
616  {
617  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
618  pthread_mutex_lock(&vpEndThread_mutex);
619 
620  vpDEBUG_TRACE (12, "Create speed control thread");
621  int code;
622  code = pthread_create(&control_thread, NULL,
624  &controller);
625  if (code != 0) {
626  vpCERROR << "Cannot create speed biclops control thread: " << code
627  << " strErr=" << strerror(errno)
628  << " strCode=" << strerror(code)
629  << std::endl;
630  }
631 
632  controlThreadCreated = true;
633 
634  vpDEBUG_TRACE (12, "Speed control thread created");
635  }
636  break;
637  }
638  default:
639  break ;
640  }
641 
642  return vpRobot::setRobotState (newState);
643 }
644 
645 
646 
652 void
654 {
656  q_dot = 0;
657  controller.setVelocity(q_dot);
658  //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
659  controller.stopRequest(true);
660 }
661 
672 void
674 {
675  vpHomogeneousMatrix cMe ;
676  cMe = vpBiclops::get_cMe() ;
677 
678  cVe.buildFrom(cMe) ;
679 }
680 
690 void
692 {
693  cMe = vpBiclops::get_cMe() ;
694 }
695 
696 
707 void
709 {
710  vpColVector q(2) ;
712 
713  try
714  {
715  vpBiclops::get_eJe(q,eJe) ;
716  }
717  catch(...)
718  {
719  vpERROR_TRACE("catch exception ") ;
720  throw ;
721  }
722 }
723 
731 void
733 {
734  vpColVector q(2) ;
736 
737  try
738  {
739  vpBiclops::get_fJe(q,fJe) ;
740  }
741  catch(...)
742  {
743  vpERROR_TRACE("Error caught");
744  throw ;
745  }
746 
747 }
748 
756 void
758 {
759  if (velocity < 0 || velocity > 100) {
760  vpERROR_TRACE("Bad positionning velocity");
762  "Bad positionning velocity");
763  }
764 
765  positioningVelocity = velocity;
766 }
774 double
776 {
777  return positioningVelocity;
778 }
779 
780 
796 void
798  const vpColVector & q )
799 {
800 
802  {
803  vpERROR_TRACE ("Robot was not in position-based control\n"
804  "Modification of the robot state");
806  }
807 
808  switch(frame)
809  {
811  vpERROR_TRACE ("Cannot move the robot in camera frame: "
812  "not implemented");
814  "Cannot move the robot in camera frame: "
815  "not implemented");
816  break;
818  vpERROR_TRACE ("Cannot move the robot in reference frame: "
819  "not implemented");
821  "Cannot move the robot in reference frame: "
822  "not implemented");
823  break;
824  case vpRobot::MIXT_FRAME:
825  vpERROR_TRACE ("Cannot move the robot in mixt frame: "
826  "not implemented");
828  "Cannot move the robot in mixt frame: "
829  "not implemented");
830  break;
832  break ;
833  }
834 
835  // test if position reachable
836 // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
837 // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
838 // vpERROR_TRACE ("Positionning error.");
839 // throw vpRobotException (vpRobotException::wrongStateError,
840 // "Positionning error.");
841 // }
842 
843  vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
844  pthread_mutex_lock(&vpEndThread_mutex);
845  controller.setPosition( q, positioningVelocity );
846  vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
847  pthread_mutex_unlock(&vpEndThread_mutex);
848  return ;
849 }
850 
868  const double &q1, const double &q2)
869 {
870  try{
871  vpColVector q(2) ;
872  q[0] = q1 ;
873  q[1] = q2 ;
874 
875  setPosition(frame,q) ;
876  }
877  catch(...)
878  {
879  vpERROR_TRACE("Error caught");
880  throw ;
881  }
882 }
883 
897 void
898 vpRobotBiclops::setPosition(const char *filename)
899 {
900  vpColVector q ;
901  if (readPositionFile(filename, q) == false) {
902  vpERROR_TRACE ("Cannot get biclops position from file");
904  "Cannot get biclops position from file");
905  }
907 }
908 
924 void
926  vpColVector & q)
927 {
928  switch(frame)
929  {
930  case vpRobot::CAMERA_FRAME :
931  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
933  "Cannot get position in camera frame: "
934  "not implemented");
935  break;
937  vpERROR_TRACE ("Cannot get position in reference frame: "
938  "not implemented");
940  "Cannot get position in reference frame: "
941  "not implemented");
942  break;
943  case vpRobot::MIXT_FRAME:
944  vpERROR_TRACE ("Cannot get position in mixt frame: "
945  "not implemented");
947  "Cannot get position in mixt frame: "
948  "not implemented");
949  break;
951  break ;
952  }
953 
955  state = vpRobot::getRobotState();
956 
957  switch (state) {
958  case STATE_STOP:
960  q = controller.getPosition();
961 
962  break;
965  default:
967 
968  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
969  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
970 
971  vpRobotBiclopsController::shmType shm;
972 
973  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
974  pthread_mutex_lock(&vpShm_mutex);
975 
976  shm = controller.readShm();
977 
978  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
979  pthread_mutex_unlock(&vpShm_mutex);
980 
981  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
982  q[i] = shm.actual_q[i];
983  }
984 
985  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
986 
987  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
988  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
989 
990  break;
991 
992  }
993 }
994 
995 
1022 void
1024  const vpColVector & q_dot)
1025 {
1027  {
1028  vpERROR_TRACE ("Cannot send a velocity to the robot "
1029  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1031  "Cannot send a velocity to the robot "
1032  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1033  }
1034 
1035  switch(frame)
1036  {
1037  case vpRobot::CAMERA_FRAME :
1038  {
1039  vpERROR_TRACE ("Cannot send a velocity to the robot "
1040  "in the camera frame: "
1041  "functionality not implemented");
1043  "Cannot send a velocity to the robot "
1044  "in the camera frame:"
1045  "functionality not implemented");
1046  break ;
1047  }
1049  {
1050  if ( q_dot.getRows() != 2) {
1051  vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
1053  "Bad dimension for speed vector "
1054  "in articular frame");
1055  }
1056  break ;
1057  }
1059  {
1060  vpERROR_TRACE ("Cannot send a velocity to the robot "
1061  "in the reference frame: "
1062  "functionality not implemented");
1064  "Cannot send a velocity to the robot "
1065  "in the reference frame:"
1066  "functionality not implemented");
1067  break ;
1068  }
1069  case vpRobot::MIXT_FRAME :
1070  {
1071  vpERROR_TRACE ("Cannot send a velocity to the robot "
1072  "in the mixt frame: "
1073  "functionality not implemented");
1075  "Cannot send a velocity to the robot "
1076  "in the mixt frame:"
1077  "functionality not implemented");
1078  break ;
1079  }
1080  default:
1081  {
1082  vpERROR_TRACE ("Error in spec of vpRobot. "
1083  "Case not taken in account.");
1085  "Cannot send a velocity to the robot ");
1086  }
1087  }
1088 
1089  vpDEBUG_TRACE (12, "Velocity limitation.");
1090  bool norm = false; // Flag to indicate when velocities need to be nomalized
1091 
1092  // Saturate articular speed
1093  double max = vpBiclops::speedLimit;
1094  vpColVector q_dot_sat(vpBiclops::ndof);
1095 
1096  // init q_dot_saturated
1097  q_dot_sat = q_dot;
1098 
1099  for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
1100  {
1101  if (fabs (q_dot[i]) > max)
1102  {
1103  norm = true;
1104  max = fabs (q_dot[i]);
1105  vpERROR_TRACE ("Excess velocity: ROTATION "
1106  "(axe nr.%d).", i);
1107  }
1108  }
1109  // Rotations velocities normalisation
1110  if (norm == true) {
1111  max = vpBiclops::speedLimit / max;
1112  q_dot_sat = q_dot * max;
1113  }
1114 
1115  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
1116 
1117  vpRobotBiclopsController::shmType shm;
1118 
1119  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1120  pthread_mutex_lock(&vpShm_mutex);
1121 
1122  shm = controller.readShm();
1123 
1124  for (unsigned int i=0; i < vpBiclops::ndof; i ++)
1125  shm.q_dot[i] = q_dot[i];
1126 
1127  controller.writeShm(shm);
1128 
1129  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1130  pthread_mutex_unlock(&vpShm_mutex);
1131 
1132  return;
1133 }
1134 
1135 
1136 /* ------------------------------------------------------------------------- */
1137 /* --- GET ----------------------------------------------------------------- */
1138 /* ------------------------------------------------------------------------- */
1139 
1140 
1152 void
1154  vpColVector & q_dot)
1155 {
1156  switch(frame)
1157  {
1158  case vpRobot::CAMERA_FRAME :
1159  vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
1161  "Cannot get position in camera frame: "
1162  "not implemented");
1163  break;
1165  vpERROR_TRACE ("Cannot get position in reference frame: "
1166  "not implemented");
1168  "Cannot get position in reference frame: "
1169  "not implemented");
1170  break;
1171  case vpRobot::MIXT_FRAME:
1172  vpERROR_TRACE ("Cannot get position in mixt frame: "
1173  "not implemented");
1175  "Cannot get position in mixt frame: "
1176  "not implemented");
1177  break;
1179  break ;
1180  }
1181 
1183  state = vpRobot::getRobotState();
1184 
1185  switch (state) {
1186  case STATE_STOP:
1188  q_dot = controller.getVelocity();
1189 
1190  break;
1193  default:
1194  q_dot.resize(vpBiclops::ndof);
1195 
1196  vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
1197  pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1198 
1199  vpRobotBiclopsController::shmType shm;
1200 
1201  vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
1202  pthread_mutex_lock(&vpShm_mutex);
1203 
1204  shm = controller.readShm();
1205 
1206  vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
1207  pthread_mutex_unlock(&vpShm_mutex);
1208 
1209  for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
1210  q_dot[i] = shm.actual_q_dot[i];
1211  }
1212 
1213  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1214 
1215  vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
1216  pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1217 
1218  break;
1219 
1220  }
1221 }
1222 
1223 
1237 {
1238  vpColVector q_dot;
1239  getVelocity (frame, q_dot);
1240 
1241  return q_dot;
1242 }
1243 
1263 bool
1265 {
1266  FILE * pt_f ;
1267  pt_f = fopen(filename,"r") ;
1268 
1269  if (pt_f == NULL) {
1270  vpERROR_TRACE ("Can not open biclops position file %s", filename);
1271  return false;
1272  }
1273 
1274  char line[FILENAME_MAX];
1275  char head[] = "R:";
1276  bool end = false;
1277 
1278  do {
1279  // skip lines begining with # for comments
1280  if (fgets (line, 100, pt_f) != NULL) {
1281  if ( strncmp (line, "#", 1) != 0) {
1282  // this line is not a comment
1283  if ( fscanf (pt_f, "%s", line) != EOF) {
1284  if ( strcmp (line, head) == 0)
1285  end = true; // robot position was found
1286  }
1287  else
1288  return (false); // end of file without position
1289  }
1290  }
1291  else {
1292  return (false);// end of file
1293  }
1294 
1295  }
1296  while ( end != true );
1297 
1298  double q1,q2;
1299  // Read positions
1300  if (fscanf(pt_f, "%lf %lf", &q1, &q2) == EOF) {
1301  std::cout << "Cannot read joint positions." << std::endl;
1302  return false;
1303  }
1304  q.resize(vpBiclops::ndof) ;
1305 
1306  q[0] = vpMath::rad(q1) ; // Rot tourelle
1307  q[1] = vpMath::rad(q2) ;
1308 
1309  fclose(pt_f) ;
1310  return (true);
1311 }
1312 
1325 void
1326 vpRobotBiclops::getCameraDisplacement(vpColVector &d)
1327 {
1329 
1330 }
1342 void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
1343 {
1345 }
1346 
1371 void
1373  vpColVector &d)
1374 {
1375  vpColVector q_current; // current position
1376 
1378 
1379  switch(frame) {
1382  d = q_current - q_previous;
1383  break ;
1384 
1385  case vpRobot::CAMERA_FRAME: {
1386  d.resize(6);
1387  vpHomogeneousMatrix fMc_current;
1388  vpHomogeneousMatrix fMc_previous;
1389  fMc_current = vpBiclops::get_fMc(q_current);
1390  fMc_previous = vpBiclops::get_fMc(q_previous);
1391  vpHomogeneousMatrix c_previousMc_current;
1392  // fMc_c = fMc_p * c_pMc_c
1393  // => c_pMc_c = (fMc_p)^-1 * fMc_c
1394  c_previousMc_current = fMc_previous.inverse() * fMc_current;
1395 
1396  // Compute the instantaneous velocity from this homogeneous matrix.
1397  d = vpExponentialMap::inverse( c_previousMc_current );
1398  break ;
1399  }
1400 
1402  vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
1403  "functionality not implemented");
1405  "Cannot get a velocity in the reference frame:"
1406  "functionality not implemented");
1407  break ;
1408  case vpRobot::MIXT_FRAME:
1409  vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
1410  "functionality not implemented");
1412  "Cannot get a velocity in the mixt frame:"
1413  "functionality not implemented");
1414  break ;
1415  }
1416 
1417 
1418  q_previous = q_current; // Update for next call of this method
1419 
1420 }
1421 
1422 
1423 
1424 /*
1425  * Local variables:
1426  * c-basic-offset: 2
1427  * End:
1428  */
1429 
1430 #endif
1431 
static vpColVector inverse(const vpHomogeneousMatrix &M)
#define vpDEBUG_TRACE
Definition: vpDebug.h:454
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void setVelocity(const vpColVector &q_dot)
void get_eJe(const vpColVector &q, vpMatrix &eJe)
Definition: vpBiclops.cpp:389
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:379
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:354
virtual vpRobotStateType getRobotState(void)
Definition: vpRobot.h:139
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
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:154
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
void get_cVe(vpVelocityTwistMatrix &_cVe)
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc)
Definition: vpBiclops.cpp:101
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)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
Definition: vpBiclops.cpp:431
bool readPositionFile(const char *filename, vpColVector &q)
vpRowVector t() const
transpose of Vector
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
#define vpCDEBUG(niv)
Definition: vpDebug.h:478
static double rad(double deg)
Definition: vpMath.h:100
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
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:157
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:154
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:94