Visual Servoing Platform version 3.5.0
vpRobotBiclops.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Interface for the Biclops robot.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <cmath> // std::fabs
40#include <errno.h>
41#include <limits> // numeric_limits
42#include <signal.h>
43#include <string.h>
44
45#include <visp3/core/vpConfig.h>
46
47#ifdef VISP_HAVE_BICLOPS
48
49#include <visp3/core/vpTime.h>
50#include <visp3/core/vpExponentialMap.h>
51#include <visp3/core/vpIoTools.h>
52#include <visp3/robot/vpBiclops.h>
53#include <visp3/robot/vpRobotBiclops.h>
54#include <visp3/robot/vpRobotException.h>
55
56//#define VP_DEBUG // Activate the debug mode
57//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
58#include <visp3/core/vpDebug.h>
59
60/* ---------------------------------------------------------------------- */
61/* --- STATIC ------------------------------------------------------------ */
62/* ------------------------------------------------------------------------ */
63
64bool vpRobotBiclops::robotAlreadyCreated = false;
66
67static pthread_mutex_t vpEndThread_mutex;
68static pthread_mutex_t vpShm_mutex;
69static pthread_mutex_t vpMeasure_mutex;
70
71/* ----------------------------------------------------------------------- */
72/* --- CONSTRUCTOR ------------------------------------------------------ */
73/* ---------------------------------------------------------------------- */
74
113 : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
114 q_previous(), controlThreadCreated(false)
115{
116 vpDEBUG_TRACE(12, "Begin default constructor.");
117
118 vpRobotBiclops::robotAlreadyCreated = false;
119 setConfigFile("/usr/share/BiclopsDefault.cfg");
120
121 // Initialize the mutex dedicated to she shm protection
122 pthread_mutex_init(&vpShm_mutex, NULL);
123 pthread_mutex_init(&vpEndThread_mutex, NULL);
124 pthread_mutex_init(&vpMeasure_mutex, NULL);
125
126 control_thread = 0;
127}
128
159vpRobotBiclops::vpRobotBiclops(const std::string &filename)
160 : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
161 q_previous(), controlThreadCreated(false)
162{
163 vpDEBUG_TRACE(12, "Begin default constructor.");
164
165 vpRobotBiclops::robotAlreadyCreated = false;
166 setConfigFile(filename);
167
168 // Initialize the mutex dedicated to she shm protection
169 pthread_mutex_init(&vpShm_mutex, NULL);
170 pthread_mutex_init(&vpEndThread_mutex, NULL);
171 pthread_mutex_init(&vpMeasure_mutex, NULL);
172
173 init();
174
175 return;
176}
177
186{
187
188 vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
190
191 vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
192 pthread_mutex_unlock(&vpEndThread_mutex);
193
194 /* wait the end of the control thread */
195 vpDEBUG_TRACE(12, "Wait end of control thread");
196
197 if (controlThreadCreated == true) {
198 int code = pthread_join(control_thread, NULL);
199 if (code != 0) {
200 vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno)
201 << " strCode=" << strerror(code) << std::endl;
202 }
203 }
204
205 pthread_mutex_destroy(&vpShm_mutex);
206 pthread_mutex_destroy(&vpEndThread_mutex);
207 pthread_mutex_destroy(&vpMeasure_mutex);
208
209 vpRobotBiclops::robotAlreadyCreated = false;
210
211 vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
212 return;
213}
214
215/* -------------------------------------------------------------------------
216 */
217/* --- INITIALISATION ------------------------------------------------------
218 */
219/* -------------------------------------------------------------------------
220 */
221
227void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; }
228
239{
240 // test if the config file exists
241 FILE *fd = fopen(configfile.c_str(), "r");
242 if (fd == NULL) {
243 vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
244 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
245 }
246 fclose(fd);
247
248 // Initialize the controller
249 controller.init(configfile);
250
251 try {
253 } catch (...) {
254 vpERROR_TRACE("Error caught");
255 throw;
256 }
257
258 vpRobotBiclops::robotAlreadyCreated = true;
259
260 // Initialize previous articular position to manage getDisplacement()
261 q_previous.resize(vpBiclops::ndof);
262 q_previous = 0;
263
264 controlThreadCreated = false;
265
266 return;
267}
268
269/*
270 Control loop to manage the biclops joint limits in speed control.
271
272 This control loop is running in a seperate thread in order to detect each 5
273 ms joint limits during the speed control. If a joint limit is detected the
274 axis should be halted.
275
276 \warning Velocity control mode is not exported from the top-level Biclops
277 API class provided by Traclabs. That means that there is no protection in
278 this mode to prevent an axis from striking its hard limit. In position mode,
279 Traclabs put soft limits in that keep any command from driving to a position
280 too close to the hard limits. In velocity mode this protection does not
281 exist in the current API.
282
283 \warning With the understanding that hitting the hard limits at full
284 speed/power can damage the unit, damage due to velocity mode commanding is
285 under user responsibility.
286*/
288{
289 vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
290
291 int iter = 0;
292 // PMDAxisControl *panAxis = controller->getPanAxis();
293 // PMDAxisControl *tiltAxis = controller->getTiltAxis();
294 vpRobotBiclopsController::shmType shm;
295
296 vpDEBUG_TRACE(10, "Start control loop");
297 vpColVector mes_q;
298 vpColVector mes_q_dot;
299 vpColVector softLimit(vpBiclops::ndof);
301 bool *new_q_dot = new bool[vpBiclops::ndof];
302 bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
303 bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
304 bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
305 vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
306 double secure = vpMath::rad(2); // add a security angle before joint limit
307
308 // Set the soft limits
309 softLimit[0] = vpBiclops::panJointLimit - secure;
310 softLimit[1] = vpBiclops::tiltJointLimit - secure;
311 vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1]));
312
313 // Initilisation
314 vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
315 pthread_mutex_lock(&vpShm_mutex);
316
317 shm = controller->readShm();
318
319 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
320 pthread_mutex_unlock(&vpShm_mutex);
321
322 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
323 prev_q_dot[i] = shm.q_dot[i];
324 new_q_dot[i] = false;
325 change_dir[i] = false;
326 force_halt[i] = false;
327 enable_limit[i] = true;
328 }
329
330 // Initialize actual position and velocity
331 mes_q = controller->getActualPosition();
332 mes_q_dot = controller->getActualVelocity();
333
334 vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
335 pthread_mutex_lock(&vpShm_mutex);
336
337 shm = controller->readShm();
338 // Updates the shm
339 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
340 shm.actual_q[i] = mes_q[i];
341 shm.actual_q_dot[i] = mes_q_dot[i];
342 }
343 // Update the actuals positions
344 controller->writeShm(shm);
345
346 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
347 pthread_mutex_unlock(&vpShm_mutex);
348
349 vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
350 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
351
352 while (!controller->isStopRequested()) {
353
354 // Get 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
363 // Updates the shm
364 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
365 shm.actual_q[i] = mes_q[i];
366 shm.actual_q_dot[i] = mes_q_dot[i];
367 }
368
369 vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
370 vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
371 vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
372 vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
373
374 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
375 // test if joint limits are reached
376 if (mes_q[i] < -softLimit[i]) {
377 vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
378 shm.status[i] = vpRobotBiclopsController::STOP;
379 shm.jointLimit[i] = true;
380 } else if (mes_q[i] > softLimit[i]) {
381 vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
382 shm.status[i] = vpRobotBiclopsController::STOP;
383 shm.jointLimit[i] = true;
384 } else {
385 shm.status[i] = vpRobotBiclopsController::SPEED;
386 shm.jointLimit[i] = false;
387 }
388
389 // Test if new a speed is demanded
390 // if (shm.q_dot[i] != prev_q_dot[i])
391 if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
392 std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
393 new_q_dot[i] = true;
394 else
395 new_q_dot[i] = false;
396
397 // Test if desired speed change of sign
398 if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
399 change_dir[i] = true;
400 else
401 change_dir[i] = false;
402 }
403 vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
404 vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
405 vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
406 vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
407 vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
408 vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
409
410 bool updateVelocity = false;
411 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
412 // Test if a new desired speed is to apply
413 if (new_q_dot[i]) {
414 // A new desired speed is to apply
415 if (shm.status[i] == vpRobotBiclopsController::STOP) {
416 // Axis in joint limit
417 if (change_dir[i] == false) {
418 // New desired speed without change of direction
419 // We go in the joint limit
420 if (enable_limit[i] == true) { // limit detection active
421 // We have to stop this axis
422 // Test if this axis was stopped before
423 if (force_halt[i] == false) {
424 q_dot[i] = 0.;
425 force_halt[i] = true; // indicate that it will be stopped
426 updateVelocity = true; // We have to send this new speed
427 }
428 } else {
429 // We have to apply the desired speed to go away the joint
430 // Update the desired speed
431 q_dot[i] = shm.q_dot[i];
432 shm.status[i] = vpRobotBiclopsController::SPEED;
433 force_halt[i] = false;
434 updateVelocity = true; // We have to send this new speed
435 }
436 } else {
437 // New desired speed and change of direction.
438 if (enable_limit[i] == true) { // limit detection active
439 // Update the desired speed to go away the joint limit
440 q_dot[i] = shm.q_dot[i];
441 shm.status[i] = vpRobotBiclopsController::SPEED;
442 force_halt[i] = false;
443 enable_limit[i] = false; // Disable joint limit detection
444 updateVelocity = true; // We have to send this new speed
445 } else {
446 // We have to stop this axis
447 // Test if this axis was stopped before
448 if (force_halt[i] == false) {
449 q_dot[i] = 0.;
450 force_halt[i] = true; // indicate that it will be stopped
451 enable_limit[i] = true; // Joint limit detection must be active
452 updateVelocity = true; // We have to send this new speed
453 }
454 }
455 }
456 } else {
457 // Axis not in joint limit
458
459 // Update the desired speed
460 q_dot[i] = shm.q_dot[i];
461 shm.status[i] = vpRobotBiclopsController::SPEED;
462 enable_limit[i] = true; // Joint limit detection must be active
463 updateVelocity = true; // We have to send this new speed
464 }
465 } else {
466 // No change of the desired speed. We have to stop the robot in case
467 // of joint limit
468 if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
469 if (enable_limit[i] == true) { // limit detection active
470
471 // Test if this axis was stopped before
472 if (force_halt[i] == false) {
473 // We have to stop this axis
474 q_dot[i] = 0.;
475 force_halt[i] = true; // indicate that it will be stopped
476 updateVelocity = true; // We have to send this new speed
477 }
478 }
479 } else {
480 // No need to stop the robot
481 enable_limit[i] = true; // Normal situation, activate limit detection
482 }
483 }
484 }
485 // Update the actuals positions
486 controller->writeShm(shm);
487
488 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
489 pthread_mutex_unlock(&vpShm_mutex);
490
491 if (updateVelocity) {
492 vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1]));
493
494 // Apply the velocity
495 controller->setVelocity(q_dot);
496 }
497
498 // Update the previous speed for next iteration
499 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
500 prev_q_dot[i] = shm.q_dot[i];
501
502 vpDEBUG_TRACE(12, "iter: %d", iter);
503
504 // wait 5 ms
505 vpTime::wait(5.0);
506
507 // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
508 // vpDEBUG_TRACE (12, "Calling thread will end");
509 // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
510 // std::cout << "Calling thread will end" << std::endl;
511 // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
512 //
513 // pthread_mutex_unlock(&vpEndThread_mutex);
514 // break;
515 // }
516
517 iter++;
518 }
519 controller->stopRequest(false);
520 // Stop the robot
521 vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
522 q_dot = 0;
523 controller->setVelocity(q_dot);
524
525 delete[] new_q_dot;
526 delete[] change_dir;
527 delete[] force_halt;
528 delete[] enable_limit;
529 vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
530 pthread_mutex_unlock(&vpEndThread_mutex);
531
532 vpDEBUG_TRACE(10, "Exit control thread ");
533 // pthread_exit(0);
534
535 return NULL;
536}
537
545{
546 switch (newState) {
547 case vpRobot::STATE_STOP: {
549 stopMotion();
550 }
551 break;
552 }
555 vpDEBUG_TRACE(12, "Speed to position control.");
556 stopMotion();
557 }
558
559 break;
560 }
562
564 vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
565 pthread_mutex_lock(&vpEndThread_mutex);
566
567 vpDEBUG_TRACE(12, "Create speed control thread");
568 int code;
569 code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller);
570 if (code != 0) {
571 vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno)
572 << " strCode=" << strerror(code) << std::endl;
573 }
574
575 controlThreadCreated = true;
576
577 vpDEBUG_TRACE(12, "Speed control thread created");
578 }
579 break;
580 }
581 default:
582 break;
583 }
584
585 return vpRobot::setRobotState(newState);
586}
587
594{
596 q_dot = 0;
597 controller.setVelocity(q_dot);
598 // std::cout << "Request to stop the velocity controller thread...."<<
599 // std::endl;
600 controller.stopRequest(true);
601}
602
614{
616 cMe = vpBiclops::get_cMe();
617
618 cVe.buildFrom(cMe);
619}
620
631
643{
644 vpColVector q(2);
646
647 try {
648 vpBiclops::get_eJe(q, _eJe);
649 } catch (...) {
650 vpERROR_TRACE("catch exception ");
651 throw;
652 }
653}
654
663{
664 vpColVector q(2);
666
667 try {
668 vpBiclops::get_fJe(q, _fJe);
669 } catch (...) {
670 vpERROR_TRACE("Error caught");
671 throw;
672 }
673}
674
683{
684 if (velocity < 0 || velocity > 100) {
685 vpERROR_TRACE("Bad positionning velocity");
686 throw vpRobotException(vpRobotException::constructionError, "Bad positionning velocity");
687 }
688
689 positioningVelocity = velocity;
690}
698double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; }
699
716{
717
719 vpERROR_TRACE("Robot was not in position-based control\n"
720 "Modification of the robot state");
722 }
723
724 switch (frame) {
726 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
727 "not implemented");
728 break;
730 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
731 "not implemented");
732 break;
734 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
735 "not implemented");
736 break;
738 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
739 "not implemented");
740 break;
742 break;
743 }
744
745 // test if position reachable
746 // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
747 // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
748 // vpERROR_TRACE ("Positionning error.");
749 // throw vpRobotException (vpRobotException::wrongStateError,
750 // "Positionning error.");
751 // }
752
753 vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
754 pthread_mutex_lock(&vpEndThread_mutex);
755 controller.setPosition(q, positioningVelocity);
756 vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
757 pthread_mutex_unlock(&vpEndThread_mutex);
758 return;
759}
760
777void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
778{
779 try {
780 vpColVector q(2);
781 q[0] = q1;
782 q[1] = q2;
783
784 setPosition(frame, q);
785 } catch (...) {
786 vpERROR_TRACE("Error caught");
787 throw;
788 }
789}
790
804void vpRobotBiclops::setPosition(const char *filename)
805{
806 vpColVector q;
807 if (readPositionFile(filename, q) == false) {
808 vpERROR_TRACE("Cannot get biclops position from file");
809 throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file");
810 }
812}
813
830{
831 switch (frame) {
833 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
834 "not implemented");
835 break;
837 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
838 "not implemented");
839 break;
841 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
842 "not implemented");
843 break;
845 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
846 "not implemented");
847 break;
849 break;
850 }
851
853 state = vpRobot::getRobotState();
854
855 switch (state) {
856 case STATE_STOP:
858 q = controller.getPosition();
859
860 break;
863 default:
865
866 vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
867 pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
868
869 vpRobotBiclopsController::shmType shm;
870
871 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
872 pthread_mutex_lock(&vpShm_mutex);
873
874 shm = controller.readShm();
875
876 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
877 pthread_mutex_unlock(&vpShm_mutex);
878
879 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
880 q[i] = shm.actual_q[i];
881 }
882
883 vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
884
885 vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
886 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
887
888 break;
889 }
890}
891
919{
921 vpERROR_TRACE("Cannot send a velocity to the robot "
922 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
924 "Cannot send a velocity to the robot "
925 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
926 }
927
928 switch (frame) {
930 vpERROR_TRACE("Cannot send a velocity to the robot "
931 "in the camera frame: "
932 "functionality not implemented");
933 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
934 "in the camera frame:"
935 "functionality not implemented");
936 }
938 if (q_dot.getRows() != 2) {
939 vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
940 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
941 "in articular frame");
942 }
943 break;
944 }
946 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
947 "in the reference frame:"
948 "functionality not implemented");
949 }
950 case vpRobot::MIXT_FRAME: {
951 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
952 "in the mixt frame:"
953 "functionality not implemented");
954 }
956 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
957 "in the end-effector frame:"
958 "functionality not implemented");
959 }
960 default: {
961 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
962 }
963 }
964
965 vpDEBUG_TRACE(12, "Velocity limitation.");
966 bool norm = false; // Flag to indicate when velocities need to be nomalized
967
968 // Saturate articular speed
969 double max = vpBiclops::speedLimit;
970 vpColVector q_dot_sat(vpBiclops::ndof);
971
972 // init q_dot_saturated
973 q_dot_sat = q_dot;
974
975 for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
976 {
977 if (fabs(q_dot[i]) > max) {
978 norm = true;
979 max = fabs(q_dot[i]);
980 vpERROR_TRACE("Excess velocity: ROTATION "
981 "(axe nr.%d).",
982 i);
983 }
984 }
985 // Rotations velocities normalisation
986 if (norm == true) {
987 max = vpBiclops::speedLimit / max;
988 q_dot_sat = q_dot * max;
989 }
990
991 vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
992
993 vpRobotBiclopsController::shmType shm;
994
995 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
996 pthread_mutex_lock(&vpShm_mutex);
997
998 shm = controller.readShm();
999
1000 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1001 shm.q_dot[i] = q_dot[i];
1002
1003 controller.writeShm(shm);
1004
1005 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1006 pthread_mutex_unlock(&vpShm_mutex);
1007
1008 return;
1009}
1010
1011/* -------------------------------------------------------------------------
1012 */
1013/* --- GET -----------------------------------------------------------------
1014 */
1015/* -------------------------------------------------------------------------
1016 */
1017
1030{
1031 switch (frame) {
1033 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
1034 "not implemented");
1035 break;
1037 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
1038 "not implemented");
1039 break;
1041 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
1042 "not implemented");
1043 break;
1045 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
1046 "not implemented");
1047 break;
1049 break;
1050 }
1051
1053 state = vpRobot::getRobotState();
1054
1055 switch (state) {
1056 case STATE_STOP:
1058 q_dot = controller.getVelocity();
1059
1060 break;
1063 default:
1064 q_dot.resize(vpBiclops::ndof);
1065
1066 vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
1067 pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1068
1069 vpRobotBiclopsController::shmType shm;
1070
1071 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
1072 pthread_mutex_lock(&vpShm_mutex);
1073
1074 shm = controller.readShm();
1075
1076 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1077 pthread_mutex_unlock(&vpShm_mutex);
1078
1079 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
1080 q_dot[i] = shm.actual_q_dot[i];
1081 }
1082
1083 vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1084
1085 vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
1086 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1087
1088 break;
1089 }
1090}
1091
1104{
1105 vpColVector q_dot;
1106 getVelocity(frame, q_dot);
1107
1108 return q_dot;
1109}
1110
1130bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
1131{
1132 std::ifstream fd(filename.c_str(), std::ios::in);
1133
1134 if (!fd.is_open()) {
1135 return false;
1136 }
1137
1138 std::string line;
1139 std::string key("R:");
1140 std::string id("#PTU-EVI - Position");
1141 bool pos_found = false;
1142 int lineNum = 0;
1143
1145
1146 while (std::getline(fd, line)) {
1147 lineNum++;
1148 if (lineNum == 1) {
1149 if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
1150 std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
1151 return false;
1152 }
1153 }
1154 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1155 continue;
1156 }
1157 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1158 // check if there are at least njoint values in the line
1159 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1160 if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator
1161 chain = vpIoTools::splitChain(line, std::string("\t"));
1162 if (chain.size() < vpBiclops::ndof + 1)
1163 continue;
1164
1165 std::istringstream ss(line);
1166 std::string key_;
1167 ss >> key_;
1168 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1169 ss >> q[i];
1170 pos_found = true;
1171 break;
1172 }
1173 }
1174
1175 // converts rotations from degrees into radians
1176 q.deg2rad();
1177
1178 fd.close();
1179
1180 if (!pos_found) {
1181 std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
1182 return false;
1183 }
1184
1185 return true;
1186}
1187
1211{
1212 vpColVector q_current; // current position
1213
1215
1216 switch (frame) {
1218 d.resize(vpBiclops::ndof);
1219 d = q_current - q_previous;
1220 break;
1221
1222 case vpRobot::CAMERA_FRAME: {
1223 d.resize(6);
1224 vpHomogeneousMatrix fMc_current;
1225 vpHomogeneousMatrix fMc_previous;
1226 fMc_current = vpBiclops::get_fMc(q_current);
1227 fMc_previous = vpBiclops::get_fMc(q_previous);
1228 vpHomogeneousMatrix c_previousMc_current;
1229 // fMc_c = fMc_p * c_pMc_c
1230 // => c_pMc_c = (fMc_p)^-1 * fMc_c
1231 c_previousMc_current = fMc_previous.inverse() * fMc_current;
1232
1233 // Compute the instantaneous velocity from this homogeneous matrix.
1234 d = vpExponentialMap::inverse(c_previousMc_current);
1235 break;
1236 }
1237
1239 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
1240 "functionality not implemented");
1241 break;
1243 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
1244 "functionality not implemented");
1245 break;
1247 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
1248 "functionality not implemented");
1249 break;
1250 }
1251
1252 q_previous = q_current; // Update for next call of this method
1253}
1254
1255#elif !defined(VISP_BUILD_SHARED_LIBS)
1256// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no
1257// symbols
1258void dummy_vpRobotBiclops(){};
1259#endif
unsigned int getRows() const
Definition: vpArray2D.h:289
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
Definition: vpBiclops.h:78
static const float speedLimit
Definition: vpBiclops.h:133
static const float tiltJointLimit
Definition: vpBiclops.h:132
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
Definition: vpBiclops.cpp:394
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition: vpBiclops.cpp:95
vpHomogeneousMatrix get_cMe() const
Definition: vpBiclops.h:157
static const float panJointLimit
Definition: vpBiclops.h:131
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
Definition: vpBiclops.cpp:360
static const unsigned int ndof
Definition: vpBiclops.h:126
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void deg2rad()
Definition: vpColVector.h:196
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
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:1900
static double rad(double deg)
Definition: vpMath.h:110
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:145
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void setPositioningVelocity(double velocity)
virtual ~vpRobotBiclops()
double getPositioningVelocity(void)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const double defaultPositioningVelocity
void get_fJe(vpMatrix &_fJe)
bool readPositionFile(const std::string &filename, vpColVector &q)
void get_eJe(vpMatrix &_eJe)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
void get_cVe(vpVelocityTwistMatrix &_cVe) const
void setConfigFile(const std::string &filename="/usr/share/BiclopsDefault.cfg")
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
Error that can be emited by the vpRobot class and its derivates.
Class that defines a generic virtual robot.
Definition: vpRobot.h:59
virtual vpRobotStateType getRobotState(void) const
Definition: vpRobot.h:144
vpControlFrameType
Definition: vpRobot.h:75
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ MIXT_FRAME
Definition: vpRobot.h:86
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
vpRobotStateType
Definition: vpRobot.h:64
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition: vpRobot.h:68
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition: vpRobot.h:65
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpCERROR
Definition: vpDebug.h:365
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393
VISP_EXPORT int wait(double t0, double t)