110 :
vpBiclops(),
vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
111 q_previous(), controlThreadCreated(false)
115 vpRobotBiclops::robotAlreadyCreated =
false;
119 pthread_mutex_init(&vpShm_mutex, NULL);
120 pthread_mutex_init(&vpEndThread_mutex, NULL);
121 pthread_mutex_init(&vpMeasure_mutex, NULL);
157 :
vpBiclops(),
vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
158 q_previous(), controlThreadCreated(false)
162 vpRobotBiclops::robotAlreadyCreated =
false;
166 pthread_mutex_init(&vpShm_mutex, NULL);
167 pthread_mutex_init(&vpEndThread_mutex, NULL);
168 pthread_mutex_init(&vpMeasure_mutex, NULL);
286 vpRobotBiclopsController *controller =
static_cast<vpRobotBiclopsController *
>(arg);
291 vpRobotBiclopsController::shmType shm;
312 pthread_mutex_lock(&vpShm_mutex);
314 shm = controller->readShm();
317 pthread_mutex_unlock(&vpShm_mutex);
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;
328 mes_q = controller->getActualPosition();
329 mes_q_dot = controller->getActualVelocity();
332 pthread_mutex_lock(&vpShm_mutex);
334 shm = controller->readShm();
337 shm.actual_q[i] = mes_q[i];
338 shm.actual_q_dot[i] = mes_q_dot[i];
341 controller->writeShm(shm);
344 pthread_mutex_unlock(&vpShm_mutex);
347 pthread_mutex_unlock(&vpMeasure_mutex);
349 while (!controller->isStopRequested()) {
352 mes_q = controller->getActualPosition();
353 mes_q_dot = controller->getActualVelocity();
356 pthread_mutex_lock(&vpShm_mutex);
358 shm = controller->readShm();
362 shm.actual_q[i] = mes_q[i];
363 shm.actual_q_dot[i] = mes_q_dot[i];
373 if (mes_q[i] < -softLimit[i]) {
375 shm.status[i] = vpRobotBiclopsController::STOP;
376 shm.jointLimit[i] =
true;
377 }
else if (mes_q[i] > softLimit[i]) {
379 shm.status[i] = vpRobotBiclopsController::STOP;
380 shm.jointLimit[i] =
true;
382 shm.status[i] = vpRobotBiclopsController::SPEED;
383 shm.jointLimit[i] =
false;
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())
392 new_q_dot[i] =
false;
395 if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
396 change_dir[i] =
true;
398 change_dir[i] =
false;
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]);
407 bool updateVelocity =
false;
412 if (shm.status[i] == vpRobotBiclopsController::STOP) {
414 if (change_dir[i] ==
false) {
417 if (enable_limit[i] ==
true) {
420 if (force_halt[i] ==
false) {
422 force_halt[i] =
true;
423 updateVelocity =
true;
428 q_dot[i] = shm.q_dot[i];
429 shm.status[i] = vpRobotBiclopsController::SPEED;
430 force_halt[i] =
false;
431 updateVelocity =
true;
435 if (enable_limit[i] ==
true) {
437 q_dot[i] = shm.q_dot[i];
438 shm.status[i] = vpRobotBiclopsController::SPEED;
439 force_halt[i] =
false;
440 enable_limit[i] =
false;
441 updateVelocity =
true;
445 if (force_halt[i] ==
false) {
447 force_halt[i] =
true;
448 enable_limit[i] =
true;
449 updateVelocity =
true;
457 q_dot[i] = shm.q_dot[i];
458 shm.status[i] = vpRobotBiclopsController::SPEED;
459 enable_limit[i] =
true;
460 updateVelocity =
true;
465 if (shm.status[i] == vpRobotBiclopsController::STOP) {
466 if (enable_limit[i] ==
true) {
469 if (force_halt[i] ==
false) {
472 force_halt[i] =
true;
473 updateVelocity =
true;
478 enable_limit[i] =
true;
483 controller->writeShm(shm);
486 pthread_mutex_unlock(&vpShm_mutex);
488 if (updateVelocity) {
492 controller->setVelocity(q_dot);
497 prev_q_dot[i] = shm.q_dot[i];
516 controller->stopRequest(
false);
518 vpDEBUG_TRACE(10,
"End of the control thread: stop the robot");
520 controller->setVelocity(q_dot);
525 delete[] enable_limit;
527 pthread_mutex_unlock(&vpEndThread_mutex);
1121 std::ifstream fd(filename.c_str(), std::ios::in);
1123 if (!fd.is_open()) {
1128 std::string key(
"R:");
1129 std::string id(
"#PTU-EVI - Position");
1130 bool pos_found =
false;
1135 while (std::getline(fd, line)) {
1138 if (!(line.compare(0,
id.size(),
id) == 0)) {
1139 std::cout <<
"Error: this position file " << filename <<
" is not for Biclops robot" << std::endl;
1143 if ((line.compare(0, 1,
"#") == 0)) {
1146 if ((line.compare(0, key.size(), key) == 0)) {
1154 std::istringstream ss(line);
1170 std::cout <<
"Error: unable to find a position for Biclops robot in " << filename << std::endl;