Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 
68 void 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 
94 void 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 
120 void 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 
163 void 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 
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:173
void setPosition(const vpColVector &position, const int &id=1)
virtual int getPositions(const int &id, const int &max_repeats, std::vector< short int > &positions)
Definition: vpQbDevice.cpp:628
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
error that can be emited by ViSP classes.
Definition: vpException.h:71
std::vector< short int > getPositionLimits() const
Definition: vpQbDevice.cpp:612
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > &currents)
Definition: vpQbDevice.cpp:555
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
bool m_init_done
Flag used to indicate if the device is initialized.
Definition: vpQbDevice.h:115
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:126
void getCurrent(vpColVector &current, const int &id=1)
int m_max_repeats
Max number of trials to send a command.
Definition: vpQbDevice.h:114
virtual bool isInConnectedSet(const int &id)
Definition: vpQbDevice.cpp:694
void getPosition(vpColVector &position, const int &id=1)
double getCurrentMax() const
Definition: vpQbDevice.cpp:540
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
bool isReliable(int const &failures, int const &max_repeats)
Definition: vpQbDevice.cpp:716
virtual int setCommandsAsync(const int &id, std::vector< short int > &commands)
Definition: vpQbDevice.cpp:755
virtual bool init(const int &id)
Definition: vpQbDevice.cpp:655
virtual ~vpQbSoftHand()