Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpVirtuose.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: Class which enables to project an image in the 3D space
32 * and get the view of a virtual camera.
33 *
34*****************************************************************************/
35
41#include <visp3/core/vpException.h>
42#include <visp3/robot/vpVirtuose.h>
43
44#ifdef VISP_HAVE_VIRTUOSE
45
52 : m_virtContext(NULL), m_ip_port("localhost#5000"), m_verbose(false), m_apiMajorVersion(0), m_apiMinorVersion(0),
53 m_ctrlMajorVersion(0), m_ctrlMinorVersion(0), m_typeCommand(COMMAND_TYPE_IMPEDANCE), m_indexType(INDEXING_ALL),
54 m_is_init(false), m_period(0.001f), m_njoints(6)
55{
56 virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion);
57 std::cout << "API version: " << m_apiMajorVersion << "." << m_apiMinorVersion << std::endl;
58}
59
64{
65 if (m_virtContext != NULL) {
66 virtClose(m_virtContext);
67 m_virtContext = NULL;
68 }
69}
70
75
81void vpVirtuose::setIpAddressAndPort(const std::string &ip, int port)
82{
83 std::stringstream ss;
84 ss << ip << "#" << port;
85
86 m_ip_port = ss.str();
87}
88
97{
98 if (force.size() != 6) {
100 "Cannot apply a force feedback (dim %d) to the haptic "
101 "device that is not 6-dimension",
102 force.size()));
103 }
104
105 init();
106
107 float virtforce[6];
108 for (unsigned int i = 0; i < 6; i++)
109 virtforce[i] = (float)force[i];
110
111 if (virtAddForce(m_virtContext, virtforce)) {
112 int err = virtGetErrorCode(m_virtContext);
113 throw(vpException(vpException::fatalError, "Error calling virtAddForce: error code %d", err));
114 }
115}
116
122{
123 init();
124
125 if (virtEnableForceFeedback(m_virtContext, enable)) {
126 int err = virtGetErrorCode(m_virtContext);
127 throw(vpException(vpException::fatalError, "Error calling virtEnableForceFeedback(): error code %d", err));
128 }
129}
130
135{
136 if (!m_is_init) {
137 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
138 }
139
140 float articular_position_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
141
142 if (virtGetArticularPosition(m_virtContext, articular_position_)) {
143 int err = virtGetErrorCode(m_virtContext);
144 throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition(): error code %d", err));
145 }
146
147 vpColVector articularPosition(m_njoints, 0);
148 for (unsigned int i = 0; i < m_njoints; i++)
149 articularPosition[i] = articular_position_[i];
150
151 return articularPosition;
152}
153
158{
159 if (!m_is_init) {
160 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
161 }
162
163 float articular_velocity_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
164
165 if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) {
166 int err = virtGetErrorCode(m_virtContext);
167 throw(vpException(vpException::fatalError, "Error calling virtGetArticularSpeed: error code %d", err));
168 }
169
170 vpColVector articularVelocity(m_njoints, 0);
171
172 for (unsigned int i = 0; i < m_njoints; i++)
173 articularVelocity[i] = articular_velocity_[i];
174
175 return articularVelocity;
176}
177
186{
187 if (!m_is_init) {
188 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
189 }
190
191 float position_[7];
192 vpPoseVector position;
193 vpTranslationVector translation;
194 vpQuaternionVector quaternion;
195
196 if (virtGetAvatarPosition(m_virtContext, position_)) {
197 int err = virtGetErrorCode(m_virtContext);
198 throw(vpException(vpException::fatalError, "Error calling virtGetAvatarPosition: error code %d", err));
199 } else {
200 for (int i = 0; i < 3; i++)
201 translation[i] = position_[i];
202 for (int i = 0; i < 4; i++)
203 quaternion[i] = position_[3 + i];
204
205 vpThetaUVector thetau(quaternion);
206
207 position.buildFrom(translation, thetau);
208
209 return position;
210 }
211}
212
220{
221 if (!m_is_init) {
222 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
223 }
224
225 vpPoseVector position;
226 float position_[7];
227 vpTranslationVector translation;
228 vpQuaternionVector quaternion;
229
230 if (virtGetBaseFrame(m_virtContext, position_)) {
231 int err = virtGetErrorCode(m_virtContext);
232 throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err));
233 } else {
234 for (int i = 0; i < 3; i++)
235 translation[i] = position_[i];
236 for (int i = 0; i < 4; i++)
237 quaternion[i] = position_[3 + i];
238
239 vpThetaUVector thetau(quaternion);
240
241 position.buildFrom(translation, thetau);
242
243 return position;
244 }
245}
246
250VirtCommandType vpVirtuose::getCommandType() const
251{
252 if (!m_is_init) {
253 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
254 }
255
256 VirtCommandType type;
257
258 if (virtGetCommandType(m_virtContext, &type)) {
259 int err = virtGetErrorCode(m_virtContext);
260 throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err));
261 }
262 return type;
263}
264
270{
271 if (!m_is_init) {
272 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
273 }
274
275 int deadman;
276 if (virtGetDeadMan(m_virtContext, &deadman)) {
277 int err = virtGetErrorCode(m_virtContext);
278 throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err));
279 }
280 return (deadman ? true : false);
281}
282
289{
290 if (!m_is_init) {
291 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
292 }
293
294 int emergencyStop;
295 if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {
296 int err = virtGetErrorCode(m_virtContext);
297 throw(vpException(vpException::fatalError, "Error calling virtGetEmergencyStop: error code %d", err));
298 }
299 return (emergencyStop ? true : false);
300}
301
307{
308 if (!m_is_init) {
309 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
310 }
311
312 vpColVector force(6, 0);
313 float force_[6];
314 if (virtGetForce(m_virtContext, force_)) {
315 int err = virtGetErrorCode(m_virtContext);
316 throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err));
317 }
318
319 for (unsigned int i = 0; i < 6; i++)
320 force[i] = force_[i];
321 return force;
322}
323
356VirtContext vpVirtuose::getHandler() { return m_virtContext; }
357
363unsigned int vpVirtuose::getJointsNumber() const
364{
365 if (!m_is_init) {
366 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
367 }
368
369 return m_njoints;
370}
371
379{
380 if (!m_is_init) {
381 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
382 }
383
384 vpPoseVector position;
385 float position_[7];
386 vpTranslationVector translation;
387 vpQuaternionVector quaternion;
388
389 if (virtGetObservationFrame(m_virtContext, position_)) {
390 int err = virtGetErrorCode(m_virtContext);
391 throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err));
392 } else {
393 for (int i = 0; i < 3; i++)
394 translation[i] = position_[i];
395 for (int i = 0; i < 4; i++)
396 quaternion[i] = position_[3 + i];
397
398 vpThetaUVector thetau(quaternion);
399
400 position.buildFrom(translation, thetau);
401 }
402 return position;
403}
404
412{
413 if (!m_is_init) {
414 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
415 }
416
417 vpPoseVector position;
418 float position_[7];
419 vpTranslationVector translation;
420 vpQuaternionVector quaternion;
421
422 if (virtGetPhysicalPosition(m_virtContext, position_)) {
423 int err = virtGetErrorCode(m_virtContext);
424 throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalPosition: error code %d", err));
425 } else {
426 for (int i = 0; i < 3; i++)
427 translation[i] = position_[i];
428 for (int i = 0; i < 4; i++)
429 quaternion[i] = position_[3 + i];
430
431 vpThetaUVector thetau(quaternion);
432
433 position.buildFrom(translation, thetau);
434 }
435 return position;
436}
437
445{
446 if (!m_is_init) {
447 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
448 }
449
450 vpColVector vel(6, 0);
451 float speed[6];
452 if (virtGetPhysicalSpeed(m_virtContext, speed)) {
453 int err = virtGetErrorCode(m_virtContext);
454 throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s",
455 virtGetErrorMessage(err)));
456 }
457 for (unsigned int i = 0; i < 6; i++)
458 vel[i] = speed[i];
459 return vel;
460}
461
468{
469 if (!m_is_init) {
470 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
471 }
472
473 vpPoseVector position;
474 float position_[7];
475 vpTranslationVector translation;
476 vpQuaternionVector quaternion;
477
478 if (virtGetPosition(m_virtContext, position_)) {
479 int err = virtGetErrorCode(m_virtContext);
480 throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err));
481 } else {
482 for (int i = 0; i < 3; i++)
483 translation[i] = position_[i];
484 for (int i = 0; i < 4; i++)
485 quaternion[i] = position_[3 + i];
486
487 vpThetaUVector thetau(quaternion);
488
489 position.buildFrom(translation, thetau);
490 }
491 return position;
492}
493
498{
499 if (!m_is_init) {
500 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
501 }
502
503 int power;
504 virtGetPowerOn(m_virtContext, &power);
505 return (power ? true : false);
506}
507
515{
516 if (!m_is_init) {
517 throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
518 }
519
520 vpColVector vel(6, 0);
521 float speed[6];
522 if (virtGetSpeed(m_virtContext, speed)) {
523 int err = virtGetErrorCode(m_virtContext);
524 throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err)));
525 }
526 for (unsigned int i = 0; i < 6; i++)
527 vel[i] = speed[i];
528 return vel;
529}
530
537{
538 if (!m_is_init) {
539 m_virtContext = virtOpen(m_ip_port.c_str());
540
541 if (m_virtContext == NULL) {
542 int err = virtGetErrorCode(m_virtContext);
544 "Cannot open communication with haptic device using %s: %s. Check ip and port values",
545 m_ip_port.c_str(), virtGetErrorMessage(err)));
546 }
547
548 if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {
549 int err = virtGetErrorCode(m_virtContext);
550 throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",
551 virtGetErrorMessage(err)));
552 }
553
554 if (m_verbose) {
555 std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;
556 }
557
558 if (virtSetCommandType(m_virtContext, m_typeCommand)) {
559 int err = virtGetErrorCode(m_virtContext);
560 throw(
561 vpException(vpException::fatalError, "Cannot set haptic device command type: %s", virtGetErrorMessage(err)));
562 }
563
564 if (virtSetTimeStep(m_virtContext, m_period)) {
565 int err = virtGetErrorCode(m_virtContext);
566 throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
567 }
568
569 // Update number of joints
570 float articular_position_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
571
572 if (virtGetArticularPosition(m_virtContext, articular_position_)) {
573 int err = virtGetErrorCode(m_virtContext);
574 throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition() in int(): error code %d",
575 err));
576 }
577
578 m_njoints = 6; // At least 6 joints
579 for (unsigned int i = m_njoints; i < 20; i++) {
580 m_njoints = i;
581 if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
582 break;
583 }
584 }
585
586 m_is_init = true;
587 }
588}
589
598{
599 init();
600
601 if (articularForce.size() != m_njoints) {
603 "Cannot apply an articular force feedback (dim %d) to "
604 "the haptic device that is not 6-dimension",
605 articularForce.size()));
606 }
607
608 float *articular_force = new float[m_njoints];
609 for (unsigned int i = 0; i < m_njoints; i++)
610 articular_force[i] = (float)articularForce[i];
611
612 if (virtSetArticularForce(m_virtContext, articular_force)) {
613 delete[] articular_force;
614 int err = virtGetErrorCode(m_virtContext);
615 throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err));
616 }
617
618 delete[] articular_force;
619}
620
628void vpVirtuose::setArticularPosition(const vpColVector &articularPosition)
629{
630 init();
631
632 if (articularPosition.size() != m_njoints) {
634 "Cannot send an articular position command (dim %d) to "
635 "the haptic device that is not %d-dimension",
636 m_njoints, articularPosition.size()));
637 }
638
639 float *articular_position = new float[m_njoints];
640 for (unsigned int i = 0; i < m_njoints; i++)
641 articular_position[i] = (float)articularPosition[i];
642
643 if (virtSetArticularPosition(m_virtContext, articular_position)) {
644 int err = virtGetErrorCode(m_virtContext);
645 delete[] articular_position;
646 throw(vpException(vpException::fatalError, "Error calling virtSetArticularPosition: error code %d", err));
647 }
648 delete[] articular_position;
649}
650
658void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity)
659{
660 init();
661
662 if (articularVelocity.size() != m_njoints) {
664 "Cannot send an articular velocity command (dim %d) to "
665 "the haptic device that is not %d-dimension",
666 m_njoints, articularVelocity.size()));
667 }
668
669 float *articular_velocity = new float[m_njoints];
670 for (unsigned int i = 0; i < m_njoints; i++)
671 articular_velocity[i] = (float)articularVelocity[i];
672
673 if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
674 int err = virtGetErrorCode(m_virtContext);
675 delete[] articular_velocity;
676 throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err));
677 }
678 delete[] articular_velocity;
679}
680
689{
690 init();
691
692 float position_[7];
693 vpTranslationVector translation;
694 vpQuaternionVector quaternion;
695
696 position.extract(translation);
697 position.extract(quaternion);
698
699 for (int i = 0; i < 3; i++)
700 position_[i] = (float)translation[i];
701 for (int i = 0; i < 4; i++)
702 position_[3 + i] = (float)quaternion[i];
703
704 if (virtSetBaseFrame(m_virtContext, position_)) {
705 int err = virtGetErrorCode(m_virtContext);
706 throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: error code %d", err));
707 }
708}
709
720void vpVirtuose::setCommandType(const VirtCommandType &type)
721{
722 init();
723
724 if (m_typeCommand != type) {
725 m_typeCommand = type;
726
727 if (virtSetCommandType(m_virtContext, m_typeCommand)) {
728 int err = virtGetErrorCode(m_virtContext);
729 throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err));
730 }
731 }
732}
733
741{
742 init();
743
744 if (force.size() != 6) {
746 "Cannot apply a force feedback (dim %d) to the haptic "
747 "device that is not 6-dimension",
748 force.size()));
749 }
750
751 float virtforce[6];
752 for (unsigned int i = 0; i < 6; i++)
753 virtforce[i] = (float)force[i];
754
755 if (virtSetForce(m_virtContext, virtforce)) {
756 int err = virtGetErrorCode(m_virtContext);
757 throw(vpException(vpException::fatalError, "Error calling virtSetForce: error code %d", err));
758 }
759}
760
766void vpVirtuose::setForceFactor(const float &forceFactor)
767{
768 init();
769
770 if (virtSetForceFactor(m_virtContext, forceFactor)) {
771 int err = virtGetErrorCode(m_virtContext);
772 throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err));
773 }
774}
775
790void vpVirtuose::setIndexingMode(const VirtIndexingType &type)
791{
792 init();
793
794 if (m_indexType != type) {
795 m_indexType = type;
796
797 if (virtSetIndexingMode(m_virtContext, m_indexType)) {
798 int err = virtGetErrorCode(m_virtContext);
799 throw(vpException(vpException::fatalError, "Error calling setIndexingMode: error code %d", err));
800 }
801 }
802}
803
812{
813 init();
814
815 float position_[7];
816 vpTranslationVector translation;
817 vpQuaternionVector quaternion;
818
819 position.extract(translation);
820 position.extract(quaternion);
821
822 for (int i = 0; i < 3; i++)
823 position_[i] = (float)translation[i];
824 for (int i = 0; i < 4; i++)
825 position_[3 + i] = (float)quaternion[i];
826
827 if (virtSetObservationFrame(m_virtContext, position_)) {
828 int err = virtGetErrorCode(m_virtContext);
829 throw(vpException(vpException::fatalError, "Error calling virtSetObservationFrame: error code %d", err));
830 }
831}
832
872void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
873{
874 init();
875
876 if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) {
877 int err = virtGetErrorCode(m_virtContext);
878 throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err));
879 }
880}
881
888{
889 init();
890
891 float position_[7];
892 vpTranslationVector translation;
893 vpQuaternionVector quaternion;
894
895 position.extract(translation);
896 position.extract(quaternion);
897
898 for (int i = 0; i < 3; i++)
899 position_[i] = (float)translation[i];
900 for (int i = 0; i < 4; i++)
901 position_[3 + i] = (float)quaternion[i];
902
903 if (virtSetPosition(m_virtContext, position_)) {
904 int err = virtGetErrorCode(m_virtContext);
905 throw(vpException(vpException::fatalError, "Error calling virtSetPosition: error code %d", err));
906 }
907}
908
913{
914 init();
915
916 if (virtSetPowerOn(m_virtContext, 0)) {
917 int err = virtGetErrorCode(m_virtContext);
918 throw(vpException(vpException::fatalError, "Error calling virtSetPowerOff: error code %d", err));
919 }
920}
921
926{
927 init();
928
929 if (virtSetPowerOn(m_virtContext, 1)) {
930 int err = virtGetErrorCode(m_virtContext);
931 throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err));
932 }
933}
934
940void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit)
941{
942 init();
943
944 if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) {
945 int err = virtGetErrorCode(m_virtContext);
946 throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err));
947 }
948}
949
955void vpVirtuose::setTimeStep(const float &timeStep)
956{
957 init();
958
959 if (m_period != timeStep) {
960 m_period = timeStep;
961
962 if (virtSetTimeStep(m_virtContext, m_period)) {
963 int err = virtGetErrorCode(m_virtContext);
964 throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
965 }
966 }
967}
968
975{
976 init();
977
978 if (velocity.size() != 6) {
979 throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension",
980 velocity.size()));
981 }
982
983 float speed[6];
984 for (unsigned int i = 0; i < 6; i++)
985 speed[i] = (float)velocity[i];
986
987 if (virtSetSpeed(m_virtContext, speed)) {
988 int err = virtGetErrorCode(m_virtContext);
989 throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err));
990 }
991}
992
998void vpVirtuose::setVelocityFactor(const float &velocityFactor)
999{
1000 init();
1001
1002 if (virtSetSpeedFactor(m_virtContext, velocityFactor)) {
1003 int err = virtGetErrorCode(m_virtContext);
1004 throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err));
1005 }
1006}
1007
1014{
1015 init();
1016
1017 if (virtStartLoop(m_virtContext)) {
1018 int err = virtGetErrorCode(m_virtContext);
1019 throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err));
1020 } else
1021 std::cout << "Haptic loop open." << std::endl;
1022}
1023
1030{
1031 init();
1032
1033 if (virtStopLoop(m_virtContext)) {
1034 int err = virtGetErrorCode(m_virtContext);
1035 throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err));
1036 } else
1037 std::cout << "Haptic loop closed." << std::endl;
1038}
1039
1040#else
1041// Work around to avoid warning
1042void dummy_vpVirtuose(){};
1043#endif
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.
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 a pose vector and operations on poses.
void extract(vpRotationMatrix &R) const
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
bool getDeadMan() const
vpColVector getVelocity() const
vpColVector getArticularPosition() const
VirtCommandType getCommandType() const
void setIpAddressAndPort(const std::string &ip, int port)
void setPowerOff()
void setTimeStep(const float &timeStep)
void setForceFactor(const float &forceFactor)
void setArticularVelocity(const vpColVector &articularVelocity)
void setObservationFrame(const vpPoseVector &position)
void enableForceFeedback(int enable)
vpPoseVector getPosition() const
bool getPower() const
vpPoseVector getBaseFrame() const
bool m_verbose
Definition vpVirtuose.h:217
VirtContext getHandler()
int m_apiMajorVersion
Definition vpVirtuose.h:218
void setForce(const vpColVector &force)
void setIndexingMode(const VirtIndexingType &type)
unsigned int getJointsNumber() const
void close()
int m_ctrlMinorVersion
Definition vpVirtuose.h:221
vpPoseVector getPhysicalPosition() const
unsigned int m_njoints
Definition vpVirtuose.h:226
float m_period
Definition vpVirtuose.h:225
bool m_is_init
Definition vpVirtuose.h:224
std::string m_ip_port
Definition vpVirtuose.h:216
void setBaseFrame(const vpPoseVector &position)
VirtContext m_virtContext
Definition vpVirtuose.h:215
vpPoseVector getObservationFrame() const
bool getEmergencyStop() const
void setCommandType(const VirtCommandType &type)
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
vpColVector getArticularVelocity() const
void stopPeriodicFunction()
int m_ctrlMajorVersion
Definition vpVirtuose.h:220
vpPoseVector getAvatarPosition() const
void setPowerOn()
void addForce(vpColVector &force)
void setArticularForce(const vpColVector &articularForce)
int m_apiMinorVersion
Definition vpVirtuose.h:219
void setPosition(vpPoseVector &position)
void setVelocity(vpColVector &velocity)
VirtCommandType m_typeCommand
Definition vpVirtuose.h:222
void startPeriodicFunction()
void setVelocityFactor(const float &velocityFactor)
void setSaturation(const float &forceLimit, const float &torqueLimit)
virtual ~vpVirtuose()
vpColVector getPhysicalVelocity() const
void setArticularPosition(const vpColVector &articularPosition)
VirtIndexingType m_indexType
Definition vpVirtuose.h:223
vpColVector getForce() const