Visual Servoing Platform  version 3.5.1 under development (2023-03-14)
vpVirtuose.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpException.h>
45 #include <visp3/robot/vpVirtuose.h>
46 
47 #ifdef VISP_HAVE_VIRTUOSE
48 
55  : m_virtContext(NULL), m_ip_port("localhost#5000"), m_verbose(false), m_apiMajorVersion(0), m_apiMinorVersion(0),
56  m_ctrlMajorVersion(0), m_ctrlMinorVersion(0), m_typeCommand(COMMAND_TYPE_IMPEDANCE), m_indexType(INDEXING_ALL),
57  m_is_init(false), m_period(0.001f), m_njoints(6)
58 {
59  virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion);
60  std::cout << "API version: " << m_apiMajorVersion << "." << m_apiMinorVersion << std::endl;
61 }
62 
67 {
68  if (m_virtContext != NULL) {
69  virtClose(m_virtContext);
70  m_virtContext = NULL;
71  }
72 }
73 
78 
84 void vpVirtuose::setIpAddressAndPort(const std::string &ip, int port)
85 {
86  std::stringstream ss;
87  ss << ip << "#" << port;
88 
89  m_ip_port = ss.str();
90 }
91 
100 {
101  if (force.size() != 6) {
103  "Cannot apply a force feedback (dim %d) to the haptic "
104  "device that is not 6-dimension",
105  force.size()));
106  }
107 
108  init();
109 
110  float virtforce[6];
111  for (unsigned int i = 0; i < 6; i++)
112  virtforce[i] = (float)force[i];
113 
114  if (virtAddForce(m_virtContext, virtforce)) {
115  int err = virtGetErrorCode(m_virtContext);
116  throw(vpException(vpException::fatalError, "Error calling virtAddForce: error code %d", err));
117  }
118 }
119 
125 {
126  init();
127 
128  if (virtEnableForceFeedback(m_virtContext, enable)) {
129  int err = virtGetErrorCode(m_virtContext);
130  throw(vpException(vpException::fatalError, "Error calling virtEnableForceFeedback(): error code %d", err));
131  }
132 }
133 
138 {
139  if (!m_is_init) {
140  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
141  }
142 
143  float articular_position_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
144 
145  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
146  int err = virtGetErrorCode(m_virtContext);
147  throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition(): error code %d", err));
148  }
149 
150  vpColVector articularPosition(m_njoints, 0);
151  for (unsigned int i = 0; i < m_njoints; i++)
152  articularPosition[i] = articular_position_[i];
153 
154  return articularPosition;
155 }
156 
161 {
162  if (!m_is_init) {
163  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
164  }
165 
166  float articular_velocity_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
167 
168  if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) {
169  int err = virtGetErrorCode(m_virtContext);
170  throw(vpException(vpException::fatalError, "Error calling virtGetArticularSpeed: error code %d", err));
171  }
172 
173  vpColVector articularVelocity(m_njoints, 0);
174 
175  for (unsigned int i = 0; i < m_njoints; i++)
176  articularVelocity[i] = articular_velocity_[i];
177 
178  return articularVelocity;
179 }
180 
189 {
190  if (!m_is_init) {
191  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
192  }
193 
194  float position_[7];
195  vpPoseVector position;
196  vpTranslationVector translation;
197  vpQuaternionVector quaternion;
198 
199  if (virtGetAvatarPosition(m_virtContext, position_)) {
200  int err = virtGetErrorCode(m_virtContext);
201  throw(vpException(vpException::fatalError, "Error calling virtGetAvatarPosition: error code %d", err));
202  } else {
203  for (int i = 0; i < 3; i++)
204  translation[i] = position_[i];
205  for (int i = 0; i < 4; i++)
206  quaternion[i] = position_[3 + i];
207 
208  vpThetaUVector thetau(quaternion);
209 
210  position.buildFrom(translation, thetau);
211 
212  return position;
213  }
214 }
215 
223 {
224  if (!m_is_init) {
225  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
226  }
227 
228  vpPoseVector position;
229  float position_[7];
230  vpTranslationVector translation;
231  vpQuaternionVector quaternion;
232 
233  if (virtGetBaseFrame(m_virtContext, position_)) {
234  int err = virtGetErrorCode(m_virtContext);
235  throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err));
236  } else {
237  for (int i = 0; i < 3; i++)
238  translation[i] = position_[i];
239  for (int i = 0; i < 4; i++)
240  quaternion[i] = position_[3 + i];
241 
242  vpThetaUVector thetau(quaternion);
243 
244  position.buildFrom(translation, thetau);
245 
246  return position;
247  }
248 }
249 
253 VirtCommandType vpVirtuose::getCommandType() const
254 {
255  if (!m_is_init) {
256  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
257  }
258 
259  VirtCommandType type;
260 
261  if (virtGetCommandType(m_virtContext, &type)) {
262  int err = virtGetErrorCode(m_virtContext);
263  throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err));
264  }
265  return type;
266 }
267 
273 {
274  if (!m_is_init) {
275  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
276  }
277 
278  int deadman;
279  if (virtGetDeadMan(m_virtContext, &deadman)) {
280  int err = virtGetErrorCode(m_virtContext);
281  throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err));
282  }
283  return (deadman ? true : false);
284 }
285 
292 {
293  if (!m_is_init) {
294  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
295  }
296 
297  int emergencyStop;
298  if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {
299  int err = virtGetErrorCode(m_virtContext);
300  throw(vpException(vpException::fatalError, "Error calling virtGetEmergencyStop: error code %d", err));
301  }
302  return (emergencyStop ? true : false);
303 }
304 
310 {
311  if (!m_is_init) {
312  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
313  }
314 
315  vpColVector force(6, 0);
316  float force_[6];
317  if (virtGetForce(m_virtContext, force_)) {
318  int err = virtGetErrorCode(m_virtContext);
319  throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err));
320  }
321 
322  for (unsigned int i = 0; i < 6; i++)
323  force[i] = force_[i];
324  return force;
325 }
326 
359 VirtContext vpVirtuose::getHandler() { return m_virtContext; }
360 
366 unsigned int vpVirtuose::getJointsNumber() const
367 {
368  if (!m_is_init) {
369  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
370  }
371 
372  return m_njoints;
373 }
374 
382 {
383  if (!m_is_init) {
384  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
385  }
386 
387  vpPoseVector position;
388  float position_[7];
389  vpTranslationVector translation;
390  vpQuaternionVector quaternion;
391 
392  if (virtGetObservationFrame(m_virtContext, position_)) {
393  int err = virtGetErrorCode(m_virtContext);
394  throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err));
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.buildFrom(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  } else {
429  for (int i = 0; i < 3; i++)
430  translation[i] = position_[i];
431  for (int i = 0; i < 4; i++)
432  quaternion[i] = position_[3 + i];
433 
434  vpThetaUVector thetau(quaternion);
435 
436  position.buildFrom(translation, thetau);
437  }
438  return position;
439 }
440 
448 {
449  if (!m_is_init) {
450  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
451  }
452 
453  vpColVector vel(6, 0);
454  float speed[6];
455  if (virtGetPhysicalSpeed(m_virtContext, speed)) {
456  int err = virtGetErrorCode(m_virtContext);
457  throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s",
458  virtGetErrorMessage(err)));
459  }
460  for (unsigned int i = 0; i < 6; i++)
461  vel[i] = speed[i];
462  return vel;
463 }
464 
471 {
472  if (!m_is_init) {
473  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
474  }
475 
476  vpPoseVector position;
477  float position_[7];
478  vpTranslationVector translation;
479  vpQuaternionVector quaternion;
480 
481  if (virtGetPosition(m_virtContext, position_)) {
482  int err = virtGetErrorCode(m_virtContext);
483  throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err));
484  } else {
485  for (int i = 0; i < 3; i++)
486  translation[i] = position_[i];
487  for (int i = 0; i < 4; i++)
488  quaternion[i] = position_[3 + i];
489 
490  vpThetaUVector thetau(quaternion);
491 
492  position.buildFrom(translation, thetau);
493  }
494  return position;
495 }
496 
501 {
502  if (!m_is_init) {
503  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
504  }
505 
506  int power;
507  virtGetPowerOn(m_virtContext, &power);
508  return (power ? true : false);
509 }
510 
518 {
519  if (!m_is_init) {
520  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
521  }
522 
523  vpColVector vel(6, 0);
524  float speed[6];
525  if (virtGetSpeed(m_virtContext, speed)) {
526  int err = virtGetErrorCode(m_virtContext);
527  throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err)));
528  }
529  for (unsigned int i = 0; i < 6; i++)
530  vel[i] = speed[i];
531  return vel;
532 }
533 
540 {
541  if (!m_is_init) {
542  m_virtContext = virtOpen(m_ip_port.c_str());
543 
544  if (m_virtContext == NULL) {
545  int err = virtGetErrorCode(m_virtContext);
547  "Cannot open communication with haptic device using %s: %s. Check ip and port values",
548  m_ip_port.c_str(), virtGetErrorMessage(err)));
549  }
550 
551  if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {
552  int err = virtGetErrorCode(m_virtContext);
553  throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",
554  virtGetErrorMessage(err)));
555  }
556 
557  if (m_verbose) {
558  std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;
559  }
560 
561  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
562  int err = virtGetErrorCode(m_virtContext);
563  throw(
564  vpException(vpException::fatalError, "Cannot set haptic device command type: %s", virtGetErrorMessage(err)));
565  }
566 
567  if (virtSetTimeStep(m_virtContext, m_period)) {
568  int err = virtGetErrorCode(m_virtContext);
569  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
570  }
571 
572  // Update number of joints
573  float articular_position_[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
574 
575  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
576  int err = virtGetErrorCode(m_virtContext);
577  throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition() in int(): error code %d",
578  err));
579  }
580 
581  m_njoints = 6; // At least 6 joints
582  for (unsigned int i = m_njoints; i < 20; i++) {
583  m_njoints = i;
584  if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
585  break;
586  }
587  }
588 
589  m_is_init = true;
590  }
591 }
592 
600 void vpVirtuose::setArticularForce(const vpColVector &articularForce)
601 {
602  init();
603 
604  if (articularForce.size() != m_njoints) {
606  "Cannot apply an articular force feedback (dim %d) to "
607  "the haptic device that is not 6-dimension",
608  articularForce.size()));
609  }
610 
611  float *articular_force = new float[m_njoints];
612  for (unsigned int i = 0; i < m_njoints; i++)
613  articular_force[i] = (float)articularForce[i];
614 
615  if (virtSetArticularForce(m_virtContext, articular_force)) {
616  delete[] articular_force;
617  int err = virtGetErrorCode(m_virtContext);
618  throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err));
619  }
620 
621  delete[] articular_force;
622 }
623 
631 void vpVirtuose::setArticularPosition(const vpColVector &articularPosition)
632 {
633  init();
634 
635  if (articularPosition.size() != m_njoints) {
637  "Cannot send an articular position command (dim %d) to "
638  "the haptic device that is not %d-dimension",
639  m_njoints, articularPosition.size()));
640  }
641 
642  float *articular_position = new float[m_njoints];
643  for (unsigned int i = 0; i < m_njoints; i++)
644  articular_position[i] = (float)articularPosition[i];
645 
646  if (virtSetArticularPosition(m_virtContext, articular_position)) {
647  int err = virtGetErrorCode(m_virtContext);
648  delete[] articular_position;
649  throw(vpException(vpException::fatalError, "Error calling virtSetArticularPosition: error code %d", err));
650  }
651  delete[] articular_position;
652 }
653 
661 void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity)
662 {
663  init();
664 
665  if (articularVelocity.size() != m_njoints) {
667  "Cannot send an articular velocity command (dim %d) to "
668  "the haptic device that is not %d-dimension",
669  m_njoints, articularVelocity.size()));
670  }
671 
672  float *articular_velocity = new float[m_njoints];
673  for (unsigned int i = 0; i < m_njoints; i++)
674  articular_velocity[i] = (float)articularVelocity[i];
675 
676  if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
677  int err = virtGetErrorCode(m_virtContext);
678  delete[] articular_velocity;
679  throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err));
680  }
681  delete[] articular_velocity;
682 }
683 
692 {
693  init();
694 
695  float position_[7];
696  vpTranslationVector translation;
697  vpQuaternionVector quaternion;
698 
699  position.extract(translation);
700  position.extract(quaternion);
701 
702  for (int i = 0; i < 3; i++)
703  position_[i] = (float)translation[i];
704  for (int i = 0; i < 4; i++)
705  position_[3 + i] = (float)quaternion[i];
706 
707  if (virtSetBaseFrame(m_virtContext, position_)) {
708  int err = virtGetErrorCode(m_virtContext);
709  throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: error code %d", err));
710  }
711 }
712 
723 void vpVirtuose::setCommandType(const VirtCommandType &type)
724 {
725  init();
726 
727  if (m_typeCommand != type) {
728  m_typeCommand = type;
729 
730  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
731  int err = virtGetErrorCode(m_virtContext);
732  throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err));
733  }
734  }
735 }
736 
744 {
745  init();
746 
747  if (force.size() != 6) {
749  "Cannot apply a force feedback (dim %d) to the haptic "
750  "device that is not 6-dimension",
751  force.size()));
752  }
753 
754  float virtforce[6];
755  for (unsigned int i = 0; i < 6; i++)
756  virtforce[i] = (float)force[i];
757 
758  if (virtSetForce(m_virtContext, virtforce)) {
759  int err = virtGetErrorCode(m_virtContext);
760  throw(vpException(vpException::fatalError, "Error calling virtSetForce: error code %d", err));
761  }
762 }
763 
769 void vpVirtuose::setForceFactor(const float &forceFactor)
770 {
771  init();
772 
773  if (virtSetForceFactor(m_virtContext, forceFactor)) {
774  int err = virtGetErrorCode(m_virtContext);
775  throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err));
776  }
777 }
778 
793 void vpVirtuose::setIndexingMode(const VirtIndexingType &type)
794 {
795  init();
796 
797  if (m_indexType != type) {
798  m_indexType = type;
799 
800  if (virtSetIndexingMode(m_virtContext, m_indexType)) {
801  int err = virtGetErrorCode(m_virtContext);
802  throw(vpException(vpException::fatalError, "Error calling setIndexingMode: error code %d", err));
803  }
804  }
805 }
806 
815 {
816  init();
817 
818  float position_[7];
819  vpTranslationVector translation;
820  vpQuaternionVector quaternion;
821 
822  position.extract(translation);
823  position.extract(quaternion);
824 
825  for (int i = 0; i < 3; i++)
826  position_[i] = (float)translation[i];
827  for (int i = 0; i < 4; i++)
828  position_[3 + i] = (float)quaternion[i];
829 
830  if (virtSetObservationFrame(m_virtContext, position_)) {
831  int err = virtGetErrorCode(m_virtContext);
832  throw(vpException(vpException::fatalError, "Error calling virtSetObservationFrame: error code %d", err));
833  }
834 }
835 
875 void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
876 {
877  init();
878 
879  if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) {
880  int err = virtGetErrorCode(m_virtContext);
881  throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err));
882  }
883 }
884 
891 {
892  init();
893 
894  float position_[7];
895  vpTranslationVector translation;
896  vpQuaternionVector quaternion;
897 
898  position.extract(translation);
899  position.extract(quaternion);
900 
901  for (int i = 0; i < 3; i++)
902  position_[i] = (float)translation[i];
903  for (int i = 0; i < 4; i++)
904  position_[3 + i] = (float)quaternion[i];
905 
906  if (virtSetPosition(m_virtContext, position_)) {
907  int err = virtGetErrorCode(m_virtContext);
908  throw(vpException(vpException::fatalError, "Error calling virtSetPosition: error code %d", err));
909  }
910 }
911 
916 {
917  init();
918 
919  if (virtSetPowerOn(m_virtContext, 0)) {
920  int err = virtGetErrorCode(m_virtContext);
921  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOff: error code %d", err));
922  }
923 }
924 
929 {
930  init();
931 
932  if (virtSetPowerOn(m_virtContext, 1)) {
933  int err = virtGetErrorCode(m_virtContext);
934  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err));
935  }
936 }
937 
943 void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit)
944 {
945  init();
946 
947  if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) {
948  int err = virtGetErrorCode(m_virtContext);
949  throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err));
950  }
951 }
952 
958 void vpVirtuose::setTimeStep(const float &timeStep)
959 {
960  init();
961 
962  if (m_period != timeStep) {
963  m_period = timeStep;
964 
965  if (virtSetTimeStep(m_virtContext, m_period)) {
966  int err = virtGetErrorCode(m_virtContext);
967  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
968  }
969  }
970 }
971 
978 {
979  init();
980 
981  if (velocity.size() != 6) {
982  throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension",
983  velocity.size()));
984  }
985 
986  float speed[6];
987  for (unsigned int i = 0; i < 6; i++)
988  speed[i] = (float)velocity[i];
989 
990  if (virtSetSpeed(m_virtContext, speed)) {
991  int err = virtGetErrorCode(m_virtContext);
992  throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err));
993  }
994 }
995 
1001 void vpVirtuose::setVelocityFactor(const float &velocityFactor)
1002 {
1003  init();
1004 
1005  if (virtSetSpeedFactor(m_virtContext, velocityFactor)) {
1006  int err = virtGetErrorCode(m_virtContext);
1007  throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err));
1008  }
1009 }
1010 
1017 {
1018  init();
1019 
1020  if (virtStartLoop(m_virtContext)) {
1021  int err = virtGetErrorCode(m_virtContext);
1022  throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err));
1023  } else
1024  std::cout << "Haptic loop open." << std::endl;
1025 }
1026 
1033 {
1034  init();
1035 
1036  if (virtStopLoop(m_virtContext)) {
1037  int err = virtGetErrorCode(m_virtContext);
1038  throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err));
1039  } else
1040  std::cout << "Haptic loop closed." << std::endl;
1041 }
1042 
1043 #else
1044 // Work around to avoid warning
1045 void dummy_vpVirtuose(){};
1046 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:290
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
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
Definition: vpVirtuose.cpp:272
vpColVector getVelocity() const
Definition: vpVirtuose.cpp:517
vpColVector getArticularPosition() const
Definition: vpVirtuose.cpp:137
VirtCommandType getCommandType() const
Definition: vpVirtuose.cpp:253
void setIpAddressAndPort(const std::string &ip, int port)
Definition: vpVirtuose.cpp:84
void init()
Definition: vpVirtuose.cpp:539
void setPowerOff()
Definition: vpVirtuose.cpp:915
void setTimeStep(const float &timeStep)
Definition: vpVirtuose.cpp:958
void setForceFactor(const float &forceFactor)
Definition: vpVirtuose.cpp:769
void setArticularVelocity(const vpColVector &articularVelocity)
Definition: vpVirtuose.cpp:661
void setObservationFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:814
void enableForceFeedback(int enable)
Definition: vpVirtuose.cpp:124
vpPoseVector getPosition() const
Definition: vpVirtuose.cpp:470
bool getPower() const
Definition: vpVirtuose.cpp:500
vpPoseVector getBaseFrame() const
Definition: vpVirtuose.cpp:222
bool m_verbose
Definition: vpVirtuose.h:220
VirtContext getHandler()
Definition: vpVirtuose.cpp:359
int m_apiMajorVersion
Definition: vpVirtuose.h:221
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:743
void setIndexingMode(const VirtIndexingType &type)
Definition: vpVirtuose.cpp:793
unsigned int getJointsNumber() const
Definition: vpVirtuose.cpp:366
void close()
Definition: vpVirtuose.cpp:66
int m_ctrlMinorVersion
Definition: vpVirtuose.h:224
vpPoseVector getPhysicalPosition() const
Definition: vpVirtuose.cpp:414
unsigned int m_njoints
Definition: vpVirtuose.h:229
float m_period
Definition: vpVirtuose.h:228
bool m_is_init
Definition: vpVirtuose.h:227
std::string m_ip_port
Definition: vpVirtuose.h:219
void setBaseFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:691
VirtContext m_virtContext
Definition: vpVirtuose.h:218
vpPoseVector getObservationFrame() const
Definition: vpVirtuose.cpp:381
bool getEmergencyStop() const
Definition: vpVirtuose.cpp:291
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:723
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
Definition: vpVirtuose.cpp:875
vpColVector getArticularVelocity() const
Definition: vpVirtuose.cpp:160
void stopPeriodicFunction()
int m_ctrlMajorVersion
Definition: vpVirtuose.h:223
vpPoseVector getAvatarPosition() const
Definition: vpVirtuose.cpp:188
void setPowerOn()
Definition: vpVirtuose.cpp:928
void addForce(vpColVector &force)
Definition: vpVirtuose.cpp:99
void setArticularForce(const vpColVector &articularForce)
Definition: vpVirtuose.cpp:600
int m_apiMinorVersion
Definition: vpVirtuose.h:222
void setPosition(vpPoseVector &position)
Definition: vpVirtuose.cpp:890
void setVelocity(vpColVector &velocity)
Definition: vpVirtuose.cpp:977
VirtCommandType m_typeCommand
Definition: vpVirtuose.h:225
void startPeriodicFunction()
void setVelocityFactor(const float &velocityFactor)
void setSaturation(const float &forceLimit, const float &torqueLimit)
Definition: vpVirtuose.cpp:943
virtual ~vpVirtuose()
Definition: vpVirtuose.cpp:77
vpColVector getPhysicalVelocity() const
Definition: vpVirtuose.cpp:447
void setArticularPosition(const vpColVector &articularPosition)
Definition: vpVirtuose.cpp:631
VirtIndexingType m_indexType
Definition: vpVirtuose.h:226
vpColVector getForce() const
Definition: vpVirtuose.cpp:309