42 #include <visp3/core/vpConfig.h>
44 #ifdef VISP_HAVE_BICLOPS
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>
55 #include <visp3/core/vpDebug.h>
61 bool vpRobotBiclops::robotAlreadyCreated =
false;
64 static pthread_mutex_t vpEndThread_mutex;
65 static pthread_mutex_t vpShm_mutex;
66 static pthread_mutex_t vpMeasure_mutex;
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);
185 vpDEBUG_TRACE(12,
"Start vpRobotBiclops::~vpRobotBiclops()");
189 pthread_mutex_unlock(&vpEndThread_mutex);
194 if (controlThreadCreated ==
true) {
195 int code = pthread_join(control_thread, NULL);
197 vpCERROR <<
"Cannot terminate the control thread: " << code <<
" strErr=" << strerror(errno)
198 <<
" strCode=" << strerror(code) << std::endl;
202 pthread_mutex_destroy(&vpShm_mutex);
203 pthread_mutex_destroy(&vpEndThread_mutex);
204 pthread_mutex_destroy(&vpMeasure_mutex);
206 vpRobotBiclops::robotAlreadyCreated =
false;
238 FILE *fd = fopen(configfile.c_str(),
"r");
240 vpCERROR <<
"Cannot open biclops config file: " << configfile << std::endl;
246 controller.init(configfile);
255 vpRobotBiclops::robotAlreadyCreated =
true;
261 controlThreadCreated =
false;
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);
562 pthread_mutex_lock(&vpEndThread_mutex);
568 vpCERROR <<
"Cannot create speed biclops control thread: " << code <<
" strErr=" << strerror(errno)
569 <<
" strCode=" << strerror(code) << std::endl;
572 controlThreadCreated =
true;
594 controller.setVelocity(q_dot);
597 controller.stopRequest(
true);
681 if (velocity < 0 || velocity > 100) {
686 positioningVelocity = velocity;
717 "Modification of the robot state");
743 pthread_mutex_lock(&vpEndThread_mutex);
744 controller.setPosition(q, positioningVelocity);
746 pthread_mutex_unlock(&vpEndThread_mutex);
847 q = controller.getPosition();
856 pthread_mutex_lock(&vpMeasure_mutex);
858 vpRobotBiclopsController::shmType shm;
861 pthread_mutex_lock(&vpShm_mutex);
863 shm = controller.readShm();
866 pthread_mutex_unlock(&vpShm_mutex);
869 q[i] = shm.actual_q[i];
872 vpCDEBUG(11) <<
"++++++++ Measure actuals: " << q.
t();
875 pthread_mutex_unlock(&vpMeasure_mutex);
911 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
913 "Cannot send a velocity to the robot "
914 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
920 "in the camera frame: "
921 "functionality not implemented");
923 "in the camera frame:"
924 "functionality not implemented");
928 vpERROR_TRACE(
"Bad dimension fo speed vector in articular frame");
930 "in articular frame");
936 "in the reference frame:"
937 "functionality not implemented");
942 "functionality not implemented");
946 "in the end-effector frame:"
947 "functionality not implemented");
966 if (fabs(q_dot[i]) > max) {
968 max = fabs(q_dot[i]);
977 q_dot_sat = q_dot * max;
980 vpCDEBUG(12) <<
"send velocity: " << q_dot_sat.
t() << std::endl;
982 vpRobotBiclopsController::shmType shm;
985 pthread_mutex_lock(&vpShm_mutex);
987 shm = controller.readShm();
990 shm.q_dot[i] = q_dot[i];
992 controller.writeShm(shm);
995 pthread_mutex_unlock(&vpShm_mutex);
1047 q_dot = controller.getVelocity();
1056 pthread_mutex_lock(&vpMeasure_mutex);
1058 vpRobotBiclopsController::shmType shm;
1061 pthread_mutex_lock(&vpShm_mutex);
1063 shm = controller.readShm();
1066 pthread_mutex_unlock(&vpShm_mutex);
1069 q_dot[i] = shm.actual_q_dot[i];
1072 vpCDEBUG(11) <<
"++++++++ Velocity actuals: " << q_dot.
t();
1075 pthread_mutex_unlock(&vpMeasure_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;
1208 d = q_current - q_previous;
1220 c_previousMc_current = fMc_previous.
inverse() * fMc_current;
1229 "functionality not implemented");
1233 "functionality not implemented");
1237 "functionality not implemented");
1241 q_previous = q_current;
1244 #elif !defined(VISP_BUILD_SHARED_LIBS)
1247 void dummy_vpRobotBiclops(){};
unsigned int getRows() const
Jacobian, geometric model functionalities... for biclops, pan, tilt head.
static const float speedLimit
static const float tiltJointLimit
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
vpHomogeneousMatrix get_cMe() const
static const float panJointLimit
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static const unsigned int ndof
Implementation of column vector and the associated operations.
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 double rad(double deg)
static Type maximum(const Type &a, const Type &b)
static double deg(double rad)
Implementation of a matrix and operations on matrices.
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.
virtual vpRobotStateType getRobotState(void) const
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_ACCELERATION_CONTROL
Initialize the acceleration controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
VISP_EXPORT int wait(double t0, double t)