Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testVirtuoseHapticBox.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 * Firas Abi Farraj
37 *
38*****************************************************************************/
39
48#include <visp3/core/vpTime.h>
49#include <visp3/robot/vpVirtuose.h>
50
51#if defined(VISP_HAVE_VIRTUOSE)
52
53void CallBackVirtuose(VirtContext VC, void *ptr)
54{
55 (void)VC;
56
57 static bool firstIteration = true;
58 static vpPoseVector localPosition0;
59
60 vpPoseVector localPosition;
61 vpColVector forceFeedback(6, 0);
62 vpColVector finalForce(6, 0);
63 vpColVector forceEe(6, 0);
64 int force_limit = 15;
65 int force_increase_rate = 500;
66 float cube_size = 0.05f;
67
68 // Virtual spring to let the user know where the initial position is
69 // Estimated Virtuose handle mass = 0.1;
70 // Estimated Virtuose handle length = 0.23;
71 // In the ee frame: Virtuose Handle as a cylinder for the inertia
72 // Estimated Inertia1 = m*l*l/12
73 // Estimated Inertia2 = m*l*l/2 (rotation w.r.t. e-e z axis)
74 double virtualStiffnessAng = 20;
75 double virtualDamperAng = 0.182; // greater than sqrt 4*Inertia1*virtualStiffnessAng
76 double virtualDamperAng2 = 0.0456; // greater than sqrt 4*Inertia2*virtualStiffnessAng
77
78 vpColVector xd(3, 0);
79 vpColVector yd(3, 0);
80 vpColVector zd(3, 0);
81 vpColVector xee(3, 0);
82 vpColVector zee(3, 0);
83 vpColVector xeed(3, 0);
84 vpColVector zeed(3, 0);
85 vpColVector zYZ(3, 0);
86 vpColVector zXZ(3, 0);
87 vpColVector xXY(3, 0);
89 vpColVector omegad(3, 0);
92 vpPoseVector pee;
93 vpColVector vee(6, 0);
94 vpColVector veed(6, 0);
95
96 double alpha;
97
98 vpColVector torque1(3, 0);
99 vpColVector torque2(3, 0);
100 vpColVector torque3(3, 0);
101
102 vpVirtuose *p_virtuose = (vpVirtuose *)ptr;
103 localPosition = p_virtuose->getPhysicalPosition();
104
105 if (firstIteration) {
106 localPosition0 = localPosition;
107 firstIteration = false;
108 }
109
110 // Position and velocity in of the ee expressed in the base frame
111 pee = localPosition;
112 vee = p_virtuose->getPhysicalVelocity();
113 // Z axis = [pee_x pee_y 0]
114 zd[0] = pee[0];
115 zd[1] = pee[1];
116 zd.normalize();
117 // X axis = [0 0 1]
118 xd[2] = 1;
119 // Y axis from cross product
120 yd = zd.skew(zd) * xd;
121
122 // Current orientation of the ee frame
123 pee.extract(Qee);
124 pee.extract(tee);
125 // X and Z axis of the ee frame expressed in the base frame
126 xee = Qee.getCol(0);
127 zee = Qee.getCol(2);
128
129 // Rotation matrix from Desired Frame to Base Frame
130 Qd[0][0] = xd[0];
131 Qd[1][0] = xd[1];
132 Qd[2][0] = xd[2];
133 Qd[0][1] = yd[0];
134 Qd[1][1] = yd[1];
135 Qd[2][1] = yd[2];
136 Qd[0][2] = zd[0];
137 Qd[1][2] = zd[1];
138 Qd[2][2] = zd[2];
139
140 // X and Z axis of the ee frame expressed in the desired frame
141 xeed = Qd.inverse() * xee;
142 zeed = Qd.inverse() * zee;
143
144 vpHomogeneousMatrix dMb(tee, Qd);
145 // Velocity twist matrix for expressing velocities in the desired frame
146 vpVelocityTwistMatrix dVMb(dMb.inverse());
147 // Force twist matrix for expressing forces in the base frame
148 vpForceTwistMatrix dFMb(dMb);
149
150 veed = dVMb * vee;
151
152 // Angular velocity in the desired frame
153 omegad[0] = veed[3];
154 omegad[1] = veed[4];
155 omegad[2] = veed[5];
156
157 // Projection of Z axis of the ee frame onto plane YZ (expressed in the
158 // desired frame)
159 zYZ[1] = zeed[1];
160 zYZ[2] = zeed[2];
161
162 // Projection of Z axis of the ee frame onto plane XZ (expressed in the
163 // desired frame)
164 zXZ[0] = zeed[0];
165 zXZ[2] = zeed[2];
166
167 // Hard spring to keep Z axis of the ee frame in the horizontal plane
168 // Spring applied to the angle between the Z axis of the ee frame and its
169 // projection in the YZ (horizontal) plane
170 vpColVector rotzYZ(3, 0);
171 rotzYZ = zeed.skew(zeed) * zYZ.normalize();
172 vpColVector forceStiff1 = virtualStiffnessAng * rotzYZ;
173 vpColVector forceDamp1 = virtualDamperAng * (omegad * rotzYZ.normalize()) * rotzYZ.normalize();
174
175 for (unsigned int i = 0; i < 3; i++)
176 torque1[i] = forceStiff1[i] - forceDamp1[i];
177
178 // Hard spring to keep Z axis of the ee frame pointing at the origin
179 // Spring applied to the angle between the Z axis of the ee frame and its
180 // projection in the XZ (vertical) plane
181 vpColVector rotzXZ(3, 0);
182 rotzXZ = zeed.skew(zeed) * zXZ.normalize();
183 vpColVector forceStiff2 = virtualStiffnessAng * rotzXZ;
184 vpColVector forceDamp2 = virtualDamperAng * (omegad * rotzXZ.normalize()) * rotzXZ.normalize();
185
186 for (unsigned int i = 0; i < 3; i++)
187 torque2[i] = forceStiff2[i] - forceDamp2[i];
188
189 // Hard spring for rotation around z axis of the ee
190 xXY[0] = xeed[0];
191 xXY[1] = xeed[1];
192 vpColVector xdd(3, 0);
193 xdd[0] = 1;
194 vpColVector zdd(3, 0);
195 zdd[2] = 1;
196 vpColVector rotxXY(3, 0);
197 rotxXY = xdd.skew(xdd) * xXY.normalize();
198 alpha = asin(rotxXY[2]);
199
200 vpColVector forceStiff3 = virtualStiffnessAng * alpha * zdd;
201 vpColVector forceDamp3 = virtualDamperAng2 * (omegad * zdd) * zdd;
202 for (unsigned int i = 0; i < 3; i++)
203 torque3[i] = forceStiff3[i] - forceDamp3[i];
204
205 for (unsigned int j = 0; j < 3; j++)
206 forceEe[j + 3] = torque1[j] + torque2[j] + torque3[j];
207
208 forceEe = dFMb * forceEe;
209
210 // ---------------
211 // Haptic Box
212 // ---------------
213 vpColVector p_min(3, 0), p_max(3, 0);
214 for (unsigned int i = 0; i < 3; i++) {
215 p_min[i] = localPosition0[i] - cube_size / 2;
216 p_max[i] = localPosition0[i] + cube_size / 2;
217 }
218
219 for (int i = 0; i < 3; i++) {
220 if ((p_min[i] >= localPosition[i])) {
221 forceFeedback[i] = (p_min[i] - localPosition[i]) * force_increase_rate;
222 if (forceFeedback[i] >= force_limit)
223 forceFeedback[i] = force_limit;
224 } else if ((p_max[i] <= localPosition[i])) {
225 forceFeedback[i] = (p_max[i] - localPosition[i]) * force_increase_rate;
226 if (forceFeedback[i] <= -force_limit)
227 forceFeedback[i] = -force_limit;
228 } else
229 forceFeedback[i] = 0;
230 }
231
232 for (unsigned int j = 0; j < 6; j++)
233 finalForce[j] = forceFeedback[j] + forceEe[j];
234
235 // Set force feedback
236 p_virtuose->setForce(finalForce);
237
238 return;
239}
240
241int main(int argc, char **argv)
242{
243 std::string opt_ip = "localhost";
244 int opt_port = 5000;
245 for (int i = 0; i < argc; i++) {
246 if (std::string(argv[i]) == "--ip")
247 opt_ip = std::string(argv[i + 1]);
248 else if (std::string(argv[i]) == "--port")
249 opt_port = std::atoi(argv[i + 1]);
250 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
251 std::cout << "\nUsage: " << argv[0]
252 << " [--ip <localhost>] [--port <port>]"
253 " [--help] [-h]\n"
254 << std::endl
255 << "Description: " << std::endl
256 << " --ip <localhost>" << std::endl
257 << "\tHost IP address. Default value: \"localhost\"." << std::endl
258 << std::endl
259 << " --port <port>" << std::endl
260 << "\tCommunication port. Default value: 5000." << std::endl
261 << "\tSuggested values: " << std::endl
262 << "\t- 5000 to communicate with the Virtuose." << std::endl
263 << "\t- 53210 to communicate with the Virtuose equipped with the Glove." << std::endl
264 << std::endl;
265 return EXIT_SUCCESS;
266 }
267 }
268
269 try {
270 vpVirtuose virtuose;
271 std::cout << "Try to connect to " << opt_ip << " port " << opt_port << std::endl;
272 virtuose.setIpAddressAndPort(opt_ip, opt_port);
273 virtuose.setVerbose(true);
274 virtuose.setPowerOn();
275 virtuose.setPeriodicFunction(CallBackVirtuose);
276 virtuose.startPeriodicFunction();
277
278 int counter = 0;
279 bool swtch = true;
280
281 while (swtch) {
282 if (counter >= 10) {
283 virtuose.stopPeriodicFunction();
284 virtuose.setPowerOff();
285 swtch = false;
286 }
287 counter++;
288 vpTime::sleepMs(1000);
289 }
290 std::cout << "The end" << std::endl;
291 } catch (const vpException &e) {
292 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
293 return EXIT_FAILURE;
294 }
295 return EXIT_SUCCESS;
296}
297
298#else
299int main()
300{
301 std::cout << "You should install pthread and/or Virtuose API to use this "
302 "binary..."
303 << std::endl;
304 return EXIT_SUCCESS;
305}
306#endif
Implementation of column vector and the associated operations.
vpColVector & normalize()
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.
Implementation of a pose vector and operations on poses.
void extract(vpRotationMatrix &R) const
Implementation of a rotation matrix and operations on such kind of matrices.
vpColVector getCol(unsigned int j) const
vpRotationMatrix inverse() const
Class that consider the case of a translation vector.
void setIpAddressAndPort(const std::string &ip, int port)
void setPowerOff()
void setForce(const vpColVector &force)
vpPoseVector getPhysicalPosition() const
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt)
void stopPeriodicFunction()
void setPowerOn()
void setVerbose(bool mode)
Definition vpVirtuose.h:192
void startPeriodicFunction()
vpColVector getPhysicalVelocity() const
VISP_EXPORT void sleepMs(double t)