Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
vpRobotBiclops.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
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 https://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 
34 #include <visp3/core/vpConfig.h>
35 
36 #if defined(VISP_HAVE_BICLOPS) && defined(VISP_HAVE_THREADS)
37 
38 #include <cmath> // std::fabs
39 #include <errno.h>
40 #include <limits> // numeric_limits
41 #include <signal.h>
42 #include <string.h>
43 #include <mutex>
44 
45 #include <visp3/core/vpExponentialMap.h>
46 #include <visp3/core/vpIoTools.h>
47 #include <visp3/core/vpTime.h>
48 #include <visp3/robot/vpBiclops.h>
49 #include <visp3/robot/vpRobotBiclops.h>
50 #include <visp3/robot/vpRobotException.h>
51 
52 #include "private/vpRobotBiclopsController_impl.h"
53 
54 //#define VP_DEBUG // Activate the debug mode
55 //#define VP_DEBUG_MODE 12 // Activate debug level up to 12
56 #include <visp3/core/vpDebug.h>
57 
58 /* ---------------------------------------------------------------------- */
59 /* --- STATIC ------------------------------------------------------------ */
60 /* ------------------------------------------------------------------------ */
61 
63 
64 static std::mutex m_mutex_end_thread;
65 static std::mutex m_mutex_shm;
66 static std::mutex m_mutex_measure;
67 
68 /* ----------------------------------------------------------------------- */
69 /* --- CONSTRUCTOR ------------------------------------------------------ */
70 /* ---------------------------------------------------------------------- */
71 
73  : vpBiclops(), vpRobot(), m_control_thread(), m_positioningVelocity(defaultPositioningVelocity),
74  m_q_previous()
75 {
76  vpDEBUG_TRACE(12, "Begin default constructor.");
77 
78  m_controller = new vpRobotBiclopsController;
79  setConfigFile("/usr/share/BiclopsDefault.cfg");
80 }
81 
82 vpRobotBiclops::vpRobotBiclops(const std::string &filename)
83  : vpBiclops(), vpRobot(), m_control_thread(), m_positioningVelocity(defaultPositioningVelocity),
84  m_q_previous()
85 {
86  vpDEBUG_TRACE(12, "Begin default constructor.");
87 
88  m_controller = new vpRobotBiclopsController;
89  setConfigFile(filename);
90 
91  init();
92 }
93 
95 {
96  vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
98 
99  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
100  m_mutex_end_thread.unlock();
101 
102  /* wait the end of the control thread */
103  vpDEBUG_TRACE(12, "Wait end of control thread");
104 
105  if (m_control_thread.joinable()) {
106  m_control_thread.join();
107  }
108 
109  delete m_controller;
110  vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
111 }
112 
113 void vpRobotBiclops::setConfigFile(const std::string &filename) { m_configfile = filename; }
114 
116 {
117  // test if the config file exists
118  FILE *fd = fopen(m_configfile.c_str(), "r");
119  if (fd == nullptr) {
120  vpCERROR << "Cannot open Biclops config file: " << m_configfile << std::endl;
121  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
122  }
123  fclose(fd);
124 
125  // Initialize the controller
126  m_controller->init(m_configfile);
127 
128  try {
130  }
131  catch (...) {
132  vpERROR_TRACE("Error caught");
133  throw;
134  }
135 
136  // Initialize previous articular position to manage getDisplacement()
137  m_q_previous.resize(vpBiclops::ndof);
138  m_q_previous = 0;
139 
140  return;
141 }
142 
144 {
145  vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
146 
147  // PMDAxisControl *m_panAxis = controller->getPanAxis();
148  // PMDAxisControl *m_tiltAxis = controller->getTiltAxis();
149  vpRobotBiclopsController::shmType shm;
150 
151  vpDEBUG_TRACE(10, "Start control loop");
152  vpColVector mes_q;
153  vpColVector mes_q_dot;
154  vpColVector softLimit(vpBiclops::ndof);
156  bool *new_q_dot = new bool[vpBiclops::ndof];
157  bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
158  bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
159  bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
160  vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
161  double secure = vpMath::rad(2); // add a security angle before joint limit
162 
163  // Set the soft limits
164  softLimit[0] = vpBiclops::panJointLimit - secure;
165  softLimit[1] = vpBiclops::tiltJointLimit - secure;
166  vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1]));
167 
168  // Initialization
169  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
170  m_mutex_shm.lock();
171 
172  shm = controller->readShm();
173 
174  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
175  m_mutex_shm.unlock();
176 
177  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
178  prev_q_dot[i] = shm.q_dot[i];
179  new_q_dot[i] = false;
180  change_dir[i] = false;
181  force_halt[i] = false;
182  enable_limit[i] = true;
183  }
184 
185  // Initialize actual position and velocity
186  mes_q = controller->getActualPosition();
187  mes_q_dot = controller->getActualVelocity();
188 
189  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
190  m_mutex_shm.lock();
191 
192  shm = controller->readShm();
193  // Updates the shm
194  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
195  shm.actual_q[i] = mes_q[i];
196  shm.actual_q_dot[i] = mes_q_dot[i];
197  }
198  // Update current positions
199  controller->writeShm(shm);
200 
201  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
202  m_mutex_shm.unlock();
203 
204  vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
205  m_mutex_measure.unlock(); // A position is available
206 
207  while (!controller->isStopRequested()) {
208 
209  // Get actual position and velocity
210  mes_q = controller->getActualPosition();
211  mes_q_dot = controller->getActualVelocity();
212 
213  vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
214  m_mutex_shm.lock();
215 
216  shm = controller->readShm();
217 
218  // Updates the shm
219  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
220  shm.actual_q[i] = mes_q[i];
221  shm.actual_q_dot[i] = mes_q_dot[i];
222  }
223 
224  vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
225  vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
226  vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
227  vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
228 
229  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
230  // test if joint limits are reached
231  if (mes_q[i] < -softLimit[i]) {
232  vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
233  shm.status[i] = vpRobotBiclopsController::STOP;
234  shm.jointLimit[i] = true;
235  }
236  else if (mes_q[i] > softLimit[i]) {
237  vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
238  shm.status[i] = vpRobotBiclopsController::STOP;
239  shm.jointLimit[i] = true;
240  }
241  else {
242  shm.status[i] = vpRobotBiclopsController::SPEED;
243  shm.jointLimit[i] = false;
244  }
245 
246  // Test if new a speed is demanded
247  // if (shm.q_dot[i] != prev_q_dot[i])
248  if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
249  std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
250  new_q_dot[i] = true;
251  else
252  new_q_dot[i] = false;
253 
254  // Test if desired speed change of sign
255  if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
256  change_dir[i] = true;
257  else
258  change_dir[i] = false;
259  }
260  vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
261  vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
262  vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
263  vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
264  vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
265  vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
266 
267  bool updateVelocity = false;
268  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
269  // Test if a new desired speed is to apply
270  if (new_q_dot[i]) {
271  // A new desired speed is to apply
272  if (shm.status[i] == vpRobotBiclopsController::STOP) {
273  // Axis in joint limit
274  if (change_dir[i] == false) {
275  // New desired speed without change of direction
276  // We go in the joint limit
277  if (enable_limit[i] == true) { // limit detection active
278  // We have to stop this axis
279  // Test if this axis was stopped before
280  if (force_halt[i] == false) {
281  q_dot[i] = 0.;
282  force_halt[i] = true; // indicate that it will be stopped
283  updateVelocity = true; // We have to send this new speed
284  }
285  }
286  else {
287  // We have to apply the desired speed to go away the joint
288  // Update the desired speed
289  q_dot[i] = shm.q_dot[i];
290  shm.status[i] = vpRobotBiclopsController::SPEED;
291  force_halt[i] = false;
292  updateVelocity = true; // We have to send this new speed
293  }
294  }
295  else {
296  // New desired speed and change of direction.
297  if (enable_limit[i] == true) { // limit detection active
298  // Update the desired speed to go away the joint limit
299  q_dot[i] = shm.q_dot[i];
300  shm.status[i] = vpRobotBiclopsController::SPEED;
301  force_halt[i] = false;
302  enable_limit[i] = false; // Disable joint limit detection
303  updateVelocity = true; // We have to send this new speed
304  }
305  else {
306  // We have to stop this axis
307  // Test if this axis was stopped before
308  if (force_halt[i] == false) {
309  q_dot[i] = 0.;
310  force_halt[i] = true; // indicate that it will be stopped
311  enable_limit[i] = true; // Joint limit detection must be active
312  updateVelocity = true; // We have to send this new speed
313  }
314  }
315  }
316  }
317  else {
318  // Axis not in joint limit
319 
320  // Update the desired speed
321  q_dot[i] = shm.q_dot[i];
322  shm.status[i] = vpRobotBiclopsController::SPEED;
323  enable_limit[i] = true; // Joint limit detection must be active
324  updateVelocity = true; // We have to send this new speed
325  }
326  }
327  else {
328  // No change of the desired speed. We have to stop the robot in case
329  // of joint limit
330  if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
331  if (enable_limit[i] == true) { // limit detection active
332 
333  // Test if this axis was stopped before
334  if (force_halt[i] == false) {
335  // We have to stop this axis
336  q_dot[i] = 0.;
337  force_halt[i] = true; // indicate that it will be stopped
338  updateVelocity = true; // We have to send this new speed
339  }
340  }
341  }
342  else {
343  // No need to stop the robot
344  enable_limit[i] = true; // Normal situation, activate limit detection
345  }
346  }
347  }
348  // Update the actual positions
349  controller->writeShm(shm);
350 
351  vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
352  m_mutex_shm.unlock();
353 
354  if (updateVelocity) {
355  vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1]));
356 
357  // Apply the velocity
358  controller->setVelocity(q_dot);
359  }
360 
361  // Update the previous speed for next iteration
362  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
363  prev_q_dot[i] = shm.q_dot[i];
364 
365  // wait 5 ms
366  vpTime::wait(5.0);
367  }
368  controller->stopRequest(false);
369  // Stop the robot
370  vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
371  q_dot = 0;
372  controller->setVelocity(q_dot);
373 
374  delete[] new_q_dot;
375  delete[] change_dir;
376  delete[] force_halt;
377  delete[] enable_limit;
378  vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
379  m_mutex_end_thread.unlock();
380 }
381 
383 {
384  switch (newState) {
385  case vpRobot::STATE_STOP: {
387  stopMotion();
388  }
389  break;
390  }
393  vpDEBUG_TRACE(12, "Speed to position control.");
394  stopMotion();
395  }
396 
397  break;
398  }
400 
402  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
403  m_mutex_end_thread.lock();
404 
405  vpDEBUG_TRACE(12, "Create speed control thread");
406  m_control_thread = std::thread(&vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, m_controller);
407  vpTime::wait(100.0);
408 
409  vpDEBUG_TRACE(12, "Speed control thread created");
410  }
411  break;
412  }
413  default:
414  break;
415  }
416 
417  return vpRobot::setRobotState(newState);
418 }
419 
421 {
423  q_dot = 0;
424  m_controller->setVelocity(q_dot);
425  // std::cout << "Request to stop the velocity controller thread...." << std::endl;
426  m_controller->stopRequest(true);
427 }
428 
430 {
432  cMe = vpBiclops::get_cMe();
433 
434  cVe.build(cMe);
435 }
436 
438 
440 {
441  vpColVector q(2);
443 
445 }
446 
448 {
449  vpColVector q(2);
451 
453 }
454 
456 {
457  if (velocity < 0 || velocity > 100) {
458  vpERROR_TRACE("Bad positioning velocity");
459  throw vpRobotException(vpRobotException::constructionError, "Bad positioning velocity");
460  }
461 
462  m_positioningVelocity = velocity;
463 }
464 
465 double vpRobotBiclops::getPositioningVelocity(void) { return m_positioningVelocity; }
466 
468 {
469 
471  vpERROR_TRACE("Robot was not in position-based control\n"
472  "Modification of the robot state");
474  }
475 
476  switch (frame) {
478  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
479  "not implemented");
480  break;
482  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
483  "not implemented");
484  break;
485  case vpRobot::MIXT_FRAME:
486  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
487  "not implemented");
488  break;
490  throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
491  "not implemented");
492  break;
494  break;
495  }
496 
497  vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
498  m_mutex_end_thread.lock();
499  m_controller->setPosition(q, m_positioningVelocity);
500  vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
501  m_mutex_end_thread.unlock();
502  return;
503 }
504 
505 void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
506 {
507  try {
508  vpColVector q(2);
509  q[0] = q1;
510  q[1] = q2;
511 
512  setPosition(frame, q);
513  }
514  catch (...) {
515  vpERROR_TRACE("Error caught");
516  throw;
517  }
518 }
519 
520 void vpRobotBiclops::setPosition(const std::string &filename)
521 {
522  vpColVector q;
523  if (readPositionFile(filename.c_str(), q) == false) {
524  vpERROR_TRACE("Cannot get Biclops position from file");
525  throw vpRobotException(vpRobotException::readingParametersError, "Cannot get Biclops position from file");
526  }
528 }
529 
531 {
532  switch (frame) {
534  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
535  "not implemented");
536  break;
538  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
539  "not implemented");
540  break;
541  case vpRobot::MIXT_FRAME:
542  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
543  "not implemented");
544  break;
546  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
547  "not implemented");
548  break;
550  break;
551  }
552 
554  state = vpRobot::getRobotState();
555 
556  switch (state) {
557  case STATE_STOP:
559  q = m_controller->getPosition();
560 
561  break;
564  default:
566 
567  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
568  m_mutex_measure.lock(); // Wait until a position is available
569 
570  vpRobotBiclopsController::shmType shm;
571 
572  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
573  m_mutex_shm.lock();
574 
575  shm = m_controller->readShm();
576 
577  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
578  m_mutex_shm.unlock();
579 
580  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
581  q[i] = shm.actual_q[i];
582  }
583 
584  vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
585 
586  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
587  m_mutex_measure.unlock(); // A position is available
588 
589  break;
590  }
591 }
592 
594 {
596  vpERROR_TRACE("Cannot send a velocity to the robot "
597  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
599  "Cannot send a velocity to the robot "
600  "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
601  }
602 
603  switch (frame) {
604  case vpRobot::CAMERA_FRAME: {
605  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
606  "in the camera frame:"
607  "functionality not implemented");
608  }
609  case vpRobot::JOINT_STATE: {
610  if (q_dot.getRows() != 2) {
611  throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
612  "in joint state");
613  }
614  break;
615  }
617  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
618  "in the reference frame:"
619  "functionality not implemented");
620  }
621  case vpRobot::MIXT_FRAME: {
622  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
623  "in the mixt frame:"
624  "functionality not implemented");
625  }
627  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
628  "in the end-effector frame:"
629  "functionality not implemented");
630  }
631  default: {
632  throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
633  }
634  }
635 
636  vpDEBUG_TRACE(12, "Velocity limitation.");
637  bool norm = false; // Flag to indicate when velocities need to be normalized
638 
639  // Saturate joint speed
640  double max = vpBiclops::speedLimit;
641  vpColVector q_dot_sat(vpBiclops::ndof);
642 
643  // init q_dot_saturated
644  q_dot_sat = q_dot;
645 
646  for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
647  {
648  if (fabs(q_dot[i]) > max) {
649  norm = true;
650  max = fabs(q_dot[i]);
651  vpERROR_TRACE("Excess velocity: ROTATION "
652  "(axe nr.%d).",
653  i);
654  }
655  }
656  // Rotations velocities normalization
657  if (norm == true) {
658  max = vpBiclops::speedLimit / max;
659  q_dot_sat = q_dot * max;
660  }
661 
662  vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
663 
664  vpRobotBiclopsController::shmType shm;
665 
666  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
667  m_mutex_shm.lock();
668 
669  shm = m_controller->readShm();
670 
671  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
672  shm.q_dot[i] = q_dot[i];
673 
674  m_controller->writeShm(shm);
675 
676  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
677  m_mutex_shm.unlock();
678 
679  return;
680 }
681 
683 {
684  switch (frame) {
686  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
687  "not implemented");
688  break;
690  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
691  "not implemented");
692  break;
693  case vpRobot::MIXT_FRAME:
694  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
695  "not implemented");
696  break;
698  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
699  "not implemented");
700  break;
702  break;
703  }
704 
706  state = vpRobot::getRobotState();
707 
708  switch (state) {
709  case STATE_STOP:
711  q_dot = m_controller->getVelocity();
712 
713  break;
716  default:
717  q_dot.resize(vpBiclops::ndof);
718 
719  vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
720  m_mutex_measure.lock(); // Wait until a position is available
721 
722  vpRobotBiclopsController::shmType shm;
723 
724  vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
725  m_mutex_shm.lock();
726 
727  shm = m_controller->readShm();
728 
729  vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
730  m_mutex_shm.unlock();
731 
732  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
733  q_dot[i] = shm.actual_q_dot[i];
734  }
735 
736  vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
737 
738  vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
739  m_mutex_measure.unlock(); // A position is available
740 
741  break;
742  }
743 }
744 
746 {
747  vpColVector q_dot;
748  getVelocity(frame, q_dot);
749 
750  return q_dot;
751 }
752 
753 bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
754 {
755  std::ifstream fd(filename.c_str(), std::ios::in);
756 
757  if (!fd.is_open()) {
758  return false;
759  }
760 
761  std::string line;
762  std::string key("R:");
763  std::string id("#PTU-EVI - Position");
764  bool pos_found = false;
765  int lineNum = 0;
766 
768 
769  while (std::getline(fd, line)) {
770  lineNum++;
771  if (lineNum == 1) {
772  if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
773  std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
774  return false;
775  }
776  }
777  if ((line.compare(0, 1, "#") == 0)) { // skip comment
778  continue;
779  }
780  if ((line.compare(0, key.size(), key) == 0)) { // decode position
781  // check if there are at least njoint values in the line
782  std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
783  if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator
784  chain = vpIoTools::splitChain(line, std::string("\t"));
785  if (chain.size() < vpBiclops::ndof + 1)
786  continue;
787 
788  std::istringstream ss(line);
789  std::string key_;
790  ss >> key_;
791  for (unsigned int i = 0; i < vpBiclops::ndof; i++)
792  ss >> q[i];
793  pos_found = true;
794  break;
795  }
796  }
797 
798  // converts rotations from degrees into radians
799  q.deg2rad();
800 
801  fd.close();
802 
803  if (!pos_found) {
804  std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
805  return false;
806  }
807 
808  return true;
809 }
810 
812 {
813  vpColVector q_current; // current position
814 
815  getPosition(vpRobot::JOINT_STATE, q_current);
816 
817  switch (frame) {
819  d.resize(vpBiclops::ndof);
820  d = q_current - m_q_previous;
821  break;
822 
823  case vpRobot::CAMERA_FRAME: {
824  d.resize(6);
825  vpHomogeneousMatrix fMc_current;
826  vpHomogeneousMatrix fMc_previous;
827  fMc_current = vpBiclops::get_fMc(q_current);
828  fMc_previous = vpBiclops::get_fMc(m_q_previous);
829  vpHomogeneousMatrix c_previousMc_current;
830  // fMc_c = fMc_p * c_pMc_c
831  // => c_pMc_c = (fMc_p)^-1 * fMc_c
832  c_previousMc_current = fMc_previous.inverse() * fMc_current;
833 
834  // Compute the instantaneous velocity from this homogeneous matrix.
835  d = vpExponentialMap::inverse(c_previousMc_current);
836  break;
837  }
838 
840  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
841  "functionality not implemented");
842  break;
843  case vpRobot::MIXT_FRAME:
844  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
845  "functionality not implemented");
846  break;
848  throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
849  "functionality not implemented");
850  break;
851  }
852 
853  m_q_previous = q_current; // Update for next call of this method
854 }
855 
856 #elif !defined(VISP_BUILD_SHARED_LIBS)
857 // Work around to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no symbols
858 void dummy_vpRobotBiclops() { };
859 #endif
unsigned int getRows() const
Definition: vpArray2D.h:339
Jacobian, geometric model functionalities... for Biclops, pan, tilt head.
Definition: vpBiclops.h:62
static const float speedLimit
Pan and tilt axis max velocity in rad/s to perform a displacement.
Definition: vpBiclops.h:105
static const float tiltJointLimit
Tilt axis +/- joint limit in rad.
Definition: vpBiclops.h:104
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:236
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:56
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:193
static const float panJointLimit
Pan axis +/- joint limit in rad.
Definition: vpBiclops.h:103
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:211
static const unsigned int ndof
Number of dof.
Definition: vpBiclops.h:99
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector & deg2rad()
Definition: vpColVector.h:344
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1058
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static std::vector< std::string > splitChain(const std::string &chain, const std::string &sep)
Definition: vpIoTools.cpp:2427
static double rad(double deg)
Definition: vpMath.h:127
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:252
static double deg(double rad)
Definition: vpMath.h:117
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
void get_fJe(vpMatrix &fJe) vp_override
void setPositioningVelocity(double velocity)
virtual ~vpRobotBiclops()
double getPositioningVelocity(void)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d) vp_override
static const double defaultPositioningVelocity
void init() vp_override
bool readPositionFile(const std::string &filename, vpColVector &q)
void get_cVe(vpVelocityTwistMatrix &cVe) const
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) vp_override
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) vp_override
void setConfigFile(const std::string &filename="/usr/share/BiclopsDefault.cfg")
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
void get_eJe(vpMatrix &eJe) vp_override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
static void vpRobotBiclopsSpeedControlLoop(void *arg)
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ constructionError
Error from constructor.
@ readingParametersError
Cannot parse parameters.
Class that defines a generic virtual robot.
Definition: vpRobot.h:57
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition: vpRobot.h:104
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:153
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ JOINT_STATE
Definition: vpRobot.h:80
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:63
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:67
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:64
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition: vpRobot.h:108
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:198
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:499
#define vpCERROR
Definition: vpDebug.h:358
#define vpDEBUG_TRACE
Definition: vpDebug.h:475
#define vpERROR_TRACE
Definition: vpDebug.h:384
VISP_EXPORT int wait(double t0, double t)