41 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX 44 #include <visp3/core/vpConfig.h> 46 #ifndef DOXYGEN_SHOULD_SKIP_THIS 47 #ifdef VISP_HAVE_BICLOPS 50 #include <visp3/robot/vpRobotBiclops.h> 51 #include <visp3/robot/vpRobotBiclopsController.h> 52 #include <visp3/robot/vpRobotException.h> 57 #include <visp3/core/vpDebug.h> 66 vpRobotBiclopsController::vpRobotBiclopsController()
67 : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL), panProfile(), tiltProfile(), vergeProfile(),
68 shm(), stopControllerThread_(false)
70 axisMask = Biclops::PanMask + Biclops::TiltMask
76 biclops.SetDebugLevel(0);
83 shm.jointLimit[i] =
false;
93 vpRobotBiclopsController::~vpRobotBiclopsController() {}
108 void vpRobotBiclopsController::init(
const std::string &configfile)
112 for (
int i = 0; i < 1; i++) {
114 std::cout <<
"Try to initialize biclops head " << std::endl;
115 binit = biclops.Initialize(configfile.c_str());
119 std::cout <<
"Initialization succeed...\n";
122 std::cout <<
"Initialization failed...\n";
125 std::cout <<
"Initialization failed..." << std::endl;
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;
142 panAxis = biclops.GetAxis(Biclops::Pan);
143 tiltAxis = biclops.GetAxis(Biclops::Tilt);
144 if ((axisMask & Biclops::VergeMask) != 0)
145 vergeAxis = biclops.GetAxis(Biclops::Verge);
147 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API 148 if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) {
152 if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) {
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))
169 panAxis->GetProfile(panProfile);
170 tiltAxis->GetProfile(tiltProfile);
171 if ((axisMask & Biclops::VergeMask) != 0)
172 vergeAxis->GetProfile(vergeProfile);
190 void vpRobotBiclopsController::setPosition(
const vpColVector &q,
double percentVelocity)
197 panAxis->SetProfileMode(PMDTrapezoidalProfile);
198 tiltAxis->SetProfileMode(PMDTrapezoidalProfile);
201 PMDUtils::AxisList axisList;
202 axisList.push_back(panAxis);
203 axisList.push_back(tiltAxis);
205 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API 208 panAxis->GetProfile(panProfile);
209 tiltAxis->GetProfile(tiltProfile);
214 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
217 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
221 panAxis->SetProfile(panProfile);
222 tiltAxis->SetProfile(tiltProfile);
226 PMDAxisControl::CountsProfile desired_profile;
231 panProfile.pos = PMDUtils::RadsToRevs(q[0]);
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;
240 panAxis->SetProfile(desired_profile);
245 tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
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;
252 tiltAxis->SetProfile(desired_profile);
256 PMDUtils::Coordinate(axisList);
257 biclops.Move(Biclops::PanMask + Biclops::TiltMask );
269 void vpRobotBiclopsController::setVelocity(
const vpColVector &q_dot)
276 #ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API 279 panAxis->GetProfile(panProfile);
280 tiltAxis->GetProfile(tiltProfile);
285 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
286 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
289 panAxis->SetProfile(panProfile);
290 tiltAxis->SetProfile(tiltProfile);
292 panAxis->SetProfileMode(PMDVelocityContouringProfile);
293 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
295 panAxis->SetProfileMode(PMDVelocityContouringProfile);
296 tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
298 PMDAxisControl::CountsProfile desired_profile;
303 panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
305 panAxis->ProfileToCounts(panProfile, desired_profile);
306 panAxis->SetProfile(desired_profile);
311 tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
313 tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
314 tiltAxis->SetProfile(desired_profile);
317 biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0);
327 vpColVector vpRobotBiclopsController::getPosition()
329 vpDEBUG_TRACE(12,
"Start vpRobotBiclopsController::getPosition() ");
331 PMDint32 panpos, tiltpos;
333 panAxis->GetPosition(panpos);
334 tiltAxis->GetPosition(tiltpos);
336 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
337 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
339 vpCDEBUG(11) <<
"++++++++ Mesure : " << q.t();
340 vpDEBUG_TRACE(12,
"End vpRobotBiclopsController::getPosition()");
352 vpColVector vpRobotBiclopsController::getActualPosition()
355 PMDint32 panpos, tiltpos;
357 panAxis->GetActualPosition(panpos);
358 tiltAxis->GetActualPosition(tiltpos);
360 q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
361 q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
373 vpColVector vpRobotBiclopsController::getVelocity()
376 PMDint32 pan_vel, tilt_vel;
378 panAxis->GetVelocity(pan_vel);
379 tiltAxis->GetVelocity(tilt_vel);
381 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
382 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
394 vpColVector vpRobotBiclopsController::getActualVelocity()
397 PMDint32 pan_vel, tilt_vel;
399 panAxis->GetActualVelocity(pan_vel);
400 tiltAxis->GetActualVelocity(tilt_vel);
402 q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
403 q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
414 void vpRobotBiclopsController::writeShm(shmType &shm_)
419 memcpy(&this->shm, &shm_,
sizeof(shmType));
432 vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm()
439 memcpy(&tmp_shm, &this->shm,
sizeof(shmType));
442 vpDEBUG_TRACE(13,
"tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
448 #elif !defined(VISP_BUILD_SHARED_LIBS) 451 void dummy_vpRobotBiclopsController(){};
454 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
Error that can be emited by the vpRobot class and its derivates.
static const unsigned int ndof
static const float speedLimit
unsigned int getRows() const
Implementation of column vector and the associated operations.