Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobotFlirPtu.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 Flir Ptu Cpi robot.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_FLIR_PTU_SDK
39
40#include <signal.h>
41#include <stdexcept>
42
43extern "C" {
44#include <cpi.h>
45}
46
52#include <visp3/core/vpHomogeneousMatrix.h>
53#include <visp3/robot/vpRobotFlirPtu.h>
54
63{
64 std::stringstream msg;
65 msg << "Stop the FLIR PTU by signal (" << signo << "): " << (char)7;
66 switch (signo) {
67 case SIGINT:
68 msg << "SIGINT (stop by ^C) ";
69 break;
70 case SIGSEGV:
71 msg << "SIGSEGV (stop due to a segmentation fault) ";
72 break;
73#ifndef WIN32
74 case SIGBUS:
75 msg << "SIGBUS (stop due to a bus error) ";
76 break;
77 case SIGKILL:
78 msg << "SIGKILL (stop by CTRL \\) ";
79 break;
80 case SIGQUIT:
81 msg << "SIGQUIT ";
82 break;
83#endif
84 default:
85 msg << signo << std::endl;
86 }
87
89}
90
95{
96 // If you want to control the robot in Cartesian in a tool frame, set the corresponding transformation in m_eMc
97 // that is set to identity by default in the constructor.
98
101
102 // Set here the robot degrees of freedom number
103 nDof = 2; // Flir Ptu has 2 dof
104}
105
110 : m_eMc(), m_cer(NULL), m_status(0), m_pos_max_tics(2), m_pos_min_tics(2), m_vel_max_tics(2), m_res(2),
111 m_connected(false), m_njoints(2), m_positioning_velocity(20.)
112{
113 signal(SIGINT, vpRobotFlirPtu::emergencyStop);
114 signal(SIGSEGV, vpRobotFlirPtu::emergencyStop);
115#ifndef WIN32
116 signal(SIGBUS, vpRobotFlirPtu::emergencyStop);
117 signal(SIGKILL, vpRobotFlirPtu::emergencyStop);
118 signal(SIGQUIT, vpRobotFlirPtu::emergencyStop);
119#endif
120
121 init();
122}
123
132
133/*
134 At least one of these function has to be implemented to control the robot with a
135 Cartesian velocity:
136 - get_eJe()
137 - get_fJe()
138*/
139
147{
149 vpColVector q;
151 eJe.resize(6, 2);
152 eJe = 0;
153 eJe[3][0] = -sin(q[1]);
154 eJe[4][1] = -1;
155 eJe[5][0] = -cos(q[1]);
156 return eJe;
157}
158
166
174{
176 vpColVector q;
178 fJe.resize(6, 2);
179 fJe = 0;
180 fJe[3][1] = -sin(q[1]);
181 fJe[4][1] = -cos(q[1]);
182 fJe[5][0] = -1;
183
184 return fJe;
185}
186
194
200{
202 vpColVector q;
204 double c1 = cos(q[0]);
205 double c2 = cos(q[1]);
206 double s1 = sin(q[0]);
207 double s2 = sin(q[1]);
208
209 fMe[0][0] = c1 * c2;
210 fMe[0][1] = s1;
211 fMe[0][2] = -c1 * s2;
212
213 fMe[1][0] = -s1 * c2;
214 fMe[1][1] = c1;
215 fMe[1][2] = s1 * s2;
216
217 fMe[2][0] = s2;
218 fMe[2][1] = 0;
219 fMe[2][2] = c2;
220
221 return fMe;
222}
223
237{
239 cVe.buildFrom(m_eMc.inverse());
240
241 return cVe;
242}
243
244/*
245 At least one of these function has to be implemented to control the robot:
246 - setCartVelocity()
247 - setJointVelocity()
248*/
249
257{
258 if (v.size() != 6) {
260 "Cannot send a velocity-skew vector in tool frame that is not 6-dim (%d)", v.size()));
261 }
262
263 vpColVector v_e; // This is the velocity that the robot is able to apply in the end-effector frame
264 switch (frame) {
265 case vpRobot::TOOL_FRAME: {
266 // We have to transform the requested velocity in the end-effector frame.
267 // Knowing that the constant transformation between the tool frame and the end-effector frame obtained
268 // by extrinsic calibration is set in m_eMc we can compute the velocity twist matrix eVc that transform
269 // a velocity skew from tool (or camera) frame into end-effector frame
271 v_e = eVc * v;
272 break;
273 }
274
277 v_e = v;
278 break;
279 }
282 // Out of the scope
283 break;
284 }
285
286 // Implement your stuff here to send the end-effector velocity skew v_e
287 // - If the SDK allows to send cartesian velocities in the end-effector, it's done. Just wrap data in v_e
288 // - If the SDK allows to send cartesian velocities in the reference (or base) frame you have to implement
289 // the robot Jacobian in set_fJe() and call:
290 // vpColVector v = get_fJe().inverse() * v_e;
291 // At this point you have to wrap data in v that is the 6-dim velocity to apply to the robot
292 // - If the SDK allows to send only joint velocities you have to implement the robot Jacobian in set_eJe()
293 // and call:
294 // vpColVector qdot = get_eJe().inverse() * v_e;
295 // setJointVelocity(qdot);
296 // - If the SDK allows to send only a cartesian position trajectory of the end-effector position in the base frame
297 // called fMe (for fix frame to end-effector homogeneous transformation) you can transform the cartesian
298 // velocity in the end-effector into a displacement eMe using the exponetial map:
299 // double delta_t = 0.010; // in sec
300 // vpHomogenesousMatrix eMe = vpExponentialMap::direct(v_e, delta_t);
301 // vpHomogenesousMatrix fMe = getPosition(vpRobot::REFERENCE_FRAME);
302 // the new position to reach is than given by fMe * eMe
303 // vpColVector fpe(vpPoseVector(fMe * eMe));
304 // setPosition(vpRobot::REFERENCE_FRAME, fpe);
305
306 std::cout << "Not implemented ! " << std::endl;
307 std::cout << "To implement me you need : " << std::endl;
308 std::cout << "\t to known the robot jacobian expressed in ";
309 std::cout << "the end-effector frame (eJe) " << std::endl;
310 std::cout << "\t the frame transformation between tool or camera frame ";
311 std::cout << "and end-effector frame (cMe)" << std::endl;
312}
313
319{
320 if (!m_connected) {
321 disconnect();
322 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
323 }
324
325 std::vector<int> vel_tics(2);
326
327 for (int i = 0; i < 2; i++) {
328 vel_tics[i] = rad2tics(i, qdot[i]);
329 if (std::fabs(vel_tics[i]) > m_vel_max_tics[i]) {
330 disconnect();
331 throw(vpException(vpException::fatalError, "Cannot set joint %d velocity %f (deg/s). Out of limits [-%f, %f].", i,
332 vpMath::deg(qdot[i]), -tics2deg(i, m_vel_max_tics[i]), tics2deg(i, m_vel_max_tics[i])));
333 }
334 }
335
336 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, vel_tics[0]) ||
337 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET, vel_tics[1])) {
338 throw(vpException(vpException::fatalError, "Unable to set velocity."));
339 }
340}
341
350{
353 "Cannot send a velocity to the robot. "
354 "Call setRobotState(vpRobot::STATE_VELOCITY_CONTROL) once before "
355 "entering your control loop.");
356 }
357
358 vpColVector vel_sat(6);
359
360 // Velocity saturation
361 switch (frame) {
362 // Saturation in cartesian space
366 case vpRobot::MIXT_FRAME: {
367 if (vel.size() != 6) {
369 "Cannot apply a Cartesian velocity that is not a 6-dim vector (%d)", vel.size()));
370 }
371 vpColVector vel_max(6);
372
373 for (unsigned int i = 0; i < 3; i++)
374 vel_max[i] = getMaxTranslationVelocity();
375 for (unsigned int i = 3; i < 6; i++)
376 vel_max[i] = getMaxRotationVelocity();
377
378 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
379
380 setCartVelocity(frame, vel_sat);
381 break;
382 }
383 // Saturation in joint space
385 if (vel.size() != static_cast<size_t>(nDof)) {
386 throw(vpException(vpException::dimensionError, "Cannot apply a joint velocity that is not a %d-dim vector (%d)",
387 nDof, vel.size()));
388 }
389 vpColVector vel_max(vel.size());
390
391 // Since the robot has only rotation axis all the joint max velocities are set to getMaxRotationVelocity()
392 vel_max = getMaxRotationVelocity();
393
394 vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
395
396 setJointVelocity(vel_sat);
397 }
398 }
399}
400
401/*
402 THESE FUNCTIONS ARE NOT MANDATORY BUT ARE USUALLY USEFUL
403*/
404
410{
411 if (!m_connected) {
412 disconnect();
413 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
414 }
415
416 std::vector<int> pos_tics(2);
417
418 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_CURRENT_POS_GET, &pos_tics[0])) {
419 disconnect();
420 throw(vpException(vpException::fatalError, "Unable to query pan position."));
421 }
422 if (cpi_ptcmd(m_cer, &m_status, OP_TILT_CURRENT_POS_GET, &pos_tics[1])) {
423 disconnect();
424 throw(vpException(vpException::fatalError, "Unable to query pan position."));
425 }
426
427 q.resize(2);
428 for (int i = 0; i < 2; i++) {
429 q[i] = tics2rad(i, pos_tics[i]);
430 }
431}
432
439{
440 if (frame == JOINT_STATE) {
442 } else {
443 std::cout << "Not implemented ! " << std::endl;
444 }
445}
446
453{
454 if (frame != vpRobot::JOINT_STATE) {
455 std::cout << "FLIR PTU positioning is not implemented in this frame" << std::endl;
456 return;
457 }
458
459 if (q.size() != 2) {
460 disconnect();
461 throw(vpException(vpException::fatalError, "FLIR PTU has only %d joints. Cannot set a position that is %d-dim.",
462 m_njoints, q.size()));
463 }
464 if (!m_connected) {
465 disconnect();
466 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
467 }
468
469 double vmin = 0.01, vmax = 100.;
471 disconnect();
472 throw(
473 vpException(vpException::fatalError, "FLIR PTU Positioning velocity %f is not in range [%f, %f]", vmin, vmax));
474 }
475
476 std::vector<int> pos_tics(2);
477
478 for (int i = 0; i < 2; i++) {
479 pos_tics[i] = rad2tics(i, q[i]);
480 if (pos_tics[i] < m_pos_min_tics[i] || pos_tics[i] > m_pos_max_tics[i]) {
481 disconnect();
482 throw(vpException(vpException::fatalError, "Cannot set joint %d position %f (deg). Out of limits [%f, %f].", i,
483 vpMath::deg(q[i]), tics2deg(i, m_pos_min_tics[i]), tics2deg(i, m_pos_max_tics[i])));
484 }
485 }
486
487 // Set desired speed wrt max pan/tilt speed
488 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_SPEED_SET, (int)(m_vel_max_tics[0] * m_positioning_velocity / 100.)) ||
489 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_SPEED_SET,
490 (int)(m_vel_max_tics[1] * m_positioning_velocity / 100.))) {
491 disconnect();
492 throw(vpException(vpException::fatalError, "Setting FLIR pan/tilt positioning velocity failed"));
493 }
494
495 if (cpi_ptcmd(m_cer, &m_status, OP_PAN_DESIRED_POS_SET, pos_tics[0]) ||
496 cpi_ptcmd(m_cer, &m_status, OP_TILT_DESIRED_POS_SET, pos_tics[1])) {
497 disconnect();
498 throw(vpException(vpException::fatalError, "FLIR PTU failed to go to position %d, %d (deg).", vpMath::deg(q[0]),
499 vpMath::deg(q[1])));
500 }
501
502 if (cpi_block_until(m_cer, NULL, NULL, OP_PAN_CURRENT_POS_GET, pos_tics[0]) ||
503 cpi_block_until(m_cer, NULL, NULL, OP_TILT_CURRENT_POS_GET, pos_tics[1])) {
504 disconnect();
505 throw(vpException(vpException::fatalError, "FLIR PTU failed to wait until position %d, %d reached (deg)",
506 vpMath::deg(q[0]), vpMath::deg(q[1])));
507 }
508}
509
516{
517 (void)frame;
518 (void)q;
519 std::cout << "Not implemented ! " << std::endl;
520}
521
528void vpRobotFlirPtu::connect(const std::string &portname, int baudrate)
529{
530 char errstr[128];
531
532 if (m_connected) {
533 disconnect();
534 }
535
536 if (portname.empty()) {
537 disconnect();
538 throw(vpException(vpException::fatalError, "Port name is required to connect to FLIR PTU."));
539 }
540
541 if ((m_cer = (struct cerial *)malloc(sizeof(struct cerial))) == NULL) {
542 disconnect();
543 throw(vpException(vpException::fatalError, "Out of memory during FLIR PTU connection."));
544 }
545
546 // Open a port
547 if (ceropen(m_cer, portname.c_str(), 0)) {
548#if WIN32
549 throw(vpException(vpException::fatalError, "Failed to open %s: %s.", portname.c_str(),
550 cerstrerror(m_cer, errstr, sizeof(errstr))));
551#else
552 throw(vpException(vpException::fatalError, "Failed to open %s: %s.\nRun `sudo chmod a+rw %s`", portname.c_str(),
553 cerstrerror(m_cer, errstr, sizeof(errstr)), portname.c_str()));
554#endif
555 }
556
557 // Set baudrate
558 // ignore errors since not all devices are serial ports
559 cerioctl(m_cer, CERIAL_IOCTL_BAUDRATE_SET, &baudrate);
560
561 // Flush any characters already buffered
562 cerioctl(m_cer, CERIAL_IOCTL_FLUSH_INPUT, NULL);
563
564 // Set two second timeout */
565 int timeout = 2000;
566 if (cerioctl(m_cer, CERIAL_IOCTL_TIMEOUT_SET, &timeout)) {
567 disconnect();
568 throw(vpException(vpException::fatalError, "cerial: timeout ioctl not supported."));
569 }
570
571 // Sync and lock
572 int trial = 0;
573 do {
574 trial++;
575 } while (trial <= 3 && (cpi_resync(m_cer) || cpi_ptcmd(m_cer, &m_status, OP_NOOP)));
576 if (trial > 3) {
577 disconnect();
578 throw(vpException(vpException::fatalError, "Cannot communicate with FLIR PTU."));
579 }
580
581 // Immediately execute commands (slave mode should be opt-in)
582 int rc;
583 if ((rc = cpi_ptcmd(m_cer, &m_status, OP_EXEC_MODE_SET, (cpi_enum)CPI_IMMEDIATE_MODE))) {
584 disconnect();
585 throw(vpException(vpException::fatalError, "Set Immediate Mode failed: %s", cpi_strerror(rc)));
586 }
587
588 m_connected = true;
589
590 getLimits();
591}
592
598{
599 if (m_cer != NULL) {
600 cerclose(m_cer);
601 free(m_cer);
602 m_cer = NULL;
603 m_connected = false;
604 }
605}
606
611{
612 if (!m_connected) {
613 disconnect();
614 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
615 }
616
617 int status;
618
619 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MAX_POSITION, &m_pos_max_tics[0])) ||
620 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_MIN_POSITION, &m_pos_min_tics[0])) ||
621 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MAX_POSITION, &m_pos_max_tics[1])) ||
622 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_MIN_POSITION, &m_pos_min_tics[1])) ||
623 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[0])) ||
624 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_UPPER_SPEED_LIMIT_GET, &m_vel_max_tics[1]))) {
625 disconnect();
626 throw(vpException(vpException::fatalError, "Failed to get limits (%d) %s.", status, cpi_strerror(status)));
627 }
628
629 // Get the ptu resolution so we can convert the angles to ptu positions
630 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_RESOLUTION, &m_res[0])) ||
631 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_RESOLUTION, &m_res[1]))) {
632 disconnect();
633 throw(vpException(vpException::fatalError, "Failed to get resolution (%d) %s.", status, cpi_strerror(status)));
634 }
635
636 for (size_t i = 0; i < 2; i++) {
637 m_res[i] /= 3600.; // Resolutions are in arc-seconds, but we want degrees
638 }
639}
640
648{
649 if (!m_connected) {
650 disconnect();
651 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
652 }
653 vpColVector pan_pos_limits(2);
654 pan_pos_limits[0] = tics2rad(0, m_pos_min_tics[0]);
655 pan_pos_limits[1] = tics2rad(0, m_pos_max_tics[0]);
656
657 return pan_pos_limits;
658}
659
667{
668 if (!m_connected) {
669 disconnect();
670 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
671 }
672 vpColVector tilt_pos_limits(2);
673 tilt_pos_limits[0] = tics2rad(0, m_pos_min_tics[1]);
674 tilt_pos_limits[1] = tics2rad(0, m_pos_max_tics[1]);
675
676 return tilt_pos_limits;
677}
678
686{
687 if (!m_connected) {
688 disconnect();
689 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
690 }
691 vpColVector vel_max(2);
692 for (int i = 0; i < 2; i++) {
693 vel_max[i] = tics2rad(i, m_vel_max_tics[i]);
694 }
695 return vel_max;
696}
697
705{
706 if (!m_connected) {
707 disconnect();
708 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
709 }
710 if (pan_limits.size() != 2) {
711 disconnect();
712 throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
713 }
714 std::vector<int> pan_limits_tics(2);
715 for (int i = 0; i < 2; i++) {
716 pan_limits_tics[i] = rad2tics(i, pan_limits[i]);
717 }
718
719 int status;
720 if ((status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MIN_POS_SET, pan_limits_tics[0])) ||
721 (status = cpi_ptcmd(m_cer, &m_status, OP_PAN_USER_MAX_POS_SET, pan_limits_tics[1]))) {
722 disconnect();
723 throw(vpException(vpException::fatalError, "Failed to set pan position limits (%d) %s.", status,
724 cpi_strerror(status)));
725 }
726}
727
735{
736 if (!m_connected) {
737 disconnect();
738 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
739 }
740 if (tilt_limits.size() != 2) {
741 disconnect();
742 throw(vpException(vpException::fatalError, "Cannot set max position that is not a 2-dim vector."));
743 }
744 std::vector<int> tilt_limits_tics(2);
745 for (int i = 0; i < 2; i++) {
746 tilt_limits_tics[i] = rad2tics(i, tilt_limits[i]);
747 }
748
749 int status;
750 if ((status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MIN_POS_SET, tilt_limits_tics[0])) ||
751 (status = cpi_ptcmd(m_cer, &m_status, OP_TILT_USER_MAX_POS_SET, tilt_limits_tics[1]))) {
752 disconnect();
753 throw(vpException(vpException::fatalError, "Failed to set tilt position limits (%d) %s.", status,
754 cpi_strerror(status)));
755 }
756}
757
765
774{
775 if (!m_connected) {
776 return getRobotState();
777 }
778
779 switch (newState) {
780 case vpRobot::STATE_STOP: {
781 // Start primitive STOP only if the current state is Velocity
783 stopMotion();
784
785 // Set the PTU to pure velocity mode
786 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
787 throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
788 }
789 }
790 break;
791 }
794 std::cout << "Change the control mode from velocity to position control.\n";
795 stopMotion();
796
797 // Set the PTU to pure velocity mode
798 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_INDEPENDENT)) {
799 throw(vpException(vpException::fatalError, "Unable to set control mode independent."));
800 }
801
802 } else {
803 // std::cout << "Change the control mode from stop to position
804 // control.\n";
805 }
806 break;
807 }
810 std::cout << "Change the control mode from stop to velocity control.\n";
811
812 // Set the PTU to pure velocity mode
813 if (cpi_ptcmd(m_cer, &m_status, OP_SPEED_CONTROL_MODE_SET, (cpi_enum)CPI_CONTROL_PURE_VELOCITY)) {
814 throw(vpException(vpException::fatalError, "Unable to set velocity control mode."));
815 }
816 }
817 break;
818 }
819 default:
820 break;
821 }
822
823 return vpRobot::setRobotState(newState);
824}
825
830{
831 if (!m_connected) {
832 return;
833 }
834
835 if (cpi_ptcmd(m_cer, &m_status, OP_RESET, NULL)) {
836 throw(vpException(vpException::fatalError, "Unable to reset PTU."));
837 }
838}
839
844{
846 return;
847 }
848
849 if (!m_connected) {
850 return;
851 }
852
853 if (cpi_ptcmd(m_cer, &m_status, OP_HALT, NULL)) {
854 throw(vpException(vpException::fatalError, "Unable to stop PTU."));
855 }
856}
857
864{
865 if (!m_connected) {
866 disconnect();
867 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
868 }
869
870 char str[64];
871 if (cpi_ptcmd(m_cer, &m_status, OP_NET_IP_GET, (int)sizeof(str), NULL, &str)) {
872 throw(vpException(vpException::fatalError, "Unable to get Network IP."));
873 }
874
875 return (std::string(str));
876}
877
884{
885 if (!m_connected) {
886 disconnect();
887 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
888 }
889
890 char str[64];
891 if (cpi_ptcmd(m_cer, &m_status, OP_NET_GATEWAY_GET, (int)sizeof(str), NULL, &str)) {
892 throw(vpException(vpException::fatalError, "Unable to get Network Gateway."));
893 }
894
895 return (std::string(str));
896}
897
904{
905 if (!m_connected) {
906 disconnect();
907 throw(vpException(vpException::fatalError, "FLIR PTU is not connected."));
908 }
909
910 char str[64];
911 if (cpi_ptcmd(m_cer, &m_status, OP_NET_HOSTNAME_GET, (int)sizeof(str), NULL, &str)) {
912 throw(vpException(vpException::fatalError, "Unable to get Network hostname."));
913 }
914
915 return (std::string(str));
916}
917
926int vpRobotFlirPtu::rad2tics(int axis, double rad) { return (static_cast<int>(vpMath::deg(rad) / m_res[axis])); }
927
936double vpRobotFlirPtu::tics2deg(int axis, int tics) { return (tics * m_res[axis]); }
937
946double vpRobotFlirPtu::tics2rad(int axis, int tics) { return vpMath::rad(tics2deg(axis, tics)); }
947
948#elif !defined(VISP_BUILD_SHARED_LIBS)
949// Work around to avoid warning: libvisp_robot.a(vpRobotFlirPtu.cpp.o) has
950// no symbols
951void dummy_vpRobotFlirPtu(){};
952#endif
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
Definition vpMath.h:116
static double deg(double rad)
Definition vpMath.h:106
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
Error that can be emitted by the vpRobot class and its derivatives.
@ wrongStateError
Wrong robot state.
@ signalException
Signal exception returned after SIGINT (CTRL-C), SIGBUS, SIGSEGV, SIGSEGV (CTRL-),...
vpHomogeneousMatrix m_eMc
Constant transformation between end-effector and tool (or camera) frame.
std::vector< int > m_pos_max_tics
Pan min/max position in robot tics unit.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void connect(const std::string &portname, int baudrate=9600)
vpColVector getTiltPosLimits()
vpColVector getPanPosLimits()
double m_positioning_velocity
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q)
std::string getNetworkGateway()
std::vector< double > m_res
Pan/tilt tic resolution in deg.
static void emergencyStop(int signo)
void setPositioningVelocity(double velocity)
virtual ~vpRobotFlirPtu()
std::string getNetworkIP()
std::vector< int > m_vel_max_tics
Pan/tilt max velocity in robot tics unit.
void setCartVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
struct cerial * m_cer
vpVelocityTwistMatrix get_cVe() const
void getJointPosition(vpColVector &q)
std::string getNetworkHostName()
void setJointVelocity(const vpColVector &qdot)
void setPanPosLimits(const vpColVector &pan_limits)
std::vector< int > m_pos_min_tics
Tilt min/max position in robot tics unit.
vpColVector getPanTiltVelMax()
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
void setTiltPosLimits(const vpColVector &tilt_limits)
int nDof
number of degrees of freedom
Definition vpRobot.h:100
static const double maxTranslationVelocityDefault
Definition vpRobot.h:95
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
Definition vpRobot.h:102
virtual vpRobotStateType getRobotState(void) const
Definition vpRobot.h:142
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition vpRobot.cpp:160
vpControlFrameType
Definition vpRobot.h:73
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ JOINT_STATE
Definition vpRobot.h:78
@ TOOL_FRAME
Definition vpRobot.h:82
@ MIXT_FRAME
Definition vpRobot.h:84
@ END_EFFECTOR_FRAME
Definition vpRobot.h:79
static const double maxRotationVelocityDefault
Definition vpRobot.h:97
vpRobotStateType
Definition vpRobot.h:62
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
Definition vpRobot.h:63
double getMaxRotationVelocity(void) const
Definition vpRobot.cpp:270
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Definition vpRobot.h:106
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
double maxTranslationVelocity
Definition vpRobot.h:94
double getMaxTranslationVelocity(void) const
Definition vpRobot.cpp:248
double maxRotationVelocity
Definition vpRobot.h:96
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)