41 #include <visp3/core/vpException.h>
42 #include <visp3/robot/vpVirtuose.h>
44 #ifdef VISP_HAVE_VIRTUOSE
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)
84 ss << ip <<
"#" << port;
98 if (force.
size() != 6) {
100 "Cannot apply a force feedback (dim %d) to the haptic "
101 "device that is not 6-dimension",
108 for (
unsigned int i = 0; i < 6; i++)
109 virtforce[i] = (
float)force[i];
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};
142 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
148 for (
unsigned int i = 0; i <
m_njoints; i++)
149 articularPosition[i] = articular_position_[i];
151 return articularPosition;
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};
165 if (virtGetArticularSpeed(
m_virtContext, articular_velocity_)) {
172 for (
unsigned int i = 0; i <
m_njoints; i++)
173 articularVelocity[i] = articular_velocity_[i];
175 return articularVelocity;
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];
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];
256 VirtCommandType type;
280 return (deadman ?
true :
false);
299 return (emergencyStop ?
true :
false);
319 for (
unsigned int i = 0; i < 6; i++)
320 force[i] = force_[i];
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];
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];
455 virtGetErrorMessage(err)));
457 for (
unsigned int i = 0; i < 6; i++)
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];
505 return (power ?
true :
false);
526 for (
unsigned int i = 0; i < 6; i++)
544 "Cannot open communication with haptic device using %s: %s. Check ip and port values",
545 m_ip_port.c_str(), virtGetErrorMessage(err)));
551 virtGetErrorMessage(err)));
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};
572 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
579 for (
unsigned int i =
m_njoints; i < 20; i++) {
581 if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
603 "Cannot apply an articular force feedback (dim %d) to "
604 "the haptic device that is not 6-dimension",
605 articularForce.
size()));
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];
613 delete[] articular_force;
618 delete[] articular_force;
634 "Cannot send an articular position command (dim %d) to "
635 "the haptic device that is not %d-dimension",
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];
643 if (virtSetArticularPosition(
m_virtContext, articular_position)) {
645 delete[] articular_position;
648 delete[] articular_position;
664 "Cannot send an articular velocity command (dim %d) to "
665 "the haptic device that is not %d-dimension",
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];
673 if (virtSetArticularSpeed(
m_virtContext, articular_velocity)) {
675 delete[] articular_velocity;
678 delete[] articular_velocity;
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];
744 if (force.
size() != 6) {
746 "Cannot apply a force feedback (dim %d) to the haptic "
747 "device that is not 6-dimension",
752 for (
unsigned int i = 0; i < 6; i++)
753 virtforce[i] = (
float)force[i];
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];
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];
944 if (virtSaturateTorque(
m_virtContext, forceLimit, torqueLimit)) {
978 if (velocity.
size() != 6) {
984 for (
unsigned int i = 0; i < 6; i++)
985 speed[i] = (
float)velocity[i];
1021 std::cout <<
"Haptic loop open." << std::endl;
1037 std::cout <<
"Haptic loop closed." << std::endl;
1042 void dummy_vpVirtuose(){};
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
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.
vpColVector getVelocity() const
vpColVector getArticularPosition() const
VirtCommandType getCommandType() const
void setIpAddressAndPort(const std::string &ip, int port)
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
vpPoseVector getBaseFrame() const
void setForce(const vpColVector &force)
void setIndexingMode(const VirtIndexingType &type)
unsigned int getJointsNumber() const
vpPoseVector getPhysicalPosition() const
void setBaseFrame(const vpPoseVector &position)
VirtContext m_virtContext
vpPoseVector getObservationFrame() const
bool getEmergencyStop() const
void setCommandType(const VirtCommandType &type)
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
vpColVector getArticularVelocity() const
void stopPeriodicFunction()
vpPoseVector getAvatarPosition() const
void addForce(vpColVector &force)
void setArticularForce(const vpColVector &articularForce)
void setPosition(vpPoseVector &position)
void setVelocity(vpColVector &velocity)
VirtCommandType m_typeCommand
void startPeriodicFunction()
void setVelocityFactor(const float &velocityFactor)
void setSaturation(const float &forceLimit, const float &torqueLimit)
vpColVector getPhysicalVelocity() const
void setArticularPosition(const vpColVector &articularPosition)
VirtIndexingType m_indexType
vpColVector getForce() const