Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotBiclops.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
36#include <cmath> // std::fabs
37#include <errno.h>
38#include <limits> // numeric_limits
39#include <signal.h>
40#include <string.h>
41
42#include <visp3/core/vpConfig.h>
43
44#ifdef VISP_HAVE_BICLOPS
45
46#include <visp3/core/vpExponentialMap.h>
47#include <visp3/core/vpIoTools.h>
48#include <visp3/core/vpTime.h>
49#include <visp3/robot/vpBiclops.h>
50#include <visp3/robot/vpRobotBiclops.h>
51#include <visp3/robot/vpRobotException.h>
52
53//#define VP_DEBUG // Activate the debug mode
54//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
55#include <visp3/core/vpDebug.h>
56
57/* ---------------------------------------------------------------------- */
58/* --- STATIC ------------------------------------------------------------ */
59/* ------------------------------------------------------------------------ */
60
61bool vpRobotBiclops::robotAlreadyCreated = false;
63
64static pthread_mutex_t vpEndThread_mutex;
65static pthread_mutex_t vpShm_mutex;
66static pthread_mutex_t vpMeasure_mutex;
67
68/* ----------------------------------------------------------------------- */
69/* --- CONSTRUCTOR ------------------------------------------------------ */
70/* ---------------------------------------------------------------------- */
71
110 : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
111 q_previous(), controlThreadCreated(false)
112{
113 vpDEBUG_TRACE(12, "Begin default constructor.");
114
115 vpRobotBiclops::robotAlreadyCreated = false;
116 setConfigFile("/usr/share/BiclopsDefault.cfg");
117
118 // Initialize the mutex dedicated to she shm protection
119 pthread_mutex_init(&vpShm_mutex, NULL);
120 pthread_mutex_init(&vpEndThread_mutex, NULL);
121 pthread_mutex_init(&vpMeasure_mutex, NULL);
122
123 control_thread = 0;
124}
125
156vpRobotBiclops::vpRobotBiclops(const std::string &filename)
157 : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
158 q_previous(), controlThreadCreated(false)
159{
160 vpDEBUG_TRACE(12, "Begin default constructor.");
161
162 vpRobotBiclops::robotAlreadyCreated = false;
163 setConfigFile(filename);
164
165 // Initialize the mutex dedicated to she shm protection
166 pthread_mutex_init(&vpShm_mutex, NULL);
167 pthread_mutex_init(&vpEndThread_mutex, NULL);
168 pthread_mutex_init(&vpMeasure_mutex, NULL);
169
170 init();
171
172 return;
173}
174
183{
184
185 vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
187
188 vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
189 pthread_mutex_unlock(&vpEndThread_mutex);
190
191 /* wait the end of the control thread */
192 vpDEBUG_TRACE(12, "Wait end of control thread");
193
194 if (controlThreadCreated == true) {
195 int code = pthread_join(control_thread, NULL);
196 if (code != 0) {
197 vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno)
198 << " strCode=" << strerror(code) << std::endl;
199 }
200 }
201
202 pthread_mutex_destroy(&vpShm_mutex);
203 pthread_mutex_destroy(&vpEndThread_mutex);
204 pthread_mutex_destroy(&vpMeasure_mutex);
205
206 vpRobotBiclops::robotAlreadyCreated = false;
207
208 vpDEBUG_TRACE(12, "Stop vpRobotBiclops::~vpRobotBiclops()");
209 return;
210}
211
212/* -------------------------------------------------------------------------
213 */
214/* --- INITIALISATION ------------------------------------------------------
215 */
216/* -------------------------------------------------------------------------
217 */
218
224void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; }
225
236{
237 // test if the config file exists
238 FILE *fd = fopen(configfile.c_str(), "r");
239 if (fd == NULL) {
240 vpCERROR << "Cannot open biclops config file: " << configfile << std::endl;
241 throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
242 }
243 fclose(fd);
244
245 // Initialize the controller
246 controller.init(configfile);
247
248 try {
250 } catch (...) {
251 vpERROR_TRACE("Error caught");
252 throw;
253 }
254
255 vpRobotBiclops::robotAlreadyCreated = true;
256
257 // Initialize previous articular position to manage getDisplacement()
258 q_previous.resize(vpBiclops::ndof);
259 q_previous = 0;
260
261 controlThreadCreated = false;
262
263 return;
264}
265
266/*
267 Control loop to manage the biclops joint limits in speed control.
268
269 This control loop is running in a separate thread in order to detect each 5
270 ms joint limits during the speed control. If a joint limit is detected the
271 axis should be halted.
272
273 \warning Velocity control mode is not exported from the top-level Biclops
274 API class provided by Traclabs. That means that there is no protection in
275 this mode to prevent an axis from striking its hard limit. In position mode,
276 Traclabs put soft limits in that keep any command from driving to a position
277 too close to the hard limits. In velocity mode this protection does not
278 exist in the current API.
279
280 \warning With the understanding that hitting the hard limits at full
281 speed/power can damage the unit, damage due to velocity mode commanding is
282 under user responsibility.
283*/
285{
286 vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
287
288 int iter = 0;
289 // PMDAxisControl *panAxis = controller->getPanAxis();
290 // PMDAxisControl *tiltAxis = controller->getTiltAxis();
291 vpRobotBiclopsController::shmType shm;
292
293 vpDEBUG_TRACE(10, "Start control loop");
294 vpColVector mes_q;
295 vpColVector mes_q_dot;
296 vpColVector softLimit(vpBiclops::ndof);
298 bool *new_q_dot = new bool[vpBiclops::ndof];
299 bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
300 bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
301 bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
302 vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
303 double secure = vpMath::rad(2); // add a security angle before joint limit
304
305 // Set the soft limits
306 softLimit[0] = vpBiclops::panJointLimit - secure;
307 softLimit[1] = vpBiclops::tiltJointLimit - secure;
308 vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f", vpMath::deg(softLimit[0]), vpMath::deg(softLimit[1]));
309
310 // Initilisation
311 vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
312 pthread_mutex_lock(&vpShm_mutex);
313
314 shm = controller->readShm();
315
316 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
317 pthread_mutex_unlock(&vpShm_mutex);
318
319 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
320 prev_q_dot[i] = shm.q_dot[i];
321 new_q_dot[i] = false;
322 change_dir[i] = false;
323 force_halt[i] = false;
324 enable_limit[i] = true;
325 }
326
327 // Initialize actual position and velocity
328 mes_q = controller->getActualPosition();
329 mes_q_dot = controller->getActualVelocity();
330
331 vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
332 pthread_mutex_lock(&vpShm_mutex);
333
334 shm = controller->readShm();
335 // Updates the shm
336 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
337 shm.actual_q[i] = mes_q[i];
338 shm.actual_q_dot[i] = mes_q_dot[i];
339 }
340 // Update the actuals positions
341 controller->writeShm(shm);
342
343 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
344 pthread_mutex_unlock(&vpShm_mutex);
345
346 vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
347 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
348
349 while (!controller->isStopRequested()) {
350
351 // Get actual position and velocity
352 mes_q = controller->getActualPosition();
353 mes_q_dot = controller->getActualVelocity();
354
355 vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
356 pthread_mutex_lock(&vpShm_mutex);
357
358 shm = controller->readShm();
359
360 // Updates the shm
361 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
362 shm.actual_q[i] = mes_q[i];
363 shm.actual_q_dot[i] = mes_q_dot[i];
364 }
365
366 vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
367 vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
368 vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
369 vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
370
371 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
372 // test if joint limits are reached
373 if (mes_q[i] < -softLimit[i]) {
374 vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
375 shm.status[i] = vpRobotBiclopsController::STOP;
376 shm.jointLimit[i] = true;
377 } else if (mes_q[i] > softLimit[i]) {
378 vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
379 shm.status[i] = vpRobotBiclopsController::STOP;
380 shm.jointLimit[i] = true;
381 } else {
382 shm.status[i] = vpRobotBiclopsController::SPEED;
383 shm.jointLimit[i] = false;
384 }
385
386 // Test if new a speed is demanded
387 // if (shm.q_dot[i] != prev_q_dot[i])
388 if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
389 std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
390 new_q_dot[i] = true;
391 else
392 new_q_dot[i] = false;
393
394 // Test if desired speed change of sign
395 if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
396 change_dir[i] = true;
397 else
398 change_dir[i] = false;
399 }
400 vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
401 vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
402 vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
403 vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
404 vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
405 vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
406
407 bool updateVelocity = false;
408 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
409 // Test if a new desired speed is to apply
410 if (new_q_dot[i]) {
411 // A new desired speed is to apply
412 if (shm.status[i] == vpRobotBiclopsController::STOP) {
413 // Axis in joint limit
414 if (change_dir[i] == false) {
415 // New desired speed without change of direction
416 // We go in the joint limit
417 if (enable_limit[i] == true) { // limit detection active
418 // We have to stop this axis
419 // Test if this axis was stopped before
420 if (force_halt[i] == false) {
421 q_dot[i] = 0.;
422 force_halt[i] = true; // indicate that it will be stopped
423 updateVelocity = true; // We have to send this new speed
424 }
425 } else {
426 // We have to apply the desired speed to go away the joint
427 // Update the desired speed
428 q_dot[i] = shm.q_dot[i];
429 shm.status[i] = vpRobotBiclopsController::SPEED;
430 force_halt[i] = false;
431 updateVelocity = true; // We have to send this new speed
432 }
433 } else {
434 // New desired speed and change of direction.
435 if (enable_limit[i] == true) { // limit detection active
436 // Update the desired speed to go away the joint limit
437 q_dot[i] = shm.q_dot[i];
438 shm.status[i] = vpRobotBiclopsController::SPEED;
439 force_halt[i] = false;
440 enable_limit[i] = false; // Disable joint limit detection
441 updateVelocity = true; // We have to send this new speed
442 } else {
443 // We have to stop this axis
444 // Test if this axis was stopped before
445 if (force_halt[i] == false) {
446 q_dot[i] = 0.;
447 force_halt[i] = true; // indicate that it will be stopped
448 enable_limit[i] = true; // Joint limit detection must be active
449 updateVelocity = true; // We have to send this new speed
450 }
451 }
452 }
453 } else {
454 // Axis not in joint limit
455
456 // Update the desired speed
457 q_dot[i] = shm.q_dot[i];
458 shm.status[i] = vpRobotBiclopsController::SPEED;
459 enable_limit[i] = true; // Joint limit detection must be active
460 updateVelocity = true; // We have to send this new speed
461 }
462 } else {
463 // No change of the desired speed. We have to stop the robot in case
464 // of joint limit
465 if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
466 if (enable_limit[i] == true) { // limit detection active
467
468 // Test if this axis was stopped before
469 if (force_halt[i] == false) {
470 // We have to stop this axis
471 q_dot[i] = 0.;
472 force_halt[i] = true; // indicate that it will be stopped
473 updateVelocity = true; // We have to send this new speed
474 }
475 }
476 } else {
477 // No need to stop the robot
478 enable_limit[i] = true; // Normal situation, activate limit detection
479 }
480 }
481 }
482 // Update the actuals positions
483 controller->writeShm(shm);
484
485 vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
486 pthread_mutex_unlock(&vpShm_mutex);
487
488 if (updateVelocity) {
489 vpDEBUG_TRACE(12, "apply q_dot : %f %f", vpMath::deg(q_dot[0]), vpMath::deg(q_dot[1]));
490
491 // Apply the velocity
492 controller->setVelocity(q_dot);
493 }
494
495 // Update the previous speed for next iteration
496 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
497 prev_q_dot[i] = shm.q_dot[i];
498
499 vpDEBUG_TRACE(12, "iter: %d", iter);
500
501 // wait 5 ms
502 vpTime::wait(5.0);
503
504 // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
505 // vpDEBUG_TRACE (12, "Calling thread will end");
506 // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
507 // std::cout << "Calling thread will end" << std::endl;
508 // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
509 //
510 // pthread_mutex_unlock(&vpEndThread_mutex);
511 // break;
512 // }
513
514 iter++;
515 }
516 controller->stopRequest(false);
517 // Stop the robot
518 vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
519 q_dot = 0;
520 controller->setVelocity(q_dot);
521
522 delete[] new_q_dot;
523 delete[] change_dir;
524 delete[] force_halt;
525 delete[] enable_limit;
526 vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
527 pthread_mutex_unlock(&vpEndThread_mutex);
528
529 vpDEBUG_TRACE(10, "Exit control thread ");
530 // pthread_exit(0);
531
532 return NULL;
533}
534
542{
543 switch (newState) {
544 case vpRobot::STATE_STOP: {
546 stopMotion();
547 }
548 break;
549 }
552 vpDEBUG_TRACE(12, "Speed to position control.");
553 stopMotion();
554 }
555
556 break;
557 }
559
561 vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
562 pthread_mutex_lock(&vpEndThread_mutex);
563
564 vpDEBUG_TRACE(12, "Create speed control thread");
565 int code;
566 code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller);
567 if (code != 0) {
568 vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno)
569 << " strCode=" << strerror(code) << std::endl;
570 }
571
572 controlThreadCreated = true;
573
574 vpDEBUG_TRACE(12, "Speed control thread created");
575 }
576 break;
577 }
578 default:
579 break;
580 }
581
582 return vpRobot::setRobotState(newState);
583}
584
591{
593 q_dot = 0;
594 controller.setVelocity(q_dot);
595 // std::cout << "Request to stop the velocity controller thread...."<<
596 // std::endl;
597 controller.stopRequest(true);
598}
599
611{
613 cMe = vpBiclops::get_cMe();
614
615 cVe.buildFrom(cMe);
616}
617
628
640{
641 vpColVector q(2);
643
644 try {
645 vpBiclops::get_eJe(q, _eJe);
646 } catch (...) {
647 vpERROR_TRACE("catch exception ");
648 throw;
649 }
650}
651
660{
661 vpColVector q(2);
663
664 try {
665 vpBiclops::get_fJe(q, _fJe);
666 } catch (...) {
667 vpERROR_TRACE("Error caught");
668 throw;
669 }
670}
671
680{
681 if (velocity < 0 || velocity > 100) {
682 vpERROR_TRACE("Bad positioning velocity");
683 throw vpRobotException(vpRobotException::constructionError, "Bad positioning velocity");
684 }
685
686 positioningVelocity = velocity;
687}
695double vpRobotBiclops::getPositioningVelocity(void) { return positioningVelocity; }
696
713{
714
716 vpERROR_TRACE("Robot was not in position-based control\n"
717 "Modification of the robot state");
719 }
720
721 switch (frame) {
723 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
724 "not implemented");
725 break;
727 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
728 "not implemented");
729 break;
731 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
732 "not implemented");
733 break;
735 throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in end-effector frame: "
736 "not implemented");
737 break;
739 break;
740 }
741
742 vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
743 pthread_mutex_lock(&vpEndThread_mutex);
744 controller.setPosition(q, positioningVelocity);
745 vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
746 pthread_mutex_unlock(&vpEndThread_mutex);
747 return;
748}
749
766void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
767{
768 try {
769 vpColVector q(2);
770 q[0] = q1;
771 q[1] = q2;
772
773 setPosition(frame, q);
774 } catch (...) {
775 vpERROR_TRACE("Error caught");
776 throw;
777 }
778}
779
793void vpRobotBiclops::setPosition(const char *filename)
794{
795 vpColVector q;
796 if (readPositionFile(filename, q) == false) {
797 vpERROR_TRACE("Cannot get biclops position from file");
798 throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file");
799 }
801}
802
819{
820 switch (frame) {
822 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
823 "not implemented");
824 break;
826 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
827 "not implemented");
828 break;
830 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
831 "not implemented");
832 break;
834 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
835 "not implemented");
836 break;
838 break;
839 }
840
842 state = vpRobot::getRobotState();
843
844 switch (state) {
845 case STATE_STOP:
847 q = controller.getPosition();
848
849 break;
852 default:
854
855 vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
856 pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
857
858 vpRobotBiclopsController::shmType shm;
859
860 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
861 pthread_mutex_lock(&vpShm_mutex);
862
863 shm = controller.readShm();
864
865 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
866 pthread_mutex_unlock(&vpShm_mutex);
867
868 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
869 q[i] = shm.actual_q[i];
870 }
871
872 vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
873
874 vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
875 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
876
877 break;
878 }
879}
880
908{
910 vpERROR_TRACE("Cannot send a velocity to the robot "
911 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
913 "Cannot send a velocity to the robot "
914 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
915 }
916
917 switch (frame) {
919 vpERROR_TRACE("Cannot send a velocity to the robot "
920 "in the camera frame: "
921 "functionality not implemented");
922 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
923 "in the camera frame:"
924 "functionality not implemented");
925 }
927 if (q_dot.getRows() != 2) {
928 vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
929 throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
930 "in articular frame");
931 }
932 break;
933 }
935 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
936 "in the reference frame:"
937 "functionality not implemented");
938 }
939 case vpRobot::MIXT_FRAME: {
940 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
941 "in the mixt frame:"
942 "functionality not implemented");
943 }
945 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
946 "in the end-effector frame:"
947 "functionality not implemented");
948 }
949 default: {
950 throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
951 }
952 }
953
954 vpDEBUG_TRACE(12, "Velocity limitation.");
955 bool norm = false; // Flag to indicate when velocities need to be nomalized
956
957 // Saturate articular speed
958 double max = vpBiclops::speedLimit;
959 vpColVector q_dot_sat(vpBiclops::ndof);
960
961 // init q_dot_saturated
962 q_dot_sat = q_dot;
963
964 for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
965 {
966 if (fabs(q_dot[i]) > max) {
967 norm = true;
968 max = fabs(q_dot[i]);
969 vpERROR_TRACE("Excess velocity: ROTATION "
970 "(axe nr.%d).",
971 i);
972 }
973 }
974 // Rotations velocities normalisation
975 if (norm == true) {
976 max = vpBiclops::speedLimit / max;
977 q_dot_sat = q_dot * max;
978 }
979
980 vpCDEBUG(12) << "send velocity: " << q_dot_sat.t() << std::endl;
981
982 vpRobotBiclopsController::shmType shm;
983
984 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
985 pthread_mutex_lock(&vpShm_mutex);
986
987 shm = controller.readShm();
988
989 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
990 shm.q_dot[i] = q_dot[i];
991
992 controller.writeShm(shm);
993
994 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
995 pthread_mutex_unlock(&vpShm_mutex);
996
997 return;
998}
999
1000/* -------------------------------------------------------------------------
1001 */
1002/* --- GET -----------------------------------------------------------------
1003 */
1004/* -------------------------------------------------------------------------
1005 */
1006
1019{
1020 switch (frame) {
1022 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
1023 "not implemented");
1024 break;
1026 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
1027 "not implemented");
1028 break;
1030 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
1031 "not implemented");
1032 break;
1034 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in end-effector frame: "
1035 "not implemented");
1036 break;
1038 break;
1039 }
1040
1042 state = vpRobot::getRobotState();
1043
1044 switch (state) {
1045 case STATE_STOP:
1047 q_dot = controller.getVelocity();
1048
1049 break;
1052 default:
1053 q_dot.resize(vpBiclops::ndof);
1054
1055 vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
1056 pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
1057
1058 vpRobotBiclopsController::shmType shm;
1059
1060 vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
1061 pthread_mutex_lock(&vpShm_mutex);
1062
1063 shm = controller.readShm();
1064
1065 vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
1066 pthread_mutex_unlock(&vpShm_mutex);
1067
1068 for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
1069 q_dot[i] = shm.actual_q_dot[i];
1070 }
1071
1072 vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
1073
1074 vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
1075 pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
1076
1077 break;
1078 }
1079}
1080
1093{
1094 vpColVector q_dot;
1095 getVelocity(frame, q_dot);
1096
1097 return q_dot;
1098}
1099
1119bool vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
1120{
1121 std::ifstream fd(filename.c_str(), std::ios::in);
1122
1123 if (!fd.is_open()) {
1124 return false;
1125 }
1126
1127 std::string line;
1128 std::string key("R:");
1129 std::string id("#PTU-EVI - Position");
1130 bool pos_found = false;
1131 int lineNum = 0;
1132
1134
1135 while (std::getline(fd, line)) {
1136 lineNum++;
1137 if (lineNum == 1) {
1138 if (!(line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
1139 std::cout << "Error: this position file " << filename << " is not for Biclops robot" << std::endl;
1140 return false;
1141 }
1142 }
1143 if ((line.compare(0, 1, "#") == 0)) { // skip comment
1144 continue;
1145 }
1146 if ((line.compare(0, key.size(), key) == 0)) { // decode position
1147 // check if there are at least njoint values in the line
1148 std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
1149 if (chain.size() < vpBiclops::ndof + 1) // try to split with tab separator
1150 chain = vpIoTools::splitChain(line, std::string("\t"));
1151 if (chain.size() < vpBiclops::ndof + 1)
1152 continue;
1153
1154 std::istringstream ss(line);
1155 std::string key_;
1156 ss >> key_;
1157 for (unsigned int i = 0; i < vpBiclops::ndof; i++)
1158 ss >> q[i];
1159 pos_found = true;
1160 break;
1161 }
1162 }
1163
1164 // converts rotations from degrees into radians
1165 q.deg2rad();
1166
1167 fd.close();
1168
1169 if (!pos_found) {
1170 std::cout << "Error: unable to find a position for Biclops robot in " << filename << std::endl;
1171 return false;
1172 }
1173
1174 return true;
1175}
1176
1200{
1201 vpColVector q_current; // current position
1202
1204
1205 switch (frame) {
1207 d.resize(vpBiclops::ndof);
1208 d = q_current - q_previous;
1209 break;
1210
1211 case vpRobot::CAMERA_FRAME: {
1212 d.resize(6);
1213 vpHomogeneousMatrix fMc_current;
1214 vpHomogeneousMatrix fMc_previous;
1215 fMc_current = vpBiclops::get_fMc(q_current);
1216 fMc_previous = vpBiclops::get_fMc(q_previous);
1217 vpHomogeneousMatrix c_previousMc_current;
1218 // fMc_c = fMc_p * c_pMc_c
1219 // => c_pMc_c = (fMc_p)^-1 * fMc_c
1220 c_previousMc_current = fMc_previous.inverse() * fMc_current;
1221
1222 // Compute the instantaneous velocity from this homogeneous matrix.
1223 d = vpExponentialMap::inverse(c_previousMc_current);
1224 break;
1225 }
1226
1228 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
1229 "functionality not implemented");
1230 break;
1232 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
1233 "functionality not implemented");
1234 break;
1236 throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the end-effector frame:"
1237 "functionality not implemented");
1238 break;
1239 }
1240
1241 q_previous = q_current; // Update for next call of this method
1242}
1243
1244#elif !defined(VISP_BUILD_SHARED_LIBS)
1245// Work around to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no
1246// symbols
1247void dummy_vpRobotBiclops(){};
1248#endif
unsigned int getRows() const
Definition vpArray2D.h:290
Jacobian, geometric model functionalities... for biclops, pan, tilt head.
Definition vpBiclops.h:75
static const float speedLimit
Definition vpBiclops.h:130
static const float tiltJointLimit
Definition vpBiclops.h:129
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
Definition vpBiclops.cpp:92
vpHomogeneousMatrix get_cMe() const
Definition vpBiclops.h:154
static const float panJointLimit
Definition vpBiclops.h:128
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static const unsigned int ndof
Definition vpBiclops.h:123
Implementation of column vector and the associated operations.
vpColVector & deg2rad()
vpRowVector t() const
void resize(unsigned int i, bool flagNullify=true)
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)
static double rad(double deg)
Definition vpMath.h:116
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
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 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
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ MIXT_FRAME
Definition vpRobot.h:84
@ CAMERA_FRAME
Definition vpRobot.h:80
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
Definition vpRobot.h:66
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
#define vpCDEBUG(level)
Definition vpDebug.h:506
#define vpCERROR
Definition vpDebug.h:360
#define vpDEBUG_TRACE
Definition vpDebug.h:482
#define vpERROR_TRACE
Definition vpDebug.h:388
VISP_EXPORT int wait(double t0, double t)