44 #include <visp3/core/vpTime.h>
46 #include <visp3/core/vpConfig.h>
48 #ifdef VISP_HAVE_BICLOPS
50 #include <visp3/robot/vpBiclops.h>
51 #include <visp3/robot/vpRobotBiclops.h>
52 #include <visp3/robot/vpRobotException.h>
53 #include <visp3/core/vpExponentialMap.h>
54 #include <visp3/core/vpIoTools.h>
58 #include <visp3/core/vpDebug.h>
64 bool vpRobotBiclops::robotAlreadyCreated =
false;
67 static pthread_mutex_t vpEndThread_mutex;
68 static pthread_mutex_t vpShm_mutex;
69 static pthread_mutex_t vpMeasure_mutex;
115 positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
119 vpRobotBiclops::robotAlreadyCreated =
false;
123 pthread_mutex_init (&vpShm_mutex, NULL);
124 pthread_mutex_init (&vpEndThread_mutex, NULL);
125 pthread_mutex_init (&vpMeasure_mutex, NULL);
165 positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
169 vpRobotBiclops::robotAlreadyCreated =
false;
173 pthread_mutex_init (&vpShm_mutex, NULL);
174 pthread_mutex_init (&vpEndThread_mutex, NULL);
175 pthread_mutex_init (&vpMeasure_mutex, NULL);
192 vpDEBUG_TRACE(12,
"Start vpRobotBiclops::~vpRobotBiclops()");
196 pthread_mutex_unlock(&vpEndThread_mutex);
201 if (controlThreadCreated ==
true) {
202 int code = pthread_join(control_thread, NULL);
204 vpCERROR <<
"Cannot terminate the control thread: " << code
205 <<
" strErr=" << strerror(errno)
206 <<
" strCode=" << strerror(code)
211 pthread_mutex_destroy (&vpShm_mutex);
212 pthread_mutex_destroy (&vpEndThread_mutex);
213 pthread_mutex_destroy (&vpMeasure_mutex);
215 vpRobotBiclops::robotAlreadyCreated =
false;
234 this->configfile = filename;
250 FILE *fd = fopen(configfile.c_str(),
"r");
252 vpCERROR <<
"Cannot open biclops config file: " << configfile << std::endl;
254 "Cannot open connection with biclops");
259 controller.init(configfile);
271 vpRobotBiclops::robotAlreadyCreated =
true;
277 controlThreadCreated =
false;
302 vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
307 vpRobotBiclopsController::shmType shm;
331 pthread_mutex_lock(&vpShm_mutex);
333 shm = controller->readShm();
336 pthread_mutex_unlock(&vpShm_mutex);
339 prev_q_dot [i] = shm.q_dot[i];
340 new_q_dot [i] =
false;
341 change_dir [i] =
false;
342 force_halt [i] =
false;
343 enable_limit[i] =
true;
347 mes_q = controller->getActualPosition();
348 mes_q_dot = controller->getActualVelocity();
351 pthread_mutex_lock(&vpShm_mutex);
353 shm = controller->readShm();
356 shm.actual_q[i] = mes_q[i];
357 shm.actual_q_dot[i] = mes_q_dot[i];
360 controller->writeShm(shm);
363 pthread_mutex_unlock(&vpShm_mutex);
366 pthread_mutex_unlock(&vpMeasure_mutex);
368 while (! controller->isStopRequested()) {
371 mes_q = controller->getActualPosition();
372 mes_q_dot = controller->getActualVelocity();
375 pthread_mutex_lock(&vpShm_mutex);
378 shm = controller->readShm();
382 shm.actual_q[i] = mes_q[i];
383 shm.actual_q_dot[i] = mes_q_dot[i];
401 if (mes_q[i] < -softLimit[i]) {
403 shm.status[i] = vpRobotBiclopsController::STOP;
404 shm.jointLimit[i] =
true;
406 else if (mes_q[i] > softLimit[i]) {
408 shm.status[i] = vpRobotBiclopsController::STOP;
409 shm.jointLimit[i] =
true;
412 shm.status[i] = vpRobotBiclopsController::SPEED;
413 shm.jointLimit[i] =
false;
418 if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(
vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
421 new_q_dot[i] =
false;
424 if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
425 change_dir[i] =
true;
427 change_dir[i] =
false;
430 vpDEBUG_TRACE(13,
"status : %d %d", shm.status[0], shm.status[1]);
431 vpDEBUG_TRACE(13,
"joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
432 vpDEBUG_TRACE(13,
"new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
433 vpDEBUG_TRACE(13,
"new dir : %d %d", change_dir[0], change_dir[1]);
434 vpDEBUG_TRACE(13,
"force halt : %d %d", force_halt[0], force_halt[1]);
435 vpDEBUG_TRACE(13,
"enable limit: %d %d", enable_limit[0], enable_limit[1]);
438 bool updateVelocity =
false;
443 if (shm.status[i] == vpRobotBiclopsController::STOP) {
445 if (change_dir[i] ==
false) {
448 if (enable_limit[i] ==
true) {
451 if (force_halt[i] ==
false) {
453 force_halt[i] =
true;
454 updateVelocity =
true;
460 q_dot[i] = shm.q_dot[i];
461 shm.status[i] = vpRobotBiclopsController::SPEED;
462 force_halt[i] =
false;
463 updateVelocity =
true;
468 if (enable_limit[i] ==
true) {
470 q_dot[i] = shm.q_dot[i];
471 shm.status[i] = vpRobotBiclopsController::SPEED;
472 force_halt[i] =
false;
473 enable_limit[i] =
false;
474 updateVelocity =
true;
479 if (force_halt[i] ==
false) {
481 force_halt[i] =
true;
482 enable_limit[i] =
true;
483 updateVelocity =
true;
492 q_dot[i] = shm.q_dot[i];
493 shm.status[i] = vpRobotBiclopsController::SPEED;
494 enable_limit[i] =
true;
495 updateVelocity =
true;
501 if (shm.status[i] == vpRobotBiclopsController::STOP) {
502 if (enable_limit[i] ==
true) {
505 if (force_halt[i] ==
false) {
508 force_halt[i] =
true;
509 updateVelocity =
true;
515 enable_limit[i] =
true;
520 controller->writeShm(shm);
523 pthread_mutex_unlock(&vpShm_mutex);
525 if (updateVelocity) {
537 prev_q_dot[i] = shm.q_dot[i];
556 controller->stopRequest(
false);
558 vpDEBUG_TRACE(10,
"End of the control thread: stop the robot");
563 delete [] change_dir;
564 delete [] force_halt;
565 delete [] enable_limit;
567 pthread_mutex_unlock(&vpEndThread_mutex);
611 pthread_mutex_lock(&vpEndThread_mutex);
615 code = pthread_create(&control_thread, NULL,
619 vpCERROR <<
"Cannot create speed biclops control thread: " << code
620 <<
" strErr=" << strerror(errno)
621 <<
" strCode=" << strerror(code)
625 controlThreadCreated =
true;
650 controller.setVelocity(q_dot);
652 controller.stopRequest(
true);
752 if (velocity < 0 || velocity > 100) {
755 "Bad positionning velocity");
758 positioningVelocity = velocity;
770 return positioningVelocity;
797 "Modification of the robot state");
807 "Cannot move the robot in camera frame: "
814 "Cannot move the robot in reference frame: "
821 "Cannot move the robot in mixt frame: "
837 pthread_mutex_lock(&vpEndThread_mutex);
838 controller.setPosition( q, positioningVelocity );
840 pthread_mutex_unlock(&vpEndThread_mutex);
861 const double &q1,
const double &q2)
897 "Cannot get biclops position from file");
924 vpERROR_TRACE (
"Cannot get position in camera frame: not implemented");
926 "Cannot get position in camera frame: "
933 "Cannot get position in reference frame: "
940 "Cannot get position in mixt frame: "
953 q = controller.getPosition();
962 pthread_mutex_lock(&vpMeasure_mutex);
964 vpRobotBiclopsController::shmType shm;
967 pthread_mutex_lock(&vpShm_mutex);
969 shm = controller.readShm();
972 pthread_mutex_unlock(&vpShm_mutex);
975 q[i] = shm.actual_q[i];
978 vpCDEBUG(11) <<
"++++++++ Measure actuals: " << q.
t();
981 pthread_mutex_unlock(&vpMeasure_mutex);
1022 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1024 "Cannot send a velocity to the robot "
1025 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1033 "in the camera frame: "
1034 "functionality not implemented");
1036 "Cannot send a velocity to the robot "
1037 "in the camera frame:"
1038 "functionality not implemented");
1043 vpERROR_TRACE (
"Bad dimension fo speed vector in articular frame");
1045 "Bad dimension for speed vector "
1046 "in articular frame");
1053 "in the reference frame: "
1054 "functionality not implemented");
1056 "Cannot send a velocity to the robot "
1057 "in the reference frame:"
1058 "functionality not implemented");
1063 "in the mixt frame: "
1064 "functionality not implemented");
1066 "Cannot send a velocity to the robot "
1067 "in the mixt frame:"
1068 "functionality not implemented");
1073 "Case not taken in account.");
1075 "Cannot send a velocity to the robot ");
1091 if (fabs (q_dot[i]) > max)
1094 max = fabs (q_dot[i]);
1102 q_dot_sat = q_dot * max;
1105 vpCDEBUG(12) <<
"send velocity: " << q_dot_sat.
t() << std::endl;
1107 vpRobotBiclopsController::shmType shm;
1110 pthread_mutex_lock(&vpShm_mutex);
1112 shm = controller.readShm();
1115 shm.q_dot[i] = q_dot[i];
1117 controller.writeShm(shm);
1120 pthread_mutex_unlock(&vpShm_mutex);
1149 vpERROR_TRACE (
"Cannot get position in camera frame: not implemented");
1151 "Cannot get position in camera frame: "
1158 "Cannot get position in reference frame: "
1165 "Cannot get position in mixt frame: "
1178 q_dot = controller.getVelocity();
1187 pthread_mutex_lock(&vpMeasure_mutex);
1189 vpRobotBiclopsController::shmType shm;
1192 pthread_mutex_lock(&vpShm_mutex);
1194 shm = controller.readShm();
1197 pthread_mutex_unlock(&vpShm_mutex);
1200 q_dot[i] = shm.actual_q_dot[i];
1203 vpCDEBUG(11) <<
"++++++++ Velocity actuals: " << q_dot.
t();
1206 pthread_mutex_unlock(&vpMeasure_mutex);
1256 std::ifstream fd(filename.c_str(), std::ios::in);
1258 if(! fd.is_open()) {
1263 std::string key(
"R:");
1264 std::string id(
"#PTU-EVI - Position");
1265 bool pos_found =
false;
1270 while(std::getline(fd, line)) {
1273 if(! (line.compare(0,
id.size(), id) == 0)) {
1274 std::cout <<
"Error: this position file " << filename <<
" is not for Biclops robot" << std::endl;
1278 if((line.compare(0, 1,
"#") == 0)) {
1281 if((line.compare(0, key.size(), key) == 0)) {
1289 std::istringstream ss(line);
1305 std::cout <<
"Error: unable to find a position for Biclops robot in " << filename << std::endl;
1325 vpRobotBiclops::getCameraDisplacement(
vpColVector &d)
1341 void vpRobotBiclops::getArticularDisplacement(
vpColVector &d)
1381 d = q_current - q_previous;
1393 c_previousMc_current = fMc_previous.
inverse() * fMc_current;
1401 vpERROR_TRACE (
"Cannot get a velocity in the reference frame: "
1402 "functionality not implemented");
1404 "Cannot get a velocity in the reference frame:"
1405 "functionality not implemented");
1409 "functionality not implemented");
1411 "Cannot get a velocity in the mixt frame:"
1412 "functionality not implemented");
1417 q_previous = q_current;
1421 #elif !defined(VISP_BUILD_SHARED_LIBS)
1423 void dummy_vpRobotBiclops() {};
Jacobian, geometric model functionnalities... for biclops, pan, tilt head.
void get_cVe(vpVelocityTwistMatrix &_cVe) const
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
bool readPositionFile(const std::string &filename, vpColVector &q)
VISP_EXPORT int wait(double t0, double t)
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
static const unsigned int ndof
void get_eJe(vpMatrix &_eJe)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static const float tiltJointLimit
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const float speedLimit
Initialize the position controller.
void setConfigFile(const std::string &filename="/usr/share/BiclopsDefault.cfg")
Class that defines a generic virtual robot.
void get_fJe(vpMatrix &_fJe)
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
static const double defaultPositioningVelocity
No copy constructor allowed.
virtual ~vpRobotBiclops(void)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
static Type maximum(const Type &a, const Type &b)
static const float panJointLimit
Initialize the velocity controller.
Initialize the acceleration controller.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a velocity twist matrix and operations on such kind of matrices.
unsigned int getRows() const
Return the number of rows of the 2D array.
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static double deg(double rad)
Implementation of column vector and the associated operations.
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d)
vpHomogeneousMatrix inverse() const
virtual vpRobotStateType getRobotState(void) const
double getPositioningVelocity(void)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
static void * vpRobotBiclopsSpeedControlLoop(void *arg)
vpHomogeneousMatrix get_cMe() const
void setPositioningVelocity(const double velocity)
void resize(const unsigned int i, const bool flagNullify=true)