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