Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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("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 {
79  close();
80 }
81 
90 {
91  if (force.size() != 6) {
93  "Cannot apply a force feedback (dim %d) to the haptic "
94  "device that is not 6-dimension",
95  force.size()));
96  }
97 
98  init();
99 
100  float virtforce[6];
101  for (unsigned int i = 0; i < 6; i++)
102  virtforce[i] = (float)force[i];
103 
104  if (virtAddForce(m_virtContext, virtforce)) {
105  int err = virtGetErrorCode(m_virtContext);
106  throw(vpException(vpException::fatalError, "Error calling virtAddForce: error code %d", err));
107  }
108 }
109 
115 {
116  init();
117 
118  if (virtEnableForceFeedback(m_virtContext, enable)) {
119  int err = virtGetErrorCode(m_virtContext);
120  throw(vpException(vpException::fatalError, "Error calling virtEnableForceFeedback(): error code %d", err));
121  }
122 }
123 
128 {
129  if (!m_is_init) {
130  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
131  }
132 
133 
134  float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
135 
136  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
137  int err = virtGetErrorCode(m_virtContext);
138  throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition(): error code %d", err));
139  }
140 
141  vpColVector articularPosition(m_njoints, 0);
142  for (unsigned int i = 0; i < m_njoints; i++)
143  articularPosition[i] = articular_position_[i];
144 
145  return articularPosition;
146 }
147 
152 {
153  if (!m_is_init) {
154  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
155  }
156 
157  float articular_velocity_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
158 
159  if (virtGetArticularSpeed(m_virtContext, articular_velocity_)) {
160  int err = virtGetErrorCode(m_virtContext);
161  throw(vpException(vpException::fatalError, "Error calling virtGetArticularSpeed: error code %d", err));
162  }
163 
164  vpColVector articularVelocity(m_njoints, 0);
165 
166  for (unsigned int i = 0; i < m_njoints; i++)
167  articularVelocity[i] = articular_velocity_[i];
168 
169  return articularVelocity;
170 }
171 
180 {
181  if (!m_is_init) {
182  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
183  }
184 
185  float position_[7];
186  vpPoseVector position;
187  vpTranslationVector translation;
188  vpQuaternionVector quaternion;
189 
190  if (virtGetAvatarPosition(m_virtContext, position_)) {
191  int err = virtGetErrorCode(m_virtContext);
192  throw(vpException(vpException::fatalError, "Error calling virtGetAvatarPosition: error code %d", err));
193  } else {
194  for (int i = 0; i < 3; i++)
195  translation[i] = position_[i];
196  for (int i = 0; i < 4; i++)
197  quaternion[i] = position_[3 + i];
198 
199  vpThetaUVector thetau(quaternion);
200 
201  position.buildFrom(translation, thetau);
202 
203  return position;
204  }
205 }
206 
214 {
215  if (!m_is_init) {
216  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
217  }
218 
219  vpPoseVector position;
220  float position_[7];
221  vpTranslationVector translation;
222  vpQuaternionVector quaternion;
223 
224  if (virtGetBaseFrame(m_virtContext, position_)) {
225  int err = virtGetErrorCode(m_virtContext);
226  throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err));
227  } else {
228  for (int i = 0; i < 3; i++)
229  translation[i] = position_[i];
230  for (int i = 0; i < 4; i++)
231  quaternion[i] = position_[3 + i];
232 
233  vpThetaUVector thetau(quaternion);
234 
235  position.buildFrom(translation, thetau);
236 
237  return position;
238  }
239 }
240 
244 VirtCommandType vpVirtuose::getCommandType() const
245 {
246  if (!m_is_init) {
247  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
248  }
249 
250  VirtCommandType type;
251 
252  if (virtGetCommandType(m_virtContext, &type)) {
253  int err = virtGetErrorCode(m_virtContext);
254  throw(vpException(vpException::fatalError, "Error calling virtGetCommandType: error code %d", err));
255  }
256  return type;
257 }
258 
264 {
265  if (!m_is_init) {
266  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
267  }
268 
269  int deadman;
270  if (virtGetDeadMan(m_virtContext, &deadman)) {
271  int err = virtGetErrorCode(m_virtContext);
272  throw(vpException(vpException::fatalError, "Error calling virtGetDeadMan: error code %d", err));
273  }
274  return (deadman ? true : false);
275 }
276 
283 {
284  if (!m_is_init) {
285  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
286  }
287 
288  int emergencyStop;
289  if (virtGetEmergencyStop(m_virtContext, &emergencyStop)) {
290  int err = virtGetErrorCode(m_virtContext);
291  throw(vpException(vpException::fatalError, "Error calling virtGetEmergencyStop: error code %d", err));
292  }
293  return (emergencyStop ? true : false);
294 }
295 
301 {
302  if (!m_is_init) {
303  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
304  }
305 
306  vpColVector force(6, 0);
307  float force_[6];
308  if (virtGetForce(m_virtContext, force_)) {
309  int err = virtGetErrorCode(m_virtContext);
310  throw(vpException(vpException::fatalError, "Error calling virtGetForce: error code %d", err));
311  }
312 
313  for (unsigned int i = 0; i < 6; i++)
314  force[i] = force_[i];
315  return force;
316 }
317 
350 VirtContext vpVirtuose::getHandler() { return m_virtContext; }
351 
357 unsigned int vpVirtuose::getJointsNumber() const
358 {
359  if (!m_is_init) {
360  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
361  }
362 
363  return m_njoints;
364 }
365 
366 
374 {
375  if (!m_is_init) {
376  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
377  }
378 
379  vpPoseVector position;
380  float position_[7];
381  vpTranslationVector translation;
382  vpQuaternionVector quaternion;
383 
384  if (virtGetObservationFrame(m_virtContext, position_)) {
385  int err = virtGetErrorCode(m_virtContext);
386  throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err));
387  } else {
388  for (int i = 0; i < 3; i++)
389  translation[i] = position_[i];
390  for (int i = 0; i < 4; i++)
391  quaternion[i] = position_[3 + i];
392 
393  vpThetaUVector thetau(quaternion);
394 
395  position.buildFrom(translation, thetau);
396  }
397  return position;
398 }
399 
407 {
408  if (!m_is_init) {
409  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
410  }
411 
412  vpPoseVector position;
413  float position_[7];
414  vpTranslationVector translation;
415  vpQuaternionVector quaternion;
416 
417  if (virtGetPhysicalPosition(m_virtContext, position_)) {
418  int err = virtGetErrorCode(m_virtContext);
419  throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalPosition: error code %d", err));
420  } else {
421  for (int i = 0; i < 3; i++)
422  translation[i] = position_[i];
423  for (int i = 0; i < 4; i++)
424  quaternion[i] = position_[3 + i];
425 
426  vpThetaUVector thetau(quaternion);
427 
428  position.buildFrom(translation, thetau);
429  }
430  return position;
431 }
432 
440 {
441  if (!m_is_init) {
442  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
443  }
444 
445  vpColVector vel(6, 0);
446  float speed[6];
447  if (virtGetPhysicalSpeed(m_virtContext, speed)) {
448  int err = virtGetErrorCode(m_virtContext);
449  throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalSpeed: error code %s",
450  virtGetErrorMessage(err)));
451  }
452  for (unsigned int i = 0; i < 6; i++)
453  vel[i] = speed[i];
454  return vel;
455 }
456 
463 {
464  if (!m_is_init) {
465  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
466  }
467 
468  vpPoseVector position;
469  float position_[7];
470  vpTranslationVector translation;
471  vpQuaternionVector quaternion;
472 
473  if (virtGetPosition(m_virtContext, position_)) {
474  int err = virtGetErrorCode(m_virtContext);
475  throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err));
476  } else {
477  for (int i = 0; i < 3; i++)
478  translation[i] = position_[i];
479  for (int i = 0; i < 4; i++)
480  quaternion[i] = position_[3 + i];
481 
482  vpThetaUVector thetau(quaternion);
483 
484  position.buildFrom(translation, thetau);
485  }
486  return position;
487 }
488 
493 {
494  if (!m_is_init) {
495  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
496  }
497 
498  int power;
499  virtGetPowerOn(m_virtContext, &power);
500  return (power ? true : false);
501 }
502 
510 {
511  if (!m_is_init) {
512  throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
513  }
514 
515  vpColVector vel(6, 0);
516  float speed[6];
517  if (virtGetSpeed(m_virtContext, speed)) {
518  int err = virtGetErrorCode(m_virtContext);
519  throw(vpException(vpException::fatalError, "Cannot get haptic device velocity: %s", virtGetErrorMessage(err)));
520  }
521  for (unsigned int i = 0; i < 6; i++)
522  vel[i] = speed[i];
523  return vel;
524 }
525 
532 {
533  if (!m_is_init) {
534  m_virtContext = virtOpen(m_ip.c_str());
535 
536  if (m_virtContext == NULL) {
537  int err = virtGetErrorCode(m_virtContext);
538  throw(vpException(vpException::fatalError, "Cannot open haptic device: %s", virtGetErrorMessage(err)));
539  }
540 
541  if (virtGetControlerVersion(m_virtContext, &m_ctrlMajorVersion, &m_ctrlMinorVersion)) {
542  int err = virtGetErrorCode(m_virtContext);
543  throw(vpException(vpException::fatalError, "Cannot get haptic device controller version: %s",
544  virtGetErrorMessage(err)));
545  }
546 
547  if (m_verbose) {
548  std::cout << "Controller version: " << m_ctrlMajorVersion << "." << m_ctrlMinorVersion << std::endl;
549  }
550 
551  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
552  int err = virtGetErrorCode(m_virtContext);
553  throw(
554  vpException(vpException::fatalError, "Cannot set haptic device command type: %s", virtGetErrorMessage(err)));
555  }
556 
557  if (virtSetTimeStep(m_virtContext, m_period)) {
558  int err = virtGetErrorCode(m_virtContext);
559  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
560  }
561 
562  // Update number of joints
563  float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
564 
565  if (virtGetArticularPosition(m_virtContext, articular_position_)) {
566  int err = virtGetErrorCode(m_virtContext);
567  throw(vpException(vpException::fatalError, "Error calling virtGetArticularPosition() in int(): error code %d", err));
568  }
569 
570  m_njoints = 6; // At least 6 joints
571  for (unsigned int i=m_njoints; i < 20; i++) {
572  m_njoints = i;
573  if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
574  break;
575  }
576  }
577 
578  m_is_init = true;
579  }
580 }
581 
589 void vpVirtuose::setArticularForce(const vpColVector &articularForce)
590 {
591  init();
592 
593  if (articularForce.size() != 6) {
595  "Cannot apply an articular force feedback (dim %d) to "
596  "the haptic device that is not 6-dimension",
597  articularForce.size()));
598  }
599 
600  float articular_force[6];
601  for (unsigned int i = 0; i < 6; i++)
602  articular_force[i] = (float)articularForce[i];
603 
604  if (virtSetArticularForce(m_virtContext, articular_force)) {
605  int err = virtGetErrorCode(m_virtContext);
606  throw(vpException(vpException::fatalError, "Error calling virtSetArticularForce: error code %d", err));
607  }
608 }
609 
617 void vpVirtuose::setArticularPosition(const vpColVector &articularPosition)
618 {
619  init();
620 
621  if (articularPosition.size() != m_njoints) {
623  "Cannot send an articular position command (dim %d) to "
624  "the haptic device that is not %d-dimension",
625  m_njoints, articularPosition.size()));
626  }
627 
628  float *articular_position = new float[m_njoints];
629  for (unsigned int i = 0; i < m_njoints; i++)
630  articular_position[i] = (float)articularPosition[i];
631 
632  if (virtSetArticularPosition(m_virtContext, articular_position)) {
633  int err = virtGetErrorCode(m_virtContext);
634  delete [] articular_position;
635  throw(vpException(vpException::fatalError, "Error calling virtSetArticularPosition: error code %d", err));
636  }
637  delete[] articular_position;
638 }
639 
647 void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity)
648 {
649  init();
650 
651  if (articularVelocity.size() != m_njoints) {
653  "Cannot send an articular velocity command (dim %d) to "
654  "the haptic device that is not %d-dimension",
655  m_njoints, articularVelocity.size()));
656  }
657 
658  float *articular_velocity = new float [m_njoints];
659  for (unsigned int i = 0; i < m_njoints; i++)
660  articular_velocity[i] = (float)articularVelocity[i];
661 
662  if (virtSetArticularSpeed(m_virtContext, articular_velocity)) {
663  int err = virtGetErrorCode(m_virtContext);
664  delete [] articular_velocity;
665  throw(vpException(vpException::fatalError, "Error calling virtSetArticularVelocity: error code %d", err));
666  }
667  delete[] articular_velocity;
668 }
669 
678 {
679  init();
680 
681  float position_[7];
682  vpTranslationVector translation;
683  vpQuaternionVector quaternion;
684 
685  position.extract(translation);
686  position.extract(quaternion);
687 
688  for (int i = 0; i < 3; i++)
689  position_[i] = (float)translation[i];
690  for (int i = 0; i < 4; i++)
691  position_[3 + i] = (float)quaternion[i];
692 
693  if (virtSetBaseFrame(m_virtContext, position_)) {
694  int err = virtGetErrorCode(m_virtContext);
695  throw(vpException(vpException::fatalError, "Error calling virtSetBaseFrame: error code %d", err));
696  }
697 }
698 
709 void vpVirtuose::setCommandType(const VirtCommandType &type)
710 {
711  init();
712 
713  if (m_typeCommand != type) {
714  m_typeCommand = type;
715 
716  if (virtSetCommandType(m_virtContext, m_typeCommand)) {
717  int err = virtGetErrorCode(m_virtContext);
718  throw(vpException(vpException::fatalError, "Error calling virtSetCommandType: error code %d", err));
719  }
720  }
721 }
722 
730 {
731  init();
732 
733  if (force.size() != 6) {
735  "Cannot apply a force feedback (dim %d) to the haptic "
736  "device that is not 6-dimension",
737  force.size()));
738  }
739 
740  float virtforce[6];
741  for (unsigned int i = 0; i < 6; i++)
742  virtforce[i] = (float)force[i];
743 
744  if (virtSetForce(m_virtContext, virtforce)) {
745  int err = virtGetErrorCode(m_virtContext);
746  throw(vpException(vpException::fatalError, "Error calling virtSetForce: error code %d", err));
747  }
748 }
749 
755 void vpVirtuose::setForceFactor(const float &forceFactor)
756 {
757  init();
758 
759  if (virtSetForceFactor(m_virtContext, forceFactor)) {
760  int err = virtGetErrorCode(m_virtContext);
761  throw(vpException(vpException::fatalError, "Error calling virtSetForceFactor: error code %d", err));
762  }
763 }
764 
779 void vpVirtuose::setIndexingMode(const VirtIndexingType &type)
780 {
781  init();
782 
783  if (m_indexType != type) {
784  m_indexType = type;
785 
786  if (virtSetIndexingMode(m_virtContext, m_indexType)) {
787  int err = virtGetErrorCode(m_virtContext);
788  throw(vpException(vpException::fatalError, "Error calling setIndexingMode: error code %d", err));
789  }
790  }
791 }
792 
801 {
802  init();
803 
804  float position_[7];
805  vpTranslationVector translation;
806  vpQuaternionVector quaternion;
807 
808  position.extract(translation);
809  position.extract(quaternion);
810 
811  for (int i = 0; i < 3; i++)
812  position_[i] = (float)translation[i];
813  for (int i = 0; i < 4; i++)
814  position_[3 + i] = (float)quaternion[i];
815 
816  if (virtSetObservationFrame(m_virtContext, position_)) {
817  int err = virtGetErrorCode(m_virtContext);
818  throw(vpException(vpException::fatalError, "Error calling virtSetObservationFrame: error code %d", err));
819  }
820 }
821 
861 void vpVirtuose::setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
862 {
863  init();
864 
865  if (virtSetPeriodicFunction(m_virtContext, CallBackVirt, &m_period, this)) {
866  int err = virtGetErrorCode(m_virtContext);
867  throw(vpException(vpException::fatalError, "Error calling virtSetPeriodicFunction: error code %d", err));
868  }
869 }
870 
877 {
878  init();
879 
880  float position_[7];
881  vpTranslationVector translation;
882  vpQuaternionVector quaternion;
883 
884  position.extract(translation);
885  position.extract(quaternion);
886 
887  for (int i = 0; i < 3; i++)
888  position_[i] = (float)translation[i];
889  for (int i = 0; i < 4; i++)
890  position_[3 + i] = (float)quaternion[i];
891 
892  if (virtSetPosition(m_virtContext, position_)) {
893  int err = virtGetErrorCode(m_virtContext);
894  throw(vpException(vpException::fatalError, "Error calling virtSetPosition: error code %d", err));
895  }
896 }
897 
902 {
903  init();
904 
905  if (virtSetPowerOn(m_virtContext, 0)) {
906  int err = virtGetErrorCode(m_virtContext);
907  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOff: error code %d", err));
908  }
909 }
910 
915 {
916  init();
917 
918  if (virtSetPowerOn(m_virtContext, 1)) {
919  int err = virtGetErrorCode(m_virtContext);
920  throw(vpException(vpException::fatalError, "Error calling virtSetPowerOn: error code %d", err));
921  }
922 }
923 
929 void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit)
930 {
931  init();
932 
933  if (virtSaturateTorque(m_virtContext, forceLimit, torqueLimit)) {
934  int err = virtGetErrorCode(m_virtContext);
935  throw(vpException(vpException::fatalError, "Error calling virtSaturateTorque: error code %d", err));
936  }
937 }
938 
944 void vpVirtuose::setTimeStep(const float &timeStep)
945 {
946  init();
947 
948  if (m_period != timeStep) {
949  m_period = timeStep;
950 
951  if (virtSetTimeStep(m_virtContext, m_period)) {
952  int err = virtGetErrorCode(m_virtContext);
953  throw(vpException(vpException::fatalError, "Error calling virtSetTimeStep: error code %d", err));
954  }
955  }
956 }
957 
964 {
965  init();
966 
967  if (velocity.size() != 6) {
968  throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension",
969  velocity.size()));
970  }
971 
972  float speed[6];
973  for (unsigned int i = 0; i < 6; i++)
974  speed[i] = (float)velocity[i];
975 
976  if (virtSetSpeed(m_virtContext, speed)) {
977  int err = virtGetErrorCode(m_virtContext);
978  throw(vpException(vpException::fatalError, "Error calling virtSetSpeed: error code %d", err));
979  }
980 }
981 
987 void vpVirtuose::setVelocityFactor(const float &velocityFactor)
988 {
989  init();
990 
991  if (virtSetSpeedFactor(m_virtContext, velocityFactor)) {
992  int err = virtGetErrorCode(m_virtContext);
993  throw(vpException(vpException::fatalError, "Error calling setVelocityFactor: error code %d", err));
994  }
995 }
996 
1003 {
1004  init();
1005 
1006  if (virtStartLoop(m_virtContext)) {
1007  int err = virtGetErrorCode(m_virtContext);
1008  throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err));
1009  } else
1010  std::cout << "Haptic loop open." << std::endl;
1011 }
1012 
1019 {
1020  init();
1021 
1022  if (virtStopLoop(m_virtContext)) {
1023  int err = virtGetErrorCode(m_virtContext);
1024  throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err));
1025  } else
1026  std::cout << "Haptic loop closed." << std::endl;
1027 }
1028 
1029 #else
1030 // Work around to avoid warning
1031 void dummy_vpVirtuose(){};
1032 #endif
void setSaturation(const float &forceLimit, const float &torqueLimit)
Definition: vpVirtuose.cpp:929
void startPeriodicFunction()
int m_apiMinorVersion
Definition: vpVirtuose.h:206
vpPoseVector getObservationFrame() const
Definition: vpVirtuose.cpp:373
VirtCommandType m_typeCommand
Definition: vpVirtuose.h:209
void setVelocityFactor(const float &velocityFactor)
Definition: vpVirtuose.cpp:987
void close()
Definition: vpVirtuose.cpp:66
unsigned int m_njoints
Definition: vpVirtuose.h:213
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
Definition: vpVirtuose.cpp:861
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:158
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:729
void setForceFactor(const float &forceFactor)
Definition: vpVirtuose.cpp:755
VirtContext m_virtContext
Definition: vpVirtuose.h:202
VirtIndexingType m_indexType
Definition: vpVirtuose.h:210
vpColVector getArticularVelocity() const
Definition: vpVirtuose.cpp:151
bool getEmergencyStop() const
Definition: vpVirtuose.cpp:282
bool m_verbose
Definition: vpVirtuose.h:204
void setArticularVelocity(const vpColVector &articularVelocity)
Definition: vpVirtuose.cpp:647
vpColVector getForce() const
Definition: vpVirtuose.cpp:300
void extract(vpRotationMatrix &R) const
void setVelocity(vpColVector &velocity)
Definition: vpVirtuose.cpp:963
vpColVector getPhysicalVelocity() const
Definition: vpVirtuose.cpp:439
vpPoseVector getPosition() const
Definition: vpVirtuose.cpp:462
bool m_is_init
Definition: vpVirtuose.h:211
vpColVector getArticularPosition() const
Definition: vpVirtuose.cpp:127
void enableForceFeedback(int enable)
Definition: vpVirtuose.cpp:114
vpPoseVector getAvatarPosition() const
Definition: vpVirtuose.cpp:179
VirtContext getHandler()
Definition: vpVirtuose.cpp:350
int m_ctrlMinorVersion
Definition: vpVirtuose.h:208
void setPowerOn()
Definition: vpVirtuose.cpp:914
virtual ~vpVirtuose()
Definition: vpVirtuose.cpp:77
void setPowerOff()
Definition: vpVirtuose.cpp:901
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:709
void setBaseFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:677
std::string m_ip
Definition: vpVirtuose.h:203
vpColVector getVelocity() const
Definition: vpVirtuose.cpp:509
Implementation of a rotation vector as quaternion angle minimal representation.
void setPosition(vpPoseVector &position)
Definition: vpVirtuose.cpp:876
void setArticularForce(const vpColVector &articularForce)
Definition: vpVirtuose.cpp:589
VirtCommandType getCommandType() const
Definition: vpVirtuose.cpp:244
void setArticularPosition(const vpColVector &articularPosition)
Definition: vpVirtuose.cpp:617
void stopPeriodicFunction()
unsigned int getJointsNumber() const
Definition: vpVirtuose.cpp:357
void setIndexingMode(const VirtIndexingType &type)
Definition: vpVirtuose.cpp:779
bool getDeadMan() const
Definition: vpVirtuose.cpp:263
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
float m_period
Definition: vpVirtuose.h:212
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpPoseVector getPhysicalPosition() const
Definition: vpVirtuose.cpp:406
bool getPower() const
Definition: vpVirtuose.cpp:492
int m_apiMajorVersion
Definition: vpVirtuose.h:205
int m_ctrlMajorVersion
Definition: vpVirtuose.h:207
void init()
Definition: vpVirtuose.cpp:531
void setObservationFrame(const vpPoseVector &position)
Definition: vpVirtuose.cpp:800
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
vpPoseVector getBaseFrame() const
Definition: vpVirtuose.cpp:213
void addForce(vpColVector &force)
Definition: vpVirtuose.cpp:89
void setTimeStep(const float &timeStep)
Definition: vpVirtuose.cpp:944
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.