Visual Servoing Platform version 3.5.0
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
66vpRobotBiclopsController::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
93vpRobotBiclopsController::~vpRobotBiclopsController() {}
94
108void 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
190void 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
269void 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
327vpColVector 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
352vpColVector 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
373vpColVector 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
394vpColVector 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
414void 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
432vpRobotBiclopsController::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
451void dummy_vpRobotBiclopsController(){};
452#endif
453
454#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
unsigned int getRows() const
Definition: vpArray2D.h:289
static const float speedLimit
Definition: vpBiclops.h:133
static const unsigned int ndof
Definition: vpBiclops.h:126
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
Error that can be emited by the vpRobot class and its derivates.
#define vpCDEBUG(level)
Definition: vpDebug.h:511
#define vpDEBUG_TRACE
Definition: vpDebug.h:487
#define vpERROR_TRACE
Definition: vpDebug.h:393