47 #include <visp3/core/vpTime.h>
48 #include <visp3/robot/vpRobotAfma6.h>
49 #include <visp3/robot/vpVirtuose.h>
53 #if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
68 float force_limit = 15;
69 int force_increase_rate = 500;
71 double cube_size = 0.15;
74 rMv[0][0] = rMv[0][2] = 0;
75 rMv[1][1] = rMv[1][2] = 0;
76 rMv[2][0] = rMv[2][1] = 0;
77 rMv[0][1] = rMv[1][0] = rMv[2][2] = -1;
78 std::cout <<
"rMv:\n" << rMv << std::endl;
88 robot.setPositioningVelocity(10);
93 for (
unsigned int i = 0; i < 3; i++) {
94 min[i] = robot_cart_position_init[i] - cube_size / 2;
95 max[i] = robot_cart_position_init[i] + cube_size / 2;
97 std::cout <<
"min: " << min.
t() << std::endl;
98 std::cout <<
"max: " << max.
t() << std::endl;
103 for (
unsigned int iter = 0; iter < 10000; iter++) {
105 std::cout <<
"Virtuose velocity: " << virt_velocity.
t() << std::endl;
109 for (
int i = 0; i < 3; i++) {
110 if (robot_cart_position[i] >= max[i]) {
111 force_feedback_robot[i] = (max[i] - robot_cart_position[i]) * force_increase_rate;
112 if (force_feedback_robot[i] <= -force_limit)
113 force_feedback_robot[i] = -force_limit;
114 }
else if (robot_cart_position[i] <= min[i]) {
115 force_feedback_robot[i] = (min[i] - robot_cart_position[i]) * force_increase_rate;
116 if (force_feedback_robot[i] >= force_limit)
117 force_feedback_robot[i] = force_limit;
119 force_feedback_robot[i] = 0;
124 std::cout <<
"Force feedback: " << force_feedback_virt.
t() << std::endl;
126 robot_velocity = rVv * virt_velocity;
131 force_feedback.insert(0, force_feedback_virt);
139 std::cout <<
"The end" << std::endl;
145 std::cout <<
"You should install Virtuose SDK to use this binary..." << std::endl;
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
const std::string & getStringMessage() const
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) vp_override
@ 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)