Visual Servoing Platform version 3.5.0
testVirtuoseAfma6.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test for Virtuose SDK wrapper.
33 *
34 * Authors:
35 * Fabien Spindler
36 * Nicolò Pedemonte
37 *
38 *****************************************************************************/
39
48#include <visp3/core/vpTime.h>
49#include <visp3/robot/vpRobotAfma6.h>
50#include <visp3/robot/vpVirtuose.h>
51
52int main()
53{
54#if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
55 vpRobotAfma6 robot;
56 try {
57 vpVirtuose virtuose;
58 virtuose.setVerbose(true);
59 virtuose.setCommandType(COMMAND_TYPE_IMPEDANCE);
60 virtuose.setPowerOn();
61 // virtuose.setSaturation(1.0f,0.0f);
62
63 vpColVector virt_velocity;
64 vpColVector robot_velocity;
65 vpColVector robot_joint_position;
66 vpColVector robot_cart_position;
67 vpColVector robot_cart_position_init;
68 vpColVector force_feedback_robot(3);
69 float force_limit = 15;
70 int force_increase_rate = 500;
71
72 double cube_size = 0.15;
73
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;
80 vpVelocityTwistMatrix rVv(rMv);
81
82 // Set the extrinsic camera parameters obtained with a perpective
83 // projection model including a distortion parameter
85 // Initialize the controller to position control
87 // Moves the robot in the joint space
88 vpColVector q(6, 0);
89 robot.setPositioningVelocity(10);
90 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
91
92 robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position_init);
93 vpColVector min(3), max(3);
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;
97 }
98 std::cout << "min: " << min.t() << std::endl;
99 std::cout << "max: " << max.t() << std::endl;
100
101 // Initialize the controller to position control
103
104 for (unsigned int iter = 0; iter < 10000; iter++) {
105 virt_velocity = virtuose.getVelocity();
106 std::cout << "Virtuose velocity: " << virt_velocity.t() << std::endl;
107
108 robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position);
109
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;
119 } else
120 force_feedback_robot[i] = 0;
121 }
122 vpColVector force_feedback_virt = rMv.getRotationMatrix().inverse() * force_feedback_robot;
123
124 // Printing force feedback
125 std::cout << "Force feedback: " << force_feedback_virt.t() << std::endl;
126
127 robot_velocity = rVv * virt_velocity;
128 robot.setVelocity(vpRobot::CAMERA_FRAME, robot_velocity);
129
130 // Set force feedback
131 vpColVector force_feedback(6, 0);
132 force_feedback.insert(0, force_feedback_virt);
133
134 virtuose.setForce(force_feedback);
135
136 vpTime::wait(10);
137 }
138 robot.stopMotion();
139 virtuose.setPowerOff();
140 std::cout << "The end" << std::endl;
141 } catch (const vpException &e) {
142 robot.stopMotion();
143 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
144 }
145#else
146 std::cout << "You should install Virtuose SDK to use this binary..." << std::endl;
147#endif
148}
@ TOOL_CCMOP
Definition: vpAfma6.h:127
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpRowVector t() const
error that can be emited by ViSP classes.
Definition: vpException.h:72
const std::string & getStringMessage() const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:92
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
Control of Irisa's gantry robot named Afma6.
Definition: vpRobotAfma6.h:212
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
@ REFERENCE_FRAME
Definition: vpRobot.h:76
@ ARTICULAR_FRAME
Definition: vpRobot.h:78
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:67
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:66
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:201
vpRotationMatrix inverse() const
vpColVector t() const
vpColVector getVelocity() const
Definition: vpVirtuose.cpp:522
void setPowerOff()
Definition: vpVirtuose.cpp:918
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:746
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:726
void setPowerOn()
Definition: vpVirtuose.cpp:931
void setVerbose(bool mode)
Definition: vpVirtuose.h:195
VISP_EXPORT int wait(double t0, double t)