Visual Servoing Platform  version 3.3.1 under development (2020-12-02)
vpReflexTakktile2.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 Reflex Takktile 2 hand from Right Hand Robotics.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpConfig.h>
40 
41 #ifdef VISP_HAVE_TAKKTILE2
42 
43 #include <reflex_driver2.h>
44 
45 #include <visp3/core/vpMath.h>
46 #include <visp3/robot/vpReflexTakktile2.h>
47 
48 #ifndef DOXYGEN_SHOULD_SKIP_THIS
49 class vpReflexTakktile2::Impl : public reflex_driver2::ReflexDriver
50 {
51 public:
52  Impl() {}
53 
54  ~Impl() {}
55 };
56 
57 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
58 
60 {
61  proximal.resize(NUM_FINGERS);
62  distal_approx.resize(NUM_FINGERS);
63 
64  pressure.resize(NUM_FINGERS);
65  contact.resize(NUM_FINGERS);
66  for (size_t i = 0; i < NUM_FINGERS; i++) {
67  pressure[i].resize(NUM_SENSORS_PER_FINGER);
68  contact[i].resize(NUM_SENSORS_PER_FINGER);
69  }
70 
71  joint_angle.resize(NUM_DYNAMIXELS);
72  raw_angle.resize(NUM_DYNAMIXELS);
73  velocity.resize(NUM_DYNAMIXELS);
74  load.resize(NUM_DYNAMIXELS);
75  voltage.resize(NUM_DYNAMIXELS);
76  temperature.resize(NUM_DYNAMIXELS);
77  error_state.resize(NUM_DYNAMIXELS);
78 }
79 
85 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpReflexTakktile2::HandInfo &hand)
86 {
87  for (size_t i = 0; i < NUM_FINGERS; i++) {
88  os << "Finger " << i + 1 << ": " << std::endl;
89 
90  os << "\tProximal: " << hand.proximal[i] << std::endl;
91  os << "\tDistal Approx: " << hand.distal_approx[i] << std::endl;
92 
93  os << "\tPressures: ";
94  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
95  os << hand.pressure[i][j] << ", ";
96  }
97  os << std::endl;
98 
99  os << "\tContact: ";
100  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
101  os << hand.contact[i][j] << ", ";
102  }
103  os << std::endl;
104 
105  os << "\tJoint Angle: " << hand.joint_angle[i] << " rad" << std::endl;
106  os << "\tJoint Angle: " << vpMath::deg(static_cast<double>(hand.joint_angle[i])) << " deg" << std::endl;
107  os << "\tVelocity: " << hand.velocity[i] << " rad/s" << std::endl;
108  os << "\tVelocity: " << vpMath::deg(static_cast<double>(hand.velocity[i])) << " deg/s" << std::endl;
109  os << "\tError State: " << hand.error_state[i] << std::endl;
110  }
111 
112  os << "Preshape: " << std::endl;
113  os << "\tJoint Angle: " << hand.joint_angle[3] << std::endl;
114  os << "\tVelocity: " << hand.velocity[3] << std::endl;
115  os << "\tError State: " << hand.error_state[3] << std::endl;
116 
117  return os;
118 }
119 
125  m_impl(new Impl())
126 {
127 }
128 
133 
137 void vpReflexTakktile2::calibrate() { m_impl->calibrate(); }
138 
142 void vpReflexTakktile2::disableTorque() { m_impl->disable_torque(); }
143 
148 {
149  for (size_t i = 0; i < NUM_FINGERS; i++) {
150  m_hand_info.proximal[i] = m_impl->hand_info.proximal[i];
151  m_hand_info.distal_approx[i] = m_impl->hand_info.distal_approx[i];
152  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
153  m_hand_info.pressure[i][j] = m_impl->hand_info.pressure[i][j];
154  m_hand_info.contact[i][j] = m_impl->hand_info.contact[i][j];
155  }
156  }
157  for (size_t i = 0; i < NUM_DYNAMIXELS; i++) {
158  m_hand_info.joint_angle[i] = m_impl->hand_info.joint_angle[i];
159  m_hand_info.raw_angle[i] = m_impl->hand_info.raw_angle[i];
160  m_hand_info.velocity[i] = m_impl->hand_info.velocity[i];
161  m_hand_info.load[i] = m_impl->hand_info.load[i];
162  m_hand_info.voltage[i] = m_impl->hand_info.voltage[i];
163  m_hand_info.temperature[i] = m_impl->hand_info.temperature[i];
164  m_hand_info.error_state[i] = m_impl->hand_info.error_state[i];
165  }
166 
167  return m_hand_info;
168 }
169 
173 int vpReflexTakktile2::getNumServos() const { return static_cast<int>(NUM_SERVOS); }
174 
178 int vpReflexTakktile2::getNumFingers() const { return static_cast<int>(NUM_FINGERS); }
179 
183 int vpReflexTakktile2::getNumSensorsPerFinger() const { return static_cast<int>(NUM_SENSORS_PER_FINGER); }
184 
190 {
191  vpColVector position(NUM_SERVOS);
192  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
193  position[i] = static_cast<double>(m_impl->hand_info.joint_angle[i]);
194  }
195  return position;
196 }
197 
203 {
204  vpColVector velocity(NUM_SERVOS);
205  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
206  velocity[i] = static_cast<double>(m_impl->hand_info.velocity[i]);
207  }
208  return velocity;
209 }
210 
220 {
221  if (targets.size() != NUM_SERVOS) {
222  vpException(vpException::dimensionError, "Wrong Takktile 2 position vector dimension (%d) instead of %d.",
223  targets.size(), NUM_SERVOS);
224  }
225  float targets_[NUM_SERVOS];
226  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
227  targets_[i] = static_cast<float>(targets[i]);
228  }
229  m_impl->set_angle_position(targets_);
230 }
231 
238 void vpReflexTakktile2::setTactileThreshold(int threshold) { m_impl->populate_tactile_thresholds(threshold); }
239 
247 void vpReflexTakktile2::setTactileThreshold(const std::vector<int> &thresholds)
248 {
249  if (thresholds.size() != NUM_FINGERS * NUM_SENSORS_PER_FINGER) {
250  vpException(vpException::dimensionError, "Wrong Takktile threshold vector dimension (%d) instead of %d.",
251  thresholds.size(), NUM_FINGERS * NUM_SENSORS_PER_FINGER);
252  }
253  int thresholds_[NUM_FINGERS * NUM_SENSORS_PER_FINGER];
254  for (size_t i = 0; i < NUM_FINGERS * NUM_SENSORS_PER_FINGER; i++) {
255  thresholds_[i] = thresholds[i];
256  }
257 
258  m_impl->set_tactile_thresholds(thresholds_);
259 }
260 
268 {
269  if (targets.size() != NUM_SERVOS) {
270  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
271  targets.size(), NUM_SERVOS);
272  }
273  float targets_[NUM_SERVOS];
274  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
275  targets_[i] = static_cast<float>(targets[i]);
276  }
277  m_impl->set_motor_speed(targets_);
278 }
279 
286 {
287  if (targets.size() != NUM_SERVOS) {
288  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
289  targets.size(), NUM_SERVOS);
290  }
291  float targets_[NUM_SERVOS];
292  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
293  targets_[i] = static_cast<float>(targets[i]);
294  }
295  m_impl->move_until_any_contact(targets_);
296 }
297 
304 {
305  if (targets.size() != NUM_SERVOS) {
306  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
307  targets.size(), NUM_SERVOS);
308  }
309  float targets_[NUM_SERVOS];
310  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
311  targets_[i] = static_cast<float>(targets[i]);
312  }
313  m_impl->move_until_each_contact(targets_);
314 }
315 
320 {
322  reflex_hand2::ReflexHandState *state = &m_impl->rh->rx_state_;
323  m_impl->rh->setStateCallback(std::bind(&reflex_driver2::ReflexDriver::reflex_hand_state_cb, m_impl, state));
324 }
325 
330 void vpReflexTakktile2::wait(int milliseconds) { m_impl->wait(milliseconds); }
331 
332 #endif
std::string m_network_interface
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
std::string m_tactile_file_name
std::vector< float > raw_angle
std::vector< std::string > error_state
std::vector< float > distal_approx
std::vector< float > voltage
void setPositioningVelocity(const vpColVector &targets)
void setVelocityUntilEachContact(const vpColVector &targets)
vpColVector getPosition() const
void wait(int milliseconds)
static double deg(double rad)
Definition: vpMath.h:103
std::string m_finger_file_name
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
std::vector< float > velocity
std::vector< std::vector< bool > > contact
std::string m_motor_file_name
int getNumSensorsPerFinger() const
std::vector< std::vector< int > > pressure
std::vector< float > proximal
void setVelocityUntilAnyContact(const vpColVector &targets)
void setPosition(const vpColVector &targets)
void setTactileThreshold(int threshold)
std::vector< uint32_t > temperature
vpColVector getVelocity() const
std::vector< float > joint_angle
std::vector< float > load