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)
90 ss << ip <<
"#" << port;
104 if (force.
size() != 6) {
106 "Cannot apply a force feedback (dim %d) to the haptic " 107 "device that is not 6-dimension",
114 for (
unsigned int i = 0; i < 6; i++)
115 virtforce[i] = (
float)force[i];
147 float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
149 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
155 for (
unsigned int i = 0; i <
m_njoints; i++)
156 articularPosition[i] = articular_position_[i];
158 return articularPosition;
170 float articular_velocity_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
172 if (virtGetArticularSpeed(
m_virtContext, articular_velocity_)) {
179 for (
unsigned int i = 0; i <
m_njoints; i++)
180 articularVelocity[i] = articular_velocity_[i];
182 return articularVelocity;
207 for (
int i = 0; i < 3; i++)
208 translation[i] = position_[i];
209 for (
int i = 0; i < 4; i++)
210 quaternion[i] = position_[3 + i];
241 for (
int i = 0; i < 3; i++)
242 translation[i] = position_[i];
243 for (
int i = 0; i < 4; i++)
244 quaternion[i] = position_[3 + i];
263 VirtCommandType type;
287 return (deadman ?
true :
false);
306 return (emergencyStop ?
true :
false);
326 for (
unsigned int i = 0; i < 6; i++)
327 force[i] = force_[i];
401 for (
int i = 0; i < 3; i++)
402 translation[i] = position_[i];
403 for (
int i = 0; i < 4; i++)
404 quaternion[i] = position_[3 + i];
434 for (
int i = 0; i < 3; i++)
435 translation[i] = position_[i];
436 for (
int i = 0; i < 4; i++)
437 quaternion[i] = position_[3 + i];
463 virtGetErrorMessage(err)));
465 for (
unsigned int i = 0; i < 6; i++)
490 for (
int i = 0; i < 3; i++)
491 translation[i] = position_[i];
492 for (
int i = 0; i < 4; i++)
493 quaternion[i] = position_[3 + i];
513 return (power ?
true :
false);
534 for (
unsigned int i = 0; i < 6; i++)
552 m_ip_port.c_str(), virtGetErrorMessage(err)));
558 virtGetErrorMessage(err)));
577 float articular_position_[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
579 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
585 for (
unsigned int i=
m_njoints; i < 20; i++) {
587 if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
609 "Cannot apply an articular force feedback (dim %d) to " 610 "the haptic device that is not 6-dimension",
611 articularForce.
size()));
614 float *articular_force =
new float [
m_njoints];
615 for (
unsigned int i = 0; i <
m_njoints; i++)
616 articular_force[i] = (
float)articularForce[i];
619 delete[] articular_force;
624 delete [] articular_force;
640 "Cannot send an articular position command (dim %d) to " 641 "the haptic device that is not %d-dimension",
645 float *articular_position =
new float[
m_njoints];
646 for (
unsigned int i = 0; i <
m_njoints; i++)
647 articular_position[i] = (
float)articularPosition[i];
649 if (virtSetArticularPosition(
m_virtContext, articular_position)) {
651 delete [] articular_position;
654 delete[] articular_position;
670 "Cannot send an articular velocity command (dim %d) to " 671 "the haptic device that is not %d-dimension",
675 float *articular_velocity =
new float [
m_njoints];
676 for (
unsigned int i = 0; i <
m_njoints; i++)
677 articular_velocity[i] = (
float)articularVelocity[i];
679 if (virtSetArticularSpeed(
m_virtContext, articular_velocity)) {
681 delete [] articular_velocity;
684 delete[] articular_velocity;
705 for (
int i = 0; i < 3; i++)
706 position_[i] = (
float)translation[i];
707 for (
int i = 0; i < 4; i++)
708 position_[3 + i] = (
float)quaternion[i];
750 if (force.
size() != 6) {
752 "Cannot apply a force feedback (dim %d) to the haptic " 753 "device that is not 6-dimension",
758 for (
unsigned int i = 0; i < 6; i++)
759 virtforce[i] = (
float)force[i];
828 for (
int i = 0; i < 3; i++)
829 position_[i] = (
float)translation[i];
830 for (
int i = 0; i < 4; i++)
831 position_[3 + i] = (
float)quaternion[i];
904 for (
int i = 0; i < 3; i++)
905 position_[i] = (
float)translation[i];
906 for (
int i = 0; i < 4; i++)
907 position_[3 + i] = (
float)quaternion[i];
950 if (virtSaturateTorque(
m_virtContext, forceLimit, torqueLimit)) {
984 if (velocity.
size() != 6) {
990 for (
unsigned int i = 0; i < 6; i++)
991 speed[i] = (
float)velocity[i];
1027 std::cout <<
"Haptic loop open." << std::endl;
1043 std::cout <<
"Haptic loop closed." << std::endl;
1048 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 getJointsNumber() const
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)
void setIpAddressAndPort(const std::string &ip, int port)
vpPoseVector getBaseFrame() const
void enableForceFeedback(int enable)
void setCommandType(const VirtCommandType &type)
void setBaseFrame(const vpPoseVector &position)
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
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
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