Visual Servoing Platform version 3.5.0
vpQbSoftHand.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 * Interface for the qb robotics devices.
33 *
34 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40#ifdef VISP_HAVE_QBDEVICE
41
42#include <regex>
43
44#include <visp3/robot/vpQbSoftHand.h>
45
51 : vpQbDevice()
52{
53}
54
60{
61}
62
68void vpQbSoftHand::getCurrent(vpColVector &current, const int &id)
69{
70 if (! m_init_done) {
71 init(id);
72 }
73
74 current.resize(1);
75 if (!isInConnectedSet(id)) {
76 throw(vpException(vpException::fatalError, "Cannot get current, Qb device is not connected"));
77 }
78
79 std::vector<short int> currents(2);
80 int failures = getCurrents(id, m_max_repeats, currents); // blocks while reading
81
82 if (! isReliable(failures, m_max_repeats)) {
83 throw(vpException(vpException::fatalError, "Cannot get current, communication error with Qb device after %d attempts", m_max_repeats));
84 }
85 current[0] = static_cast<double>(currents[0]);
86}
87
94void vpQbSoftHand::getPosition(vpColVector &position, const int &id)
95{
96 if (! m_init_done) {
97 init(id);
98 }
99
100 position.resize(1);
101 if (!isInConnectedSet(id)) {
102 throw(vpException(vpException::fatalError, "Cannot get position, Qb device is not connected"));
103 }
104
105 std::vector<short int> positions;
106 int failures = getPositions(id, m_max_repeats, positions); // blocks while reading
107
108 position[0] = static_cast<double>(positions[0])/static_cast<double>(getPositionLimits()[1]);
109
110 if (! isReliable(failures, m_max_repeats)) {
111 throw(vpException(vpException::fatalError, "Cannot get position, communication error with Qb device after %d attempts", m_max_repeats));
112 }
113}
114
120void vpQbSoftHand::setPosition(const vpColVector &position, const int &id)
121{
122 if (! m_init_done) {
123 init(id);
124 }
125
126 std::vector<short int> commands(2);
127 if (position.size() != 1) {
128 throw(vpException(vpException::fatalError, "Command vector size %d is not equal to 2", position.size()));
129 }
130
131 std::vector<short int> position_limits = getPositionLimits();
132
133 commands[0] = static_cast<short int>(position[0]*position_limits[1]);
134
135 if(commands[0] < position_limits[0]) {
136 commands[0] = position_limits[0];
137 }
138 else if (commands[0] > position_limits[1]) {
139 commands[0] = position_limits[1];
140 }
141
142 if (!isInConnectedSet(id)) {
143 throw(vpException(vpException::fatalError, "Cannot set position, Qb device is not connected"));
144 }
145
146 //int failures = setCommandsAndWait(id, m_max_repeats, commands); // FS: doesn't work
147 int failures = setCommandsAsync(id, commands);
148
149 if (! isReliable(failures, m_max_repeats)) {
150 throw(vpException(vpException::fatalError, "Cannot set position, communication error with Qb device after %d attempts", m_max_repeats));
151 }
152}
153
163void vpQbSoftHand::setPosition(const vpColVector &position, double speed_factor, double stiffness, const int &id)
164{
165 vpColVector q_mes(1), q(1), current;
166 getPosition(q_mes, id);
167 double current_max = getCurrentMax();
168
169 double max_delta_q = 1; // 0 opened, 1 closed
170 double min_delta_t = 2.0; // number of [sec] to open or close with the max velocity
171 double precision = 0.05;
172 double delta_t = 40; // [ms]
173 double max_slope = max_delta_q / min_delta_t;
174 double sign = (position[0] > q_mes[0]) ? 1.0 : -1.0;
175 double vel = speed_factor;
176 if (vel < 0.01) {
177 vel = 0.01;
178 }
179 else if (vel > 1.) {
180 vel = 1.0;
181 }
182 double current_factor = stiffness;
183 if (current_factor < 0.0) {
184 current_factor = 0.0;
185 }
186 else if (current_factor > 1.) {
187 current_factor = 1.0;
188 }
189 double slope = sign * max_slope * vel;
190
191 unsigned int i = 0;
192 int current_failures = 0;
193 do {
194 double t0 = vpTime::measureTimeMs();
195 q[0] = q_mes[0] + slope * delta_t/1000.0 * i;
196 if(q[0] < getPositionLimits()[0]) {
197 q[0] = getPositionLimits()[0];
198 }
199 else if (q[0] > getPositionLimits()[1]) {
200 q[0] = getPositionLimits()[1];
201 }
202 setPosition(q, id);
203 getCurrent(current, id);
204 i ++;
205
206 if (std::fabs(current[0]) > current_factor*current_max) {
207 current_failures ++;
208 }
209 else {
210 current_failures = 0;
211 }
212
213 vpTime::wait(t0, delta_t);
214 } while (! vpMath::equal(q[0], position[0], precision) && ! (current_failures > 1));
215}
216#endif
217
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
bool isReliable(int const &failures, int const &max_repeats)
Definition: vpQbDevice.cpp:716
double getCurrentMax() const
Definition: vpQbDevice.cpp:540
virtual bool isInConnectedSet(const int &id)
Definition: vpQbDevice.cpp:694
virtual bool init(const int &id)
Definition: vpQbDevice.cpp:655
int m_max_repeats
Max number of trials to send a command.
Definition: vpQbDevice.h:114
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > &currents)
Definition: vpQbDevice.cpp:555
std::vector< short int > getPositionLimits() const
Definition: vpQbDevice.cpp:612
virtual int getPositions(const int &id, const int &max_repeats, std::vector< short int > &positions)
Definition: vpQbDevice.cpp:628
virtual int setCommandsAsync(const int &id, std::vector< short int > &commands)
Definition: vpQbDevice.cpp:755
bool m_init_done
Flag used to indicate if the device is initialized.
Definition: vpQbDevice.h:115
void getCurrent(vpColVector &current, const int &id=1)
void setPosition(const vpColVector &position, const int &id=1)
virtual ~vpQbSoftHand()
void getPosition(vpColVector &position, const int &id=1)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()