34 #include <visp3/core/vpConfig.h>
36 #ifdef VISP_HAVE_BICLOPS
45 #include <visp3/core/vpExponentialMap.h>
46 #include <visp3/core/vpIoTools.h>
47 #include <visp3/core/vpTime.h>
48 #include <visp3/robot/vpBiclops.h>
49 #include <visp3/robot/vpRobotBiclops.h>
50 #include <visp3/robot/vpRobotException.h>
52 #include "private/vpRobotBiclopsController_impl.h"
56 #include <visp3/core/vpDebug.h>
64 static std::mutex m_mutex_end_thread;
65 static std::mutex m_mutex_shm;
66 static std::mutex m_mutex_measure;
73 :
vpBiclops(),
vpRobot(), m_control_thread(), m_positioningVelocity(defaultPositioningVelocity),
78 m_controller =
new vpRobotBiclopsController;
83 :
vpBiclops(),
vpRobot(), m_control_thread(), m_positioningVelocity(defaultPositioningVelocity),
88 m_controller =
new vpRobotBiclopsController;
100 m_mutex_end_thread.unlock();
105 if (m_control_thread.joinable()) {
106 m_control_thread.join();
118 FILE *fd = fopen(m_configfile.c_str(),
"r");
120 vpCERROR <<
"Cannot open Biclops config file: " << m_configfile << std::endl;
126 m_controller->init(m_configfile);
145 vpRobotBiclopsController *controller =
static_cast<vpRobotBiclopsController *
>(arg);
149 vpRobotBiclopsController::shmType shm;
172 shm = controller->readShm();
175 m_mutex_shm.unlock();
178 prev_q_dot[i] = shm.q_dot[i];
179 new_q_dot[i] =
false;
180 change_dir[i] =
false;
181 force_halt[i] =
false;
182 enable_limit[i] =
true;
186 mes_q = controller->getActualPosition();
187 mes_q_dot = controller->getActualVelocity();
192 shm = controller->readShm();
195 shm.actual_q[i] = mes_q[i];
196 shm.actual_q_dot[i] = mes_q_dot[i];
199 controller->writeShm(shm);
202 m_mutex_shm.unlock();
205 m_mutex_measure.unlock();
207 while (!controller->isStopRequested()) {
210 mes_q = controller->getActualPosition();
211 mes_q_dot = controller->getActualVelocity();
216 shm = controller->readShm();
220 shm.actual_q[i] = mes_q[i];
221 shm.actual_q_dot[i] = mes_q_dot[i];
231 if (mes_q[i] < -softLimit[i]) {
233 shm.status[i] = vpRobotBiclopsController::STOP;
234 shm.jointLimit[i] =
true;
236 else if (mes_q[i] > softLimit[i]) {
238 shm.status[i] = vpRobotBiclopsController::STOP;
239 shm.jointLimit[i] =
true;
242 shm.status[i] = vpRobotBiclopsController::SPEED;
243 shm.jointLimit[i] =
false;
248 if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
249 std::fabs(
vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
252 new_q_dot[i] =
false;
255 if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
256 change_dir[i] =
true;
258 change_dir[i] =
false;
260 vpDEBUG_TRACE(13,
"status : %d %d", shm.status[0], shm.status[1]);
261 vpDEBUG_TRACE(13,
"joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
262 vpDEBUG_TRACE(13,
"new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
263 vpDEBUG_TRACE(13,
"new dir : %d %d", change_dir[0], change_dir[1]);
264 vpDEBUG_TRACE(13,
"force halt : %d %d", force_halt[0], force_halt[1]);
265 vpDEBUG_TRACE(13,
"enable limit: %d %d", enable_limit[0], enable_limit[1]);
267 bool updateVelocity =
false;
272 if (shm.status[i] == vpRobotBiclopsController::STOP) {
274 if (change_dir[i] ==
false) {
277 if (enable_limit[i] ==
true) {
280 if (force_halt[i] ==
false) {
282 force_halt[i] =
true;
283 updateVelocity =
true;
289 q_dot[i] = shm.q_dot[i];
290 shm.status[i] = vpRobotBiclopsController::SPEED;
291 force_halt[i] =
false;
292 updateVelocity =
true;
297 if (enable_limit[i] ==
true) {
299 q_dot[i] = shm.q_dot[i];
300 shm.status[i] = vpRobotBiclopsController::SPEED;
301 force_halt[i] =
false;
302 enable_limit[i] =
false;
303 updateVelocity =
true;
308 if (force_halt[i] ==
false) {
310 force_halt[i] =
true;
311 enable_limit[i] =
true;
312 updateVelocity =
true;
321 q_dot[i] = shm.q_dot[i];
322 shm.status[i] = vpRobotBiclopsController::SPEED;
323 enable_limit[i] =
true;
324 updateVelocity =
true;
330 if (shm.status[i] == vpRobotBiclopsController::STOP) {
331 if (enable_limit[i] ==
true) {
334 if (force_halt[i] ==
false) {
337 force_halt[i] =
true;
338 updateVelocity =
true;
344 enable_limit[i] =
true;
349 controller->writeShm(shm);
352 m_mutex_shm.unlock();
354 if (updateVelocity) {
358 controller->setVelocity(q_dot);
363 prev_q_dot[i] = shm.q_dot[i];
368 controller->stopRequest(
false);
370 vpDEBUG_TRACE(10,
"End of the control thread: stop the robot");
372 controller->setVelocity(q_dot);
377 delete[] enable_limit;
379 m_mutex_end_thread.unlock();
403 m_mutex_end_thread.lock();
424 m_controller->setVelocity(q_dot);
426 m_controller->stopRequest(
true);
457 if (velocity < 0 || velocity > 100) {
462 m_positioningVelocity = velocity;
472 "Modification of the robot state");
498 m_mutex_end_thread.lock();
499 m_controller->setPosition(q, m_positioningVelocity);
501 m_mutex_end_thread.unlock();
559 q = m_controller->getPosition();
568 m_mutex_measure.lock();
570 vpRobotBiclopsController::shmType shm;
575 shm = m_controller->readShm();
578 m_mutex_shm.unlock();
581 q[i] = shm.actual_q[i];
584 vpCDEBUG(11) <<
"++++++++ Measure actuals: " << q.
t();
587 m_mutex_measure.unlock();
597 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
599 "Cannot send a velocity to the robot "
600 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
606 "in the camera frame:"
607 "functionality not implemented");
618 "in the reference frame:"
619 "functionality not implemented");
624 "functionality not implemented");
628 "in the end-effector frame:"
629 "functionality not implemented");
648 if (fabs(q_dot[i]) > max) {
650 max = fabs(q_dot[i]);
659 q_dot_sat = q_dot * max;
662 vpCDEBUG(12) <<
"send velocity: " << q_dot_sat.
t() << std::endl;
664 vpRobotBiclopsController::shmType shm;
669 shm = m_controller->readShm();
672 shm.q_dot[i] = q_dot[i];
674 m_controller->writeShm(shm);
677 m_mutex_shm.unlock();
711 q_dot = m_controller->getVelocity();
720 m_mutex_measure.lock();
722 vpRobotBiclopsController::shmType shm;
727 shm = m_controller->readShm();
730 m_mutex_shm.unlock();
733 q_dot[i] = shm.actual_q_dot[i];
736 vpCDEBUG(11) <<
"++++++++ Velocity actuals: " << q_dot.
t();
739 m_mutex_measure.unlock();
755 std::ifstream fd(filename.c_str(), std::ios::in);
762 std::string key(
"R:");
763 std::string id(
"#PTU-EVI - Position");
764 bool pos_found =
false;
769 while (std::getline(fd, line)) {
772 if (!(line.compare(0,
id.size(),
id) == 0)) {
773 std::cout <<
"Error: this position file " << filename <<
" is not for Biclops robot" << std::endl;
777 if ((line.compare(0, 1,
"#") == 0)) {
780 if ((line.compare(0, key.size(), key) == 0)) {
788 std::istringstream ss(line);
804 std::cout <<
"Error: unable to find a position for Biclops robot in " << filename << std::endl;
820 d = q_current - m_q_previous;
832 c_previousMc_current = fMc_previous.
inverse() * fMc_current;
841 "functionality not implemented");
845 "functionality not implemented");
849 "functionality not implemented");
853 m_q_previous = q_current;
856 #elif !defined(VISP_BUILD_SHARED_LIBS)
859 void dummy_vpRobotBiclops() { };
unsigned int getRows() const
Jacobian, geometric model functionalities... for Biclops, pan, tilt head.
static const float speedLimit
Pan and tilt axis max velocity in rad/s to perform a displacement.
static const float tiltJointLimit
Tilt axis +/- joint limit in rad.
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
Pan axis +/- joint limit in rad.
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
static const unsigned int ndof
Number of dof.
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()
void get_eJe(vpMatrix &eJe) override
double getPositioningVelocity(void)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot)
static const double defaultPositioningVelocity
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q) override
bool readPositionFile(const std::string &filename, vpColVector &q)
vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState) override
void get_cVe(vpVelocityTwistMatrix &cVe) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &q_dot) override
void get_fJe(vpMatrix &fJe) override
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d) override
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q) override
void setConfigFile(const std::string &filename="/usr/share/BiclopsDefault.cfg")
static void vpRobotBiclopsSpeedControlLoop(void *arg)
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.
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
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.
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
VISP_EXPORT int wait(double t0, double t)