43 #include <visp3/core/vpException.h> 44 #include <visp3/robot/vpVirtuose.h> 46 #ifdef VISP_HAVE_VIRTUOSE 54 : m_virtContext(NULL), m_ip(
"localhost#5000"), m_verbose(false),
55 m_apiMajorVersion(0), m_apiMinorVersion(0),
56 m_ctrlMajorVersion(0), m_ctrlMinorVersion(0),
57 m_typeCommand(COMMAND_TYPE_IMPEDANCE), m_indexType(INDEXING_ALL),
58 m_is_init(false), m_period(0.001f)
83 if (force.
size() != 6) {
85 "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
92 for(
unsigned int i=0; i<6; i++)
93 virtforce[i] = (
float)force[i];
98 "Error calling virtAddForce: error code %d", err));
113 "Error calling virtEnableForceFeedback(): error code %d", err));
128 float articular_position_[6];
129 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
132 "Error calling virtGetArticularPosition(): error code %d", err));
135 for(
unsigned int i=0; i<6; i++)
136 articularPosition[i] = articular_position_[i];
138 return articularPosition;
152 float articular_velocity_[6];
153 if (virtGetArticularSpeed(
m_virtContext, articular_velocity_)) {
156 "Error calling virtGetArticularSpeed: error code %d", err));
159 for(
unsigned int i=0; i<6; i++)
160 articularVelocity[i] = articular_velocity_[i];
162 return articularVelocity;
186 "Error calling virtGetAvatarPosition: error code %d", err));
190 for (
int i=0; i<3; i++)
191 translation[i] = position_[i];
192 for (
int i=0; i<4; i++)
193 quaternion[i] = position_[3+i];
223 "Error calling virtGetBaseFrame: error code %d", err));
227 for (
int i=0; i<3; i++)
228 translation[i] = position_[i];
229 for (
int i=0; i<4; i++)
230 quaternion[i] = position_[3+i];
249 VirtCommandType type;
254 "Error calling virtGetCommandType: error code %d", err));
273 "Error calling virtGetDeadMan: error code %d", err));
275 return (deadman ?
true :
false);
292 "Error calling virtGetEmergencyStop: error code %d", err));
294 return (emergencyStop ?
true :
false);
312 "Error calling virtGetForce: error code %d", err));
315 for(
unsigned int i=0; i<6; i++)
316 force[i] = force_[i];
376 "Error calling virtGetObservationFrame: error code %d", err));
380 for (
int i=0; i<3; i++)
381 translation[i] = position_[i];
382 for (
int i=0; i<4; i++)
383 quaternion[i] = position_[3+i];
410 "Error calling virtGetPhysicalPosition: error code %d", err));
414 for (
int i=0; i<3; i++)
415 translation[i] = position_[i];
416 for (
int i=0; i<4; i++)
417 quaternion[i] = position_[3+i];
443 virtGetErrorMessage(err)));
445 for(
unsigned int i=0; i<6; i++)
469 "Error calling virtGetPosition: error code %d", err));
473 for (
int i=0; i<3; i++)
474 translation[i] = position_[i];
475 for (
int i=0; i<4; i++)
476 quaternion[i] = position_[3+i];
496 return (power ?
true :
false);
516 virtGetErrorMessage(err)));
518 for(
unsigned int i=0; i<6; i++)
535 virtGetErrorMessage(err)));
541 virtGetErrorMessage(err)));
551 virtGetErrorMessage(err)));
557 "Error calling virtSetTimeStep: error code %d", err));
573 if (articularForce.
size() != 6) {
575 "Cannot apply an articular force feedback (dim %d) to the haptic device that is not 6-dimension",
576 articularForce.
size()));
579 float articular_force[6];
580 for(
unsigned int i=0; i<6; i++)
581 articular_force[i] = (
float)articularForce[i];
586 "Error calling virtSetArticularForce: error code %d", err));
599 if (articularPosition.
size() != 6) {
601 "Cannot send an articular position command (dim %d) to the haptic device that is not 6-dimension",
602 articularPosition.
size()));
605 float articular_position[6];
606 for(
unsigned int i=0; i<6; i++)
607 articular_position[i] = (
float)articularPosition[i];
609 if (virtSetArticularPosition(
m_virtContext, articular_position)) {
612 "Error calling virtSetArticularPosition: error code %d", err));
625 if (articularVelocity.
size() != 6) {
627 "Cannot send an articular velocity command (dim %d) to the haptic device that is not 6-dimension",
628 articularVelocity.
size()));
631 float articular_velocity[6];
632 for(
unsigned int i=0; i<6; i++)
633 articular_velocity[i] = (
float)articularVelocity[i];
635 if (virtSetArticularSpeed(
m_virtContext, articular_velocity)) {
638 "Error calling virtSetArticularVelocity: error code %d", err));
660 for (
int i=0; i<3; i++)
661 position_[i] = (
float)translation[i];
662 for (
int i=0; i<4; i++)
663 position_[3 + i] = (
float)quaternion[i];
668 "Error calling virtSetBaseFrame: error code %d", err));
691 "Error calling virtSetCommandType: error code %d", err));
705 if (force.
size() != 6) {
707 "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
712 for(
unsigned int i=0; i<6; i++)
713 virtforce[i] = (
float)force[i];
718 "Error calling virtSetForce: error code %d", err));
733 "Error calling virtSetForceFactor: error code %d", err));
756 "Error calling setIndexingMode: error code %d", err));
779 for (
int i=0; i<3; i++)
780 position_[i] = (
float)translation[i];
781 for (
int i=0; i<4; i++)
782 position_[3 + i] = (
float)quaternion[i];
787 "Error calling virtSetObservationFrame: error code %d", err));
832 "Error calling virtSetPeriodicFunction: error code %d", err));
851 for (
int i=0; i<3; i++)
852 position_[i] = (
float)translation[i];
853 for (
int i=0; i<4; i++)
854 position_[3+i] = (
float)quaternion[i];
859 "Error calling virtSetPosition: error code %d", err));
874 "Error calling virtSetPowerOff: error code %d", err));
888 "Error calling virtSetPowerOn: error code %d", err));
901 if (virtSaturateTorque(
m_virtContext, forceLimit, torqueLimit)) {
904 "Error calling virtSaturateTorque: error code %d", err));
923 "Error calling virtSetTimeStep: error code %d", err));
937 if (velocity.
size() != 6) {
939 "Cannot set a velocity vector (dim %d) that is not 6-dimension",
944 for(
unsigned int i=0; i<6; i++)
945 speed[i] = (
float)velocity[i];
950 "Error calling virtSetSpeed: error code %d", err));
984 std::cout <<
"Haptic loop open." << std::endl;
1002 std::cout <<
"Haptic loop closed." << std::endl;
1007 void dummy_vpVirtuose() {};
void setSaturation(const float &forceLimit, const float &torqueLimit)
void startPeriodicFunction()
vpColVector getForce() const
VirtCommandType m_typeCommand
VirtCommandType getCommandType() const
void setVelocityFactor(const float &velocityFactor)
void extract(vpRotationMatrix &R) const
vpPoseVector getPhysicalPosition() const
vpColVector getArticularPosition() const
vpColVector getArticularVelocity() const
error that can be emited by ViSP classes.
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
unsigned int size() const
Return the number of elements of the 2D array.
void setForce(const vpColVector &force)
void setForceFactor(const float &forceFactor)
VirtContext m_virtContext
VirtIndexingType m_indexType
void setArticularVelocity(const vpColVector &articularVelocity)
void setVelocity(vpColVector &velocity)
vpPoseVector getBaseFrame() const
void enableForceFeedback(int enable)
void setCommandType(const VirtCommandType &type)
void setBaseFrame(const vpPoseVector &position)
vpPoseVector getAvatarPosition() const
Implementation of a rotation vector as quaternion angle minimal representation.
vpPoseVector getPosition() const
void setPosition(vpPoseVector &position)
void setArticularForce(const vpColVector &articularForce)
void setArticularPosition(const vpColVector &articularPosition)
void stopPeriodicFunction()
void setIndexingMode(const VirtIndexingType &type)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpColVector getPhysicalVelocity() const
void setObservationFrame(const vpPoseVector &position)
vpColVector getVelocity() const
bool getEmergencyStop() const
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
void addForce(vpColVector &force)
void setTimeStep(const float &timeStep)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
vpPoseVector getObservationFrame() const