38#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
41#include <visp3/core/vpConfig.h>
43#ifndef DOXYGEN_SHOULD_SKIP_THIS
44#ifdef VISP_HAVE_BICLOPS
47#include <visp3/robot/vpRobotBiclops.h>
48#include <visp3/robot/vpRobotBiclopsController.h>
49#include <visp3/robot/vpRobotException.h>
54#include <visp3/core/vpDebug.h>
63vpRobotBiclopsController::vpRobotBiclopsController()
64 : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), panProfile(), tiltProfile(), vergeProfile(),
65 shm(), stopControllerThread_(false)
67 axisMask = Biclops::PanMask + Biclops::TiltMask
73 biclops.SetDebugLevel(0);
80 shm.jointLimit[i] =
false;
90vpRobotBiclopsController::~vpRobotBiclopsController() {}
105void vpRobotBiclopsController::init(
const std::string &configfile)
109 for (
int i = 0; i < 1; i++) {
111 std::cout <<
"Try to initialize biclops head " << std::endl;
112 binit = biclops.Initialize(configfile.c_str());
116 std::cout <<
"Initialization succeed...\n";
119 std::cout <<
"Initialization failed...\n";
122 std::cout <<
"Initialization failed..." << std::endl;
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;
139 panAxis = biclops.GetAxis(Biclops::Pan);
140 tiltAxis = biclops.GetAxis(Biclops::Tilt);
141 if ((axisMask & Biclops::VergeMask) != 0)
142 vergeAxis = biclops.GetAxis(Biclops::Verge);
144#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION
145 if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) {
149 if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) {
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))
166 panAxis->GetProfile(panProfile);
167 tiltAxis->GetProfile(tiltProfile);
168 if ((axisMask & Biclops::VergeMask) != 0)
169 vergeAxis->GetProfile(vergeProfile);
187void vpRobotBiclopsController::setPosition(
const vpColVector &q,
double percentVelocity)
194 panAxis->SetProfileMode(PMDTrapezoidalProfile);
195 tiltAxis->SetProfileMode(PMDTrapezoidalProfile);
198 PMDUtils::AxisList axisList;
199 axisList.push_back(panAxis);
200 axisList.push_back(tiltAxis);
202#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION
205 panAxis->GetProfile(panProfile);
206 tiltAxis->GetProfile(tiltProfile);
211 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
214 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
218 panAxis->SetProfile(panProfile);
219 tiltAxis->SetProfile(tiltProfile);
223 PMDAxisControl::CountsProfile desired_profile;
228 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
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;
237 panAxis->SetProfile(desired_profile);
242 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
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;
249 tiltAxis->SetProfile(desired_profile);
253 PMDUtils::Coordinate(axisList);
254 biclops.Move(Biclops::PanMask + Biclops::TiltMask );
266void vpRobotBiclopsController::setVelocity(
const vpColVector &q_dot)
273#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION
276 panAxis->GetProfile(panProfile);
277 tiltAxis->GetProfile(tiltProfile);
282 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
283 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
286 panAxis->SetProfile(panProfile);
287 tiltAxis->SetProfile(tiltProfile);
289 panAxis->SetProfileMode(PMDVelocityContouringProfile);
290 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
292 panAxis->SetProfileMode(PMDVelocityContouringProfile);
293 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
295 PMDAxisControl::CountsProfile desired_profile;
300 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
302 panAxis->ProfileToCounts(panProfile, desired_profile);
303 panAxis->SetProfile(desired_profile);
308 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
310 tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
311 tiltAxis->SetProfile(desired_profile);
314 biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0);
326 vpDEBUG_TRACE(12,
"Start vpRobotBiclopsController::getPosition() ");
328 PMDint32 panpos, tiltpos;
330 panAxis->GetPosition(panpos);
331 tiltAxis->GetPosition(tiltpos);
333 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
334 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
336 vpCDEBUG(11) <<
"++++++++ Mesure : " << q.
t();
337 vpDEBUG_TRACE(12,
"End vpRobotBiclopsController::getPosition()");
349vpColVector vpRobotBiclopsController::getActualPosition()
352 PMDint32 panpos, tiltpos;
354 panAxis->GetActualPosition(panpos);
355 tiltAxis->GetActualPosition(tiltpos);
357 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
358 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
373 PMDint32 pan_vel, tilt_vel;
375 panAxis->GetVelocity(pan_vel);
376 tiltAxis->GetVelocity(tilt_vel);
378 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
379 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
391vpColVector vpRobotBiclopsController::getActualVelocity()
394 PMDint32 pan_vel, tilt_vel;
396 panAxis->GetActualVelocity(pan_vel);
397 tiltAxis->GetActualVelocity(tilt_vel);
399 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
400 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
411void vpRobotBiclopsController::writeShm(shmType &shm_)
416 memcpy(&this->shm, &shm_,
sizeof(shmType));
429vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm()
436 memcpy(&tmp_shm, &this->shm,
sizeof(shmType));
439 vpDEBUG_TRACE(13,
"tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
445#elif !defined(VISP_BUILD_SHARED_LIBS)
448void dummy_vpRobotBiclopsController(){};
unsigned int getRows() const
static const float speedLimit
static const unsigned int ndof
Implementation of column vector and the associated operations.
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.