36 #include <visp3/core/vpConfig.h>
37 #ifdef VISP_HAVE_QBDEVICE
41 #include <visp3/robot/vpQbSoftHand.h>
65 std::vector<short int> currents(2);
70 "Cannot get current, communication error with Qb device after %d attempts",
m_max_repeats));
72 current[0] =
static_cast<double>(currents[0]);
92 std::vector<short int> positions;
95 position[0] =
static_cast<double>(positions[0]) /
static_cast<double>(
getPositionLimits()[1]);
99 "Cannot get position, communication error with Qb device after %d attempts",
m_max_repeats));
115 std::vector<short int> commands(2);
116 if (position.
size() != 1) {
122 commands[0] =
static_cast<short int>(position[0] * position_limits[1]);
124 if (commands[0] < position_limits[0]) {
125 commands[0] = position_limits[0];
127 else if (commands[0] > position_limits[1]) {
128 commands[0] = position_limits[1];
140 "Cannot set position, communication error with Qb device after %d attempts",
m_max_repeats));
160 double max_delta_q = 1;
161 double min_delta_t = 2.0;
162 double precision = 0.05;
164 double max_slope = max_delta_q / min_delta_t;
165 double sign = (position[0] > q_mes[0]) ? 1.0 : -1.0;
166 double vel = speed_factor;
173 double current_factor = stiffness;
174 if (current_factor < 0.0) {
175 current_factor = 0.0;
177 else if (current_factor > 1.) {
178 current_factor = 1.0;
180 double slope = sign * max_slope * vel;
183 int current_failures = 0;
186 q[0] = q_mes[0] + slope * delta_t / 1000.0 * i;
197 if (std::fabs(current[0]) > current_factor * current_max) {
201 current_failures = 0;
205 }
while (!
vpMath::equal(q[0], position[0], precision) && !(current_failures > 1));
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
static bool equal(double x, double y, double threshold=0.001)
bool isReliable(int const &failures, int const &max_repeats)
double getCurrentMax() const
virtual bool isInConnectedSet(const int &id)
virtual bool init(const int &id)
int m_max_repeats
Max number of trials to send a command.
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > ¤ts)
std::vector< short int > getPositionLimits() const
virtual int getPositions(const int &id, const int &max_repeats, std::vector< short int > &positions)
virtual int setCommandsAsync(const int &id, std::vector< short int > &commands)
bool m_init_done
Flag used to indicate if the device is initialized.
void getCurrent(vpColVector ¤t, const int &id=1)
void setPosition(const vpColVector &position, const int &id=1)
void getPosition(vpColVector &position, const int &id=1)
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()