Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testVirtuoseAfma6.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 * Nicolò Pedemonte
36 *
37*****************************************************************************/
38
47#include <visp3/core/vpTime.h>
48#include <visp3/robot/vpRobotAfma6.h>
49#include <visp3/robot/vpVirtuose.h>
50
51int main()
52{
53#if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
54 vpRobotAfma6 robot;
55 try {
56 vpVirtuose virtuose;
57 virtuose.setVerbose(true);
58 virtuose.setCommandType(COMMAND_TYPE_IMPEDANCE);
59 virtuose.setPowerOn();
60 // virtuose.setSaturation(1.0f,0.0f);
61
62 vpColVector virt_velocity;
63 vpColVector robot_velocity;
64 vpColVector robot_joint_position;
65 vpColVector robot_cart_position;
66 vpColVector robot_cart_position_init;
67 vpColVector force_feedback_robot(3);
68 float force_limit = 15;
69 int force_increase_rate = 500;
70
71 double cube_size = 0.15;
72
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;
79 vpVelocityTwistMatrix rVv(rMv);
80
81 // Set the extrinsic camera parameters obtained with a perpective
82 // projection model including a distortion parameter
84 // Initialize the controller to position control
86 // Moves the robot in the joint space
87 vpColVector q(6, 0);
88 robot.setPositioningVelocity(10);
89 robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
90
91 robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position_init);
92 vpColVector min(3), max(3);
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;
96 }
97 std::cout << "min: " << min.t() << std::endl;
98 std::cout << "max: " << max.t() << std::endl;
99
100 // Initialize the controller to position control
102
103 for (unsigned int iter = 0; iter < 10000; iter++) {
104 virt_velocity = virtuose.getVelocity();
105 std::cout << "Virtuose velocity: " << virt_velocity.t() << std::endl;
106
107 robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position);
108
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;
118 } else
119 force_feedback_robot[i] = 0;
120 }
121 vpColVector force_feedback_virt = rMv.getRotationMatrix().inverse() * force_feedback_robot;
122
123 // Printing force feedback
124 std::cout << "Force feedback: " << force_feedback_virt.t() << std::endl;
125
126 robot_velocity = rVv * virt_velocity;
127 robot.setVelocity(vpRobot::CAMERA_FRAME, robot_velocity);
128
129 // Set force feedback
130 vpColVector force_feedback(6, 0);
131 force_feedback.insert(0, force_feedback_virt);
132
133 virtuose.setForce(force_feedback);
134
135 vpTime::wait(10);
136 }
137 robot.stopMotion();
138 virtuose.setPowerOff();
139 std::cout << "The end" << std::endl;
140 } catch (const vpException &e) {
141 robot.stopMotion();
142 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
143 }
144#else
145 std::cout << "You should install Virtuose SDK to use this binary..." << std::endl;
146#endif
147}
@ TOOL_CCMOP
Definition vpAfma6.h:124
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
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)
@ REFERENCE_FRAME
Definition vpRobot.h:74
@ ARTICULAR_FRAME
Definition vpRobot.h:76
@ CAMERA_FRAME
Definition vpRobot.h:80
@ STATE_POSITION_CONTROL
Initialize the position controller.
Definition vpRobot.h:65
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition vpRobot.h:64
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition vpRobot.cpp:198
vpRotationMatrix inverse() const
vpColVector getVelocity() const
void setPowerOff()
void setForce(const vpColVector &force)
void setCommandType(const VirtCommandType &type)
void setPowerOn()
void setVerbose(bool mode)
Definition vpVirtuose.h:192
VISP_EXPORT int wait(double t0, double t)