Visual Servoing Platform  version 3.6.1 under development (2024-06-23)
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
52  : m_virtContext(nullptr), 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 != nullptr) {
66  virtClose(m_virtContext);
67  m_virtContext = nullptr;
68  }
69 }
70 
75 
81 void 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  }
200  else {
201  for (int i = 0; i < 3; i++)
202  translation[i] = position_[i];
203  for (int i = 0; i < 4; i++)
204  quaternion[i] = position_[3 + i];
205 
206  vpThetaUVector thetau(quaternion);
207 
208  position.build(translation, thetau);
209 
210  return position;
211  }
212 }
213 
221 {
222  if (!m_is_init) {
223  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
224  }
225 
226  vpPoseVector position;
227  float position_[7];
228  vpTranslationVector translation;
229  vpQuaternionVector quaternion;
230 
231  if (virtGetBaseFrame(m_virtContext, position_)) {
232  int err = virtGetErrorCode(m_virtContext);
233  throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err));
234  }
235  else {
236  for (int i = 0; i < 3; i++)
237  translation[i] = position_[i];
238  for (int i = 0; i < 4; i++)
239  quaternion[i] = position_[3 + i];
240 
241  vpThetaUVector thetau(quaternion);
242 
243  position.build(translation, thetau);
244 
245  return position;
246  }
247 }
248 
252 VirtCommandType vpVirtuose::getCommandType() const
253 {
254  if (!m_is_init) {
255  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
256  }
257 
258  VirtCommandType type;
259 
260  if (virtGetCommandType(m_virtContext, &type)) {
261  int err = virtGetErrorCode(m_virtContext);
262  throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err));
263  }
264  return type;
265 }
266 
272 {
273  if (!m_is_init) {
274  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
275  }
276 
277  int deadman;
278  if (virtGetDeadMan(m_virtContext, &deadman)) {
279  int err = virtGetErrorCode(m_virtContext);
280  throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err));
281  }
282  return (deadman ? true : false);
283 }
284 
291 {
292  if (!m_is_init) {
293  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
294  }
295 
296  int emergencyStop;
297  if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {
298  int err = virtGetErrorCode(m_virtContext);
299  throw(vpException(vpException::fatalError, "Error calling virtGetEmergencyStop: error code %d", err));
300  }
301  return (emergencyStop ? true : false);
302 }
303 
309 {
310  if (!m_is_init) {
311  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
312  }
313 
314  vpColVector force(6, 0);
315  float force_[6];
316  if (virtGetForce(m_virtContext, force_)) {
317  int err = virtGetErrorCode(m_virtContext);
318  throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err));
319  }
320 
321  for (unsigned int i = 0; i < 6; i++)
322  force[i] = force_[i];
323  return force;
324 }
325 
358 VirtContext vpVirtuose::getHandler() { return m_virtContext; }
359 
365 unsigned int vpVirtuose::getJointsNumber() const
366 {
367  if (!m_is_init) {
368  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
369  }
370 
371  return m_njoints;
372 }
373 
381 {
382  if (!m_is_init) {
383  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
384  }
385 
386  vpPoseVector position;
387  float position_[7];
388  vpTranslationVector translation;
389  vpQuaternionVector quaternion;
390 
391  if (virtGetObservationFrame(m_virtContext, position_)) {
392  int err = virtGetErrorCode(m_virtContext);
393  throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err));
394  }
395  else {
396  for (int i = 0; i < 3; i++)
397  translation[i] = position_[i];
398  for (int i = 0; i < 4; i++)
399  quaternion[i] = position_[3 + i];
400 
401  vpThetaUVector thetau(quaternion);
402 
403  position.build(translation, thetau);
404  }
405  return position;
406 }
407 
415 {
416  if (!m_is_init) {
417  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
418  }
419 
420  vpPoseVector position;
421  float position_[7];
422  vpTranslationVector translation;
423  vpQuaternionVector quaternion;
424 
425  if (virtGetPhysicalPosition(m_virtContext, position_)) {
426  int err = virtGetErrorCode(m_virtContext);
427  throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalPosition: error code %d", err));
428  }
429  else {
430  for (int i = 0; i < 3; i++)
431  translation[i] = position_[i];
432  for (int i = 0; i < 4; i++)
433  quaternion[i] = position_[3 + i];
434 
435  vpThetaUVector thetau(quaternion);
436 
437  position.build(translation, thetau);
438  }
439  return position;
440 }
441 
449 {
450  if (!m_is_init) {
451  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
452  }
453 
454  vpColVector vel(6, 0);
455  float speed[6];
456  if (virtGetPhysicalSpeed(m_virtContext, speed)) {
457  int err = virtGetErrorCode(m_virtContext);
458  throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s",
459  virtGetErrorMessage(err)));
460  }
461  for (unsigned int i = 0; i < 6; i++)
462  vel[i] = speed[i];
463  return vel;
464 }
465 
472 {
473  if (!m_is_init) {
474  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
475  }
476 
477  vpPoseVector position;
478  float position_[7];
479  vpTranslationVector translation;
480  vpQuaternionVector quaternion;
481 
482  if (virtGetPosition(m_virtContext, position_)) {
483  int err = virtGetErrorCode(m_virtContext);
484  throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err));
485  }
486  else {
487  for (int i = 0; i < 3; i++)
488  translation[i] = position_[i];
489  for (int i = 0; i < 4; i++)
490  quaternion[i] = position_[3 + i];
491 
492  vpThetaUVector thetau(quaternion);
493 
494  position.build(translation, thetau);
495  }
496  return position;
497 }
498 
503 {
504  if (!m_is_init) {
505  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
506  }
507 
508  int power;
509  virtGetPowerOn(m_virtContext, &power);
510  return (power ? true : false);
511 }
512 
520 {
521  if (!m_is_init) {
522  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
523  }
524 
525  vpColVector vel(6, 0);
526  float speed[6];
527  if (virtGetSpeed(m_virtContext, speed)) {
528  int err = virtGetErrorCode(m_virtContext);
529  throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err)));
530  }
531  for (unsigned int i = 0; i < 6; i++)
532  vel[i] = speed[i];
533  return vel;
534 }
535 
542 {
543  if (!m_is_init) {
544  m_virtContext = virtOpen(m_ip_port.c_str());
545 
546  if (m_virtContext == nullptr) {
547  int err = virtGetErrorCode(m_virtContext);
549  "Cannot open communication with haptic device using %s: %s. Check ip and port values",
550  m_ip_port.c_str(), virtGetErrorMessage(err)));
551  }
552 
553  if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {
554  int err = virtGetErrorCode(m_virtContext);
555  throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",
556  virtGetErrorMessage(err)));
557  }
558 
559  if (m_verbose) {
560  std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;
561  }
562 
563  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
564  int err = virtGetErrorCode(m_virtContext);
565  throw(
566  vpException(vpException::fatalError, "Cannot set haptic device command type: %s", virtGetErrorMessage(err)));
567  }
568 
569  if (virtSetTimeStep(m_virtContext, m_period)) {
570  int err = virtGetErrorCode(m_virtContext);
571  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
572  }
573 
574  // Update number of joints
575  float articular_position_[20] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
576 
577  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
578  int err = virtGetErrorCode(m_virtContext);
579  throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition() in int(): error code %d",
580  err));
581  }
582 
583  m_njoints = 6; // At least 6 joints
584  for (unsigned int i = m_njoints; i < 20; i++) {
585  m_njoints = i;
586  if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
587  break;
588  }
589  }
590 
591  m_is_init = true;
592  }
593 }
594 
602 void vpVirtuose::setArticularForce(const vpColVector &articularForce)
603 {
604  init();
605 
606  if (articularForce.size() != m_njoints) {
608  "Cannot apply an articular force feedback (dim %d) to "
609  "the haptic device that is not 6-dimension",
610  articularForce.size()));
611  }
612 
613  float *articular_force = new float[m_njoints];
614  for (unsigned int i = 0; i < m_njoints; i++)
615  articular_force[i] = (float)articularForce[i];
616 
617  if (virtSetArticularForce(m_virtContext, articular_force)) {
618  delete[] articular_force;
619  int err = virtGetErrorCode(m_virtContext);
620  throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err));
621  }
622 
623  delete[] articular_force;
624 }
625 
633 void vpVirtuose::setArticularPosition(const vpColVector &articularPosition)
634 {
635  init();
636 
637  if (articularPosition.size() != m_njoints) {
639  "Cannot send an articular position command (dim %d) to "
640  "the haptic device that is not %d-dimension",
641  m_njoints, articularPosition.size()));
642  }
643 
644  float *articular_position = new float[m_njoints];
645  for (unsigned int i = 0; i < m_njoints; i++)
646  articular_position[i] = (float)articularPosition[i];
647 
648  if (virtSetArticularPosition(m_virtContext, articular_position)) {
649  int err = virtGetErrorCode(m_virtContext);
650  delete[] articular_position;
651  throw(vpException(vpException::fatalError, "Error calling virtSetArticularPosition: error code %d", err));
652  }
653  delete[] articular_position;
654 }
655 
663 void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity)
664 {
665  init();
666 
667  if (articularVelocity.size() != m_njoints) {
669  "Cannot send an articular velocity command (dim %d) to "
670  "the haptic device that is not %d-dimension",
671  m_njoints, articularVelocity.size()));
672  }
673 
674  float *articular_velocity = new float[m_njoints];
675  for (unsigned int i = 0; i < m_njoints; i++)
676  articular_velocity[i] = (float)articularVelocity[i];
677 
678  if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
679  int err = virtGetErrorCode(m_virtContext);
680  delete[] articular_velocity;
681  throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err));
682  }
683  delete[] articular_velocity;
684 }
685 
694 {
695  init();
696 
697  float position_[7];
698  vpTranslationVector translation;
699  vpQuaternionVector quaternion;
700 
701  position.extract(translation);
702  position.extract(quaternion);
703 
704  for (int i = 0; i < 3; i++)
705  position_[i] = (float)translation[i];
706  for (int i = 0; i < 4; i++)
707  position_[3 + i] = (float)quaternion[i];
708 
709  if (virtSetBaseFrame(m_virtContext, position_)) {
710  int err = virtGetErrorCode(m_virtContext);
711  throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: error code %d", err));
712  }
713 }
714 
725 void vpVirtuose::setCommandType(const VirtCommandType &type)
726 {
727  init();
728 
729  if (m_typeCommand != type) {
730  m_typeCommand = type;
731 
732  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
733  int err = virtGetErrorCode(m_virtContext);
734  throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err));
735  }
736  }
737 }
738 
746 {
747  init();
748 
749  if (force.size() != 6) {
751  "Cannot apply a force feedback (dim %d) to the haptic "
752  "device that is not 6-dimension",
753  force.size()));
754  }
755 
756  float virtforce[6];
757  for (unsigned int i = 0; i < 6; i++)
758  virtforce[i] = (float)force[i];
759 
760  if (virtSetForce(m_virtContext, virtforce)) {
761  int err = virtGetErrorCode(m_virtContext);
762  throw(vpException(vpException::fatalError, "Error calling virtSetForce: error code %d", err));
763  }
764 }
765 
771 void vpVirtuose::setForceFactor(const float &forceFactor)
772 {
773  init();
774 
775  if (virtSetForceFactor(m_virtContext, forceFactor)) {
776  int err = virtGetErrorCode(m_virtContext);
777  throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err));
778  }
779 }
780 
795 void vpVirtuose::setIndexingMode(const VirtIndexingType &type)
796 {
797  init();
798 
799  if (m_indexType != type) {
800  m_indexType = type;
801 
802  if (virtSetIndexingMode(m_virtContext, m_indexType)) {
803  int err = virtGetErrorCode(m_virtContext);
804  throw(vpException(vpException::fatalError, "Error calling setIndexingMode: error code %d", err));
805  }
806  }
807 }
808 
817 {
818  init();
819 
820  float position_[7];
821  vpTranslationVector translation;
822  vpQuaternionVector quaternion;
823 
824  position.extract(translation);
825  position.extract(quaternion);
826 
827  for (int i = 0; i < 3; i++)
828  position_[i] = (float)translation[i];
829  for (int i = 0; i < 4; i++)
830  position_[3 + i] = (float)quaternion[i];
831 
832  if (virtSetObservationFrame(m_virtContext, position_)) {
833  int err = virtGetErrorCode(m_virtContext);
834  throw(vpException(vpException::fatalError, "Error calling virtSetObservationFrame: error code %d", err));
835  }
836 }
837 
877 void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
878 {
879  init();
880 
881  if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) {
882  int err = virtGetErrorCode(m_virtContext);
883  throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err));
884  }
885 }
886 
893 {
894  init();
895 
896  float position_[7];
897  vpTranslationVector translation;
898  vpQuaternionVector quaternion;
899 
900  position.extract(translation);
901  position.extract(quaternion);
902 
903  for (int i = 0; i < 3; i++)
904  position_[i] = (float)translation[i];
905  for (int i = 0; i < 4; i++)
906  position_[3 + i] = (float)quaternion[i];
907 
908  if (virtSetPosition(m_virtContext, position_)) {
909  int err = virtGetErrorCode(m_virtContext);
910  throw(vpException(vpException::fatalError, "Error calling virtSetPosition: error code %d", err));
911  }
912 }
913 
918 {
919  init();
920 
921  if (virtSetPowerOn(m_virtContext, 0)) {
922  int err = virtGetErrorCode(m_virtContext);
923  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOff: error code %d", err));
924  }
925 }
926 
931 {
932  init();
933 
934  if (virtSetPowerOn(m_virtContext, 1)) {
935  int err = virtGetErrorCode(m_virtContext);
936  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err));
937  }
938 }
939 
945 void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit)
946 {
947  init();
948 
949  if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) {
950  int err = virtGetErrorCode(m_virtContext);
951  throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err));
952  }
953 }
954 
960 void vpVirtuose::setTimeStep(const float &timeStep)
961 {
962  init();
963 
964  if (m_period != timeStep) {
965  m_period = timeStep;
966 
967  if (virtSetTimeStep(m_virtContext, m_period)) {
968  int err = virtGetErrorCode(m_virtContext);
969  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
970  }
971  }
972 }
973 
980 {
981  init();
982 
983  if (velocity.size() != 6) {
984  throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension",
985  velocity.size()));
986  }
987 
988  float speed[6];
989  for (unsigned int i = 0; i < 6; i++)
990  speed[i] = (float)velocity[i];
991 
992  if (virtSetSpeed(m_virtContext, speed)) {
993  int err = virtGetErrorCode(m_virtContext);
994  throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err));
995  }
996 }
997 
1003 void vpVirtuose::setVelocityFactor(const float &velocityFactor)
1004 {
1005  init();
1006 
1007  if (virtSetSpeedFactor(m_virtContext, velocityFactor)) {
1008  int err = virtGetErrorCode(m_virtContext);
1009  throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err));
1010  }
1011 }
1012 
1019 {
1020  init();
1021 
1022  if (virtStartLoop(m_virtContext)) {
1023  int err = virtGetErrorCode(m_virtContext);
1024  throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err));
1025  }
1026  else
1027  std::cout << "Haptic loop open." << std::endl;
1028 }
1029 
1036 {
1037  init();
1038 
1039  if (virtStopLoop(m_virtContext)) {
1040  int err = virtGetErrorCode(m_virtContext);
1041  throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err));
1042  }
1043  else
1044  std::cout << "Haptic loop closed." << std::endl;
1045 }
1047 #else
1048 // Work around to avoid warning
1049 void dummy_vpVirtuose() { };
1050 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:331
Implementation of column vector and the associated operations.
Definition: vpColVector.h:171
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:187
vpPoseVector & build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz)
void extract(vpRotationMatrix &R) const
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
Definition: vpVirtuose.cpp:271
vpColVector getVelocity() const
Definition: vpVirtuose.cpp:519
vpColVector getArticularPosition() const
Definition: vpVirtuose.cpp:134
VirtCommandType getCommandType() const
Definition: vpVirtuose.cpp:252
void setIpAddressAndPort(const std::string &ip, int port)
Definition: vpVirtuose.cpp:81
void init()
Definition: vpVirtuose.cpp:541
void setPowerOff()
Definition: vpVirtuose.cpp:917
void setTimeStep(const float &timeStep)
Definition: vpVirtuose.cpp:960
void setForceFactor(const float &forceFactor)
Definition: vpVirtuose.cpp:771
void setArticularVelocity(const vpColVector &articularVelocity)
Definition: vpVirtuose.cpp:663
void setObservationFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:816
void enableForceFeedback(int enable)
Definition: vpVirtuose.cpp:121
vpPoseVector getPosition() const
Definition: vpVirtuose.cpp:471
bool getPower() const
Definition: vpVirtuose.cpp:502
vpPoseVector getBaseFrame() const
Definition: vpVirtuose.cpp:220
bool m_verbose
Definition: vpVirtuose.h:219
VirtContext getHandler()
Definition: vpVirtuose.cpp:358
int m_apiMajorVersion
Definition: vpVirtuose.h:220
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:745
void setIndexingMode(const VirtIndexingType &type)
Definition: vpVirtuose.cpp:795
unsigned int getJointsNumber() const
Definition: vpVirtuose.cpp:365
void close()
Definition: vpVirtuose.cpp:63
int m_ctrlMinorVersion
Definition: vpVirtuose.h:223
vpPoseVector getPhysicalPosition() const
Definition: vpVirtuose.cpp:414
unsigned int m_njoints
Definition: vpVirtuose.h:228
float m_period
Definition: vpVirtuose.h:227
bool m_is_init
Definition: vpVirtuose.h:226
std::string m_ip_port
Definition: vpVirtuose.h:218
void setBaseFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:693
VirtContext m_virtContext
Definition: vpVirtuose.h:217
vpPoseVector getObservationFrame() const
Definition: vpVirtuose.cpp:380
bool getEmergencyStop() const
Definition: vpVirtuose.cpp:290
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:725
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
Definition: vpVirtuose.cpp:877
vpColVector getArticularVelocity() const
Definition: vpVirtuose.cpp:157
void stopPeriodicFunction()
int m_ctrlMajorVersion
Definition: vpVirtuose.h:222
vpPoseVector getAvatarPosition() const
Definition: vpVirtuose.cpp:185
void setPowerOn()
Definition: vpVirtuose.cpp:930
void addForce(vpColVector &force)
Definition: vpVirtuose.cpp:96
void setArticularForce(const vpColVector &articularForce)
Definition: vpVirtuose.cpp:602
int m_apiMinorVersion
Definition: vpVirtuose.h:221
void setPosition(vpPoseVector &position)
Definition: vpVirtuose.cpp:892
void setVelocity(vpColVector &velocity)
Definition: vpVirtuose.cpp:979
VirtCommandType m_typeCommand
Definition: vpVirtuose.h:224
void startPeriodicFunction()
void setVelocityFactor(const float &velocityFactor)
void setSaturation(const float &forceLimit, const float &torqueLimit)
Definition: vpVirtuose.cpp:945
virtual ~vpVirtuose()
Definition: vpVirtuose.cpp:74
vpColVector getPhysicalVelocity() const
Definition: vpVirtuose.cpp:448
void setArticularPosition(const vpColVector &articularPosition)
Definition: vpVirtuose.cpp:633
VirtIndexingType m_indexType
Definition: vpVirtuose.h:225
vpColVector getForce() const
Definition: vpVirtuose.cpp:308