44 #include <visp3/core/vpException.h>
45 #include <visp3/robot/vpVirtuose.h>
47 #ifdef VISP_HAVE_VIRTUOSE
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)
59 virtAPIVersion(&m_apiMajorVersion, &m_apiMinorVersion);
60 std::cout <<
"API version: " << m_apiMajorVersion <<
"." << m_apiMinorVersion << std::endl;
91 if (force.
size() != 6) {
93 "Cannot apply a force feedback (dim %d) to the haptic "
94 "device that is not 6-dimension",
101 for (
unsigned int i = 0; i < 6; i++)
102 virtforce[i] = (
float)force[i];
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};
136 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
142 for (
unsigned int i = 0; i <
m_njoints; i++)
143 articularPosition[i] = articular_position_[i];
145 return articularPosition;
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};
159 if (virtGetArticularSpeed(
m_virtContext, articular_velocity_)) {
166 for (
unsigned int i = 0; i <
m_njoints; i++)
167 articularVelocity[i] = articular_velocity_[i];
169 return articularVelocity;
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];
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];
250 VirtCommandType type;
274 return (deadman ?
true :
false);
293 return (emergencyStop ?
true :
false);
313 for (
unsigned int i = 0; i < 6; i++)
314 force[i] = force_[i];
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];
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];
450 virtGetErrorMessage(err)));
452 for (
unsigned int i = 0; i < 6; i++)
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];
500 return (power ?
true :
false);
521 for (
unsigned int i = 0; i < 6; i++)
544 virtGetErrorMessage(err)));
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};
565 if (virtGetArticularPosition(
m_virtContext, articular_position_)) {
571 for (
unsigned int i=
m_njoints; i < 20; i++) {
573 if (std::fabs(articular_position_[i]) <= std::numeric_limits<float>::epsilon()) {
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()));
600 float articular_force[6];
601 for (
unsigned int i = 0; i < 6; i++)
602 articular_force[i] = (
float)articularForce[i];
623 "Cannot send an articular position command (dim %d) to "
624 "the haptic device that is not %d-dimension",
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];
632 if (virtSetArticularPosition(
m_virtContext, articular_position)) {
634 delete [] articular_position;
637 delete[] articular_position;
653 "Cannot send an articular velocity command (dim %d) to "
654 "the haptic device that is not %d-dimension",
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];
662 if (virtSetArticularSpeed(
m_virtContext, articular_velocity)) {
664 delete [] articular_velocity;
667 delete[] articular_velocity;
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];
733 if (force.
size() != 6) {
735 "Cannot apply a force feedback (dim %d) to the haptic "
736 "device that is not 6-dimension",
741 for (
unsigned int i = 0; i < 6; i++)
742 virtforce[i] = (
float)force[i];
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];
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];
933 if (virtSaturateTorque(
m_virtContext, forceLimit, torqueLimit)) {
967 if (velocity.
size() != 6) {
973 for (
unsigned int i = 0; i < 6; i++)
974 speed[i] = (
float)velocity[i];
1010 std::cout <<
"Haptic loop open." << std::endl;
1026 std::cout <<
"Haptic loop closed." << std::endl;
1031 void dummy_vpVirtuose(){};