Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpRobotBiclopsController.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <signal.h>
40 #include <string.h>
41 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
42 #include <unistd.h>
43 #endif
44 #include <visp3/core/vpConfig.h>
45 
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 #ifdef VISP_HAVE_BICLOPS
48 
49 /* Headers */
50 #include <visp3/robot/vpRobotBiclops.h>
51 #include <visp3/robot/vpRobotBiclopsController.h>
52 #include <visp3/robot/vpRobotException.h>
53 
54 //#define VP_DEBUG // Activate the debug mode
55 //#define VP_DEBUG_MODE 20 // Activate debug level 1 and 2
56 
57 #include <visp3/core/vpDebug.h>
58 
59 /* ----------------------------------------------------------------------- */
60 /* --- CONSTRUCTOR ------------------------------------------------------ */
61 /* ---------------------------------------------------------------------- */
62 
66 vpRobotBiclopsController::vpRobotBiclopsController()
67  : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), panProfile(), tiltProfile(), vergeProfile(),
68  shm(), stopControllerThread_(false)
69 {
70  axisMask = Biclops::PanMask + Biclops::TiltMask
71  /*+ Biclops::VergeMask*/; // add this if you want verge.
72 
73  // Set Debug level depending on how much info you want to see about
74  // the inner workings of the API. Level 2 is highest with 0 being
75  // the default (i.e., no messages).
76  biclops.SetDebugLevel(0);
77 
78  // initialize the shared data structure
79  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
80  shm.status[i] = STOP;
81  shm.q_dot[i] = 0.;
82  shm.actual_q[i] = 0.;
83  shm.jointLimit[i] = false;
84  shm.status[i] = STOP;
85  }
86 }
87 
93 vpRobotBiclopsController::~vpRobotBiclopsController() {}
94 
108 void vpRobotBiclopsController::init(const std::string &configfile)
109 {
110  vpDEBUG_TRACE(12, "Initialize biclops.");
111  bool binit = false;
112  for (int i = 0; i < 1; i++) {
113  try {
114  std::cout << "Try to initialize biclops head " << std::endl;
115  binit = biclops.Initialize(configfile.c_str());
116  usleep(100000);
117  if (binit) {
118  // Initialization completed successfully. Close the config file.
119  std::cout << "Initialization succeed...\n";
120  break;
121  } else {
122  std::cout << "Initialization failed...\n";
123  }
124  } catch (...) {
125  std::cout << "Initialization failed..." << std::endl;
126  }
127  }
128 
129  if (!binit) {
130  std::cout << "Cannot initialize biclops head. " << std::endl;
131  std::cout << "Check if the serial cable is connected." << std::endl;
132  std::cout << "Check if the robot is powered on." << std::endl;
133  std::cout << "Check if you try to open the good serial port." << std::endl;
134  std::cout << "Try to power off/on and restart..." << std::endl;
135 
136  throw vpRobotException(vpRobotException::notInitializedError, "Cannot initialize biclops head.");
137  }
138 
139  vpDEBUG_TRACE(12, "Biclops initialized");
140 
141  // Get shortcut references to each axis.
142  panAxis = biclops.GetAxis(Biclops::Pan);
143  tiltAxis = biclops.GetAxis(Biclops::Tilt);
144  if ((axisMask & Biclops::VergeMask) != 0)
145  vergeAxis = biclops.GetAxis(Biclops::Verge);
146 
147 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
148  if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) {
149  vpDEBUG_TRACE(12, "Biclops is not homed");
150  }
151 #else // old API
152  if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) {
153  vpDEBUG_TRACE(12, "Biclops is not homed");
154  }
155 #endif
156 
157  // Execute the homing sequence for all axes.
158  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
159  vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
160  if (biclops.HomeAxes(axisMask))
161  vpDEBUG_TRACE(12, "Homing sequence succeed.");
162  else {
163  vpERROR_TRACE("Homing sequence failed. Program is stopped");
164  throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
165  }
166 
167  // Get the currently defined (default) motion profiles.
168  // PMDAxisControl::Profile panProfile,tiltProfile,vergeProfile;
169  panAxis->GetProfile(panProfile);
170  tiltAxis->GetProfile(tiltProfile);
171  if ((axisMask & Biclops::VergeMask) != 0)
172  vergeAxis->GetProfile(vergeProfile);
173 }
174 
190 void vpRobotBiclopsController::setPosition(const vpColVector &q, double percentVelocity)
191 {
192  if (q.getRows() != vpBiclops::ndof) {
193  vpERROR_TRACE("Bad dimension for positioning vector.");
194  throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for positioning vector.");
195  }
196 
197  panAxis->SetProfileMode(PMDTrapezoidalProfile);
198  tiltAxis->SetProfileMode(PMDTrapezoidalProfile);
199 
200  // Create the list of axes we want to coordinate
201  PMDUtils::AxisList axisList;
202  axisList.push_back(panAxis);
203  axisList.push_back(tiltAxis);
204 
205 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
206  // Get the currently defined (default) motion profiles.
207  // PMDAxisControl::Profile panProfile,tiltProfile;
208  panAxis->GetProfile(panProfile);
209  tiltAxis->GetProfile(tiltProfile);
210 
211  // Set a position to move to by modifying the respective profiles.
212  // NOTE: profile values are in revolutions, so here we convert
213  // from degrees (divide by 360) for readability.
214  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
215  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.);
216 
217  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
218  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.);
219 
220  // Inform the controller of the new desired position.
221  panAxis->SetProfile(panProfile);
222  tiltAxis->SetProfile(tiltProfile);
223 
224 #else // old API
225 
226  PMDAxisControl::CountsProfile desired_profile;
227 
228  // Set a position to move to by modifying the respective profiles.
229  // NOTE: profile values are in revolutions, so here we convert
230  // from degrees (divide by 360) for readability.
231  panProfile.pos = PMDUtils::RadsToRevs(q[0]);
232  panProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.);
233 
234  vpDEBUG_TRACE(12, "Speed percent: %lf", vpBiclops::speedLimit * percentVelocity / 100.);
235 
236  panAxis->ProfileToCounts(panProfile, desired_profile);
237  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
238  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
239 
240  panAxis->SetProfile(desired_profile);
241 
242  // Set a position to move to by modifying the respective profiles.
243  // NOTE: profile values are in revolutions, so here we convert
244  // from degrees (divide by 360) for readability.
245  tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
246  tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.);
247 
248  tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
249  vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
250  vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
251 
252  tiltAxis->SetProfile(desired_profile);
253 #endif
254 
255  // Coordinate motion
256  PMDUtils::Coordinate(axisList);
257  biclops.Move(Biclops::PanMask + Biclops::TiltMask /*, 0*/); //
258 }
259 
269 void vpRobotBiclopsController::setVelocity(const vpColVector &q_dot)
270 {
271  if (q_dot.getRows() != vpBiclops::ndof) {
272  vpERROR_TRACE("Bad dimension for velocity vector.");
273  throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for velocity vector.");
274  }
275 
276 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
277  // Get the currently defined (default) motion profiles.
278  // PMDAxisControl::Profile panProfile, tiltProfile;
279  panAxis->GetProfile(panProfile);
280  tiltAxis->GetProfile(tiltProfile);
281 
282  // Set a position to move to by modifying the respective profiles.
283  // NOTE: profile values are in revolutions, so here we convert
284  // from degrees (divide by 360) for readability.
285  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
286  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
287 
288  // Inform the controller of the new desired position.
289  panAxis->SetProfile(panProfile);
290  tiltAxis->SetProfile(tiltProfile);
291 
292  panAxis->SetProfileMode(PMDVelocityContouringProfile);
293  tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
294 #else // old API
295  panAxis->SetProfileMode(PMDVelocityContouringProfile);
296  tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
297 
298  PMDAxisControl::CountsProfile desired_profile;
299 
300  // Set a position to move to by modifying the respective profiles.
301  // NOTE: profile values are in revolutions, so here we convert
302  // from degrees (divide by 360) for readability.
303  panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
304 
305  panAxis->ProfileToCounts(panProfile, desired_profile);
306  panAxis->SetProfile(desired_profile);
307 
308  // Set a position to move to by modifying the respective profiles.
309  // NOTE: profile values are in revolutions, so here we convert
310  // from degrees (divide by 360) for readability.
311  tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
312 
313  tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
314  tiltAxis->SetProfile(desired_profile);
315 #endif
316  // Coordinate motion
317  biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
318 }
319 
327 vpColVector vpRobotBiclopsController::getPosition()
328 {
329  vpDEBUG_TRACE(12, "Start vpRobotBiclopsController::getPosition() ");
331  PMDint32 panpos, tiltpos;
332 
333  panAxis->GetPosition(panpos);
334  tiltAxis->GetPosition(tiltpos);
335 
336  q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
337  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
338 
339  vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
340  vpDEBUG_TRACE(12, "End vpRobotBiclopsController::getPosition()");
341 
342  return q;
343 }
344 
352 vpColVector vpRobotBiclopsController::getActualPosition()
353 {
355  PMDint32 panpos, tiltpos;
356 
357  panAxis->GetActualPosition(panpos);
358  tiltAxis->GetActualPosition(tiltpos);
359 
360  q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
361  q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
362 
363  return q;
364 }
365 
373 vpColVector vpRobotBiclopsController::getVelocity()
374 {
376  PMDint32 pan_vel, tilt_vel;
377 
378  panAxis->GetVelocity(pan_vel);
379  tiltAxis->GetVelocity(tilt_vel);
380 
381  q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
382  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
383 
384  return q_dot;
385 }
386 
394 vpColVector vpRobotBiclopsController::getActualVelocity()
395 {
397  PMDint32 pan_vel, tilt_vel;
398 
399  panAxis->GetActualVelocity(pan_vel);
400  tiltAxis->GetActualVelocity(tilt_vel);
401 
402  q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
403  q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
404 
405  return q_dot;
406 }
407 
414 void vpRobotBiclopsController::writeShm(shmType &shm_)
415 {
416  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
417  vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
418  }
419  memcpy(&this->shm, &shm_, sizeof(shmType));
420  // this->shm = shm_;
421  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
422  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
423  }
424 }
425 
432 vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm()
433 {
434  shmType tmp_shm;
435 
436  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
437  vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
438  }
439  memcpy(&tmp_shm, &this->shm, sizeof(shmType));
440  // tmp_shm = shm;
441  for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
442  vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
443  }
444 
445  return tmp_shm;
446 }
447 
448 #elif !defined(VISP_BUILD_SHARED_LIBS)
449 // Work arround to avoid warning:
450 // libvisp_robot.a(vpRobotBiclopsController.cpp.o) has no symbols
451 void dummy_vpRobotBiclopsController(){};
452 #endif
453 
454 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
Error that can be emited by the vpRobot class and its derivates.
static const unsigned int ndof
Definition: vpBiclops.h:126
static const float speedLimit
Definition: vpBiclops.h:133
#define vpERROR_TRACE
Definition: vpDebug.h:393
unsigned int getRows() const
Definition: vpArray2D.h:289
#define vpCDEBUG(level)
Definition: vpDebug.h:511
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
#define vpDEBUG_TRACE
Definition: vpDebug.h:487