Test Haption Virtuose SDK wrapper to control Afma6 robot velocity from haptic device velocity. Movements are allowed inside a cube of pre-determined side.
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/robot/vpVirtuose.h>
int main()
{
#if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
try {
float force_limit = 15;
int force_increase_rate = 500;
double cube_size = 0.15;
rMv[0][0] = rMv[0][2] = 0;
rMv[1][1] = rMv[1][2] = 0;
rMv[2][0] = rMv[2][1] = 0;
rMv[0][1] = rMv[1][0] = rMv[2][2] = -1;
std::cout << "rMv:\n" << rMv << std::endl;
robot.setPositioningVelocity(10);
for (unsigned int i = 0; i < 3; i++) {
min[i] = robot_cart_position_init[i] - cube_size / 2;
max[i] = robot_cart_position_init[i] + cube_size / 2;
}
std::cout <<
"min: " << min.
t() << std::endl;
std::cout <<
"max: " << max.
t() << std::endl;
for (unsigned int iter = 0; iter < 10000; iter++) {
std::cout <<
"Virtuose velocity: " << virt_velocity.
t() << std::endl;
for (int i = 0; i < 3; i++) {
if (robot_cart_position[i] >= max[i]) {
force_feedback_robot[i] = (max[i] - robot_cart_position[i]) * force_increase_rate;
if (force_feedback_robot[i] <= -force_limit)
force_feedback_robot[i] = -force_limit;
} else if (robot_cart_position[i] <= min[i]) {
force_feedback_robot[i] = (min[i] - robot_cart_position[i]) * force_increase_rate;
if (force_feedback_robot[i] >= force_limit)
force_feedback_robot[i] = force_limit;
} else
force_feedback_robot[i] = 0;
}
std::cout <<
"Force feedback: " << force_feedback_virt.
t() << std::endl;
robot_velocity = rVv * virt_velocity;
force_feedback.
insert(0, force_feedback_virt);
}
robot.stopMotion();
std::cout << "The end" << std::endl;
robot.stopMotion();
}
#else
std::cout << "You should install Virtuose SDK to use this binary..." << std::endl;
#endif
}
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
void insert(unsigned int i, const vpColVector &v)
error that can be emited by ViSP classes.
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
Control of Irisa's gantry robot named Afma6.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpRotationMatrix inverse() const
vpColVector getVelocity() const
void setForce(const vpColVector &force)
void setCommandType(const VirtCommandType &type)
void setVerbose(bool mode)
VISP_EXPORT int wait(double t0, double t)