48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotAfma6.h>
50 #include <visp3/robot/vpVirtuose.h>
54 #if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
69 float force_limit = 15;
70 int force_increase_rate = 500;
72 double cube_size = 0.15;
75 rMv[0][0] = rMv[0][2] = 0;
76 rMv[1][1] = rMv[1][2] = 0;
77 rMv[2][0] = rMv[2][1] = 0;
78 rMv[0][1] = rMv[1][0] = rMv[2][2] = -1;
79 std::cout <<
"rMv:\n" << rMv << std::endl;
89 robot.setPositioningVelocity(10);
94 for (
unsigned int i = 0; i < 3; i++) {
95 min[i] = robot_cart_position_init[i] - cube_size / 2;
96 max[i] = robot_cart_position_init[i] + cube_size / 2;
98 std::cout <<
"min: " << min.
t() << std::endl;
99 std::cout <<
"max: " << max.
t() << std::endl;
104 for (
unsigned int iter = 0; iter < 10000; iter++) {
106 std::cout <<
"Virtuose velocity: " << virt_velocity.
t() << std::endl;
110 for (
int i = 0; i < 3; i++) {
111 if (robot_cart_position[i] >= max[i]) {
112 force_feedback_robot[i] = (max[i] - robot_cart_position[i]) * force_increase_rate;
113 if (force_feedback_robot[i] <= -force_limit)
114 force_feedback_robot[i] = -force_limit;
115 }
else if (robot_cart_position[i] <= min[i]) {
116 force_feedback_robot[i] = (min[i] - robot_cart_position[i]) * force_increase_rate;
117 if (force_feedback_robot[i] >= force_limit)
118 force_feedback_robot[i] = force_limit;
120 force_feedback_robot[i] = 0;
125 std::cout <<
"Force feedback: " << force_feedback_virt.
t() << std::endl;
127 robot_velocity = rVv * virt_velocity;
132 force_feedback.insert(0, force_feedback_virt);
140 std::cout <<
"The end" << std::endl;
146 std::cout <<
"You should install Virtuose SDK to use this binary..." << std::endl;
@ perspectiveProjWithDistortion
Implementation of column vector and the associated operations.
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)