44 #include <visp3/core/vpException.h>
45 #include <visp3/robot/vpVirtuose.h>
47 #ifdef VISP_HAVE_VIRTUOSE
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)
87 ss << ip <<
"#" << port;
101 if (force.
size() != 6) {
103 "Cannot apply a force feedback (dim %d) to the haptic "
104 "device that is not 6-dimension",
111 for (
unsigned int i = 0; i < 6; i++)
112 virtforce[i] = (
float)force[i];
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};
145 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
151 for (
unsigned int i = 0; i <
m_njoints; i++)
152 articularPosition[i] = articular_position_[i];
154 return articularPosition;
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};
168 if (virtGetArticularSpeed(
m_virtContext, articular_velocity_)) {
175 for (
unsigned int i = 0; i <
m_njoints; i++)
176 articularVelocity[i] = articular_velocity_[i];
178 return articularVelocity;
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];
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];
259 VirtCommandType type;
283 return (deadman ?
true :
false);
302 return (emergencyStop ?
true :
false);
322 for (
unsigned int i = 0; i < 6; i++)
323 force[i] = force_[i];
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];
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];
458 virtGetErrorMessage(err)));
460 for (
unsigned int i = 0; i < 6; i++)
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];
508 return (power ?
true :
false);
529 for (
unsigned int i = 0; i < 6; i++)
547 "Cannot open communication with haptic device using %s: %s. Check ip and port values",
548 m_ip_port.c_str(), virtGetErrorMessage(err)));
554 virtGetErrorMessage(err)));
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};
575 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
582 for (
unsigned int i =
m_njoints; i < 20; i++) {
584 if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
606 "Cannot apply an articular force feedback (dim %d) to "
607 "the haptic device that is not 6-dimension",
608 articularForce.
size()));
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];
616 delete[] articular_force;
621 delete[] articular_force;
637 "Cannot send an articular position command (dim %d) to "
638 "the haptic device that is not %d-dimension",
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];
646 if (virtSetArticularPosition(
m_virtContext, articular_position)) {
648 delete[] articular_position;
651 delete[] articular_position;
667 "Cannot send an articular velocity command (dim %d) to "
668 "the haptic device that is not %d-dimension",
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];
676 if (virtSetArticularSpeed(
m_virtContext, articular_velocity)) {
678 delete[] articular_velocity;
681 delete[] articular_velocity;
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];
747 if (force.
size() != 6) {
749 "Cannot apply a force feedback (dim %d) to the haptic "
750 "device that is not 6-dimension",
755 for (
unsigned int i = 0; i < 6; i++)
756 virtforce[i] = (
float)force[i];
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];
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];
947 if (virtSaturateTorque(
m_virtContext, forceLimit, torqueLimit)) {
981 if (velocity.
size() != 6) {
987 for (
unsigned int i = 0; i < 6; i++)
988 speed[i] = (
float)velocity[i];
1024 std::cout <<
"Haptic loop open." << std::endl;
1040 std::cout <<
"Haptic loop closed." << std::endl;
1045 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 emited 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