Visual Servoing Platform  version 3.6.1 under development (2024-11-14)
vpReflexTakktile2.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  * Interface for the Reflex Takktile 2 hand from Right Hand Robotics.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_TAKKTILE2
39 
40 #include <reflex_driver2.h>
41 
42 #include <visp3/core/vpMath.h>
43 #include <visp3/robot/vpReflexTakktile2.h>
44 
45 BEGIN_VISP_NAMESPACE
46 #ifndef DOXYGEN_SHOULD_SKIP_THIS
47 class vpReflexTakktile2::Impl : public reflex_driver2::ReflexDriver
48 {
49 public:
50  Impl() { }
51 
52  ~Impl() { }
53 };
54 
55 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
56 
58 {
59  proximal.resize(NUM_FINGERS);
60  distal_approx.resize(NUM_FINGERS);
61 
62  pressure.resize(NUM_FINGERS);
63  contact.resize(NUM_FINGERS);
64  for (size_t i = 0; i < NUM_FINGERS; i++) {
65  pressure[i].resize(NUM_SENSORS_PER_FINGER);
66  contact[i].resize(NUM_SENSORS_PER_FINGER);
67  }
68 
69  joint_angle.resize(NUM_DYNAMIXELS);
70  raw_angle.resize(NUM_DYNAMIXELS);
71  velocity.resize(NUM_DYNAMIXELS);
72  load.resize(NUM_DYNAMIXELS);
73  voltage.resize(NUM_DYNAMIXELS);
74  temperature.resize(NUM_DYNAMIXELS);
75  error_state.resize(NUM_DYNAMIXELS);
76 }
77 
83 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpReflexTakktile2::HandInfo &hand)
84 {
85  for (size_t i = 0; i < NUM_FINGERS; i++) {
86  os << "Finger " << i + 1 << ": " << std::endl;
87 
88  os << "\tProximal: " << hand.proximal[i] << std::endl;
89  os << "\tDistal Approx: " << hand.distal_approx[i] << std::endl;
90 
91  os << "\tPressures: ";
92  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
93  os << hand.pressure[i][j] << ", ";
94  }
95  os << std::endl;
96 
97  os << "\tContact: ";
98  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
99  os << hand.contact[i][j] << ", ";
100  }
101  os << std::endl;
102 
103  os << "\tJoint Angle: " << hand.joint_angle[i] << " rad" << std::endl;
104  os << "\tJoint Angle: " << vpMath::deg(static_cast<double>(hand.joint_angle[i])) << " deg" << std::endl;
105  os << "\tVelocity: " << hand.velocity[i] << " rad/s" << std::endl;
106  os << "\tVelocity: " << vpMath::deg(static_cast<double>(hand.velocity[i])) << " deg/s" << std::endl;
107  os << "\tError State: " << hand.error_state[i] << std::endl;
108  }
109 
110  os << "Preshape: " << std::endl;
111  os << "\tJoint Angle: " << hand.joint_angle[3] << std::endl;
112  os << "\tVelocity: " << hand.velocity[3] << std::endl;
113  os << "\tError State: " << hand.error_state[3] << std::endl;
114 
115  return os;
116 }
117 
123  m_impl(new Impl())
124 { }
125 
130 
134 void vpReflexTakktile2::calibrate() { m_impl->calibrate(); }
135 
139 void vpReflexTakktile2::disableTorque() { m_impl->disable_torque(); }
140 
145 {
146  for (size_t i = 0; i < NUM_FINGERS; i++) {
147  m_hand_info.proximal[i] = m_impl->hand_info.proximal[i];
148  m_hand_info.distal_approx[i] = m_impl->hand_info.distal_approx[i];
149  for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
150  m_hand_info.pressure[i][j] = m_impl->hand_info.pressure[i][j];
151  m_hand_info.contact[i][j] = m_impl->hand_info.contact[i][j];
152  }
153  }
154  for (size_t i = 0; i < NUM_DYNAMIXELS; i++) {
155  m_hand_info.joint_angle[i] = m_impl->hand_info.joint_angle[i];
156  m_hand_info.raw_angle[i] = m_impl->hand_info.raw_angle[i];
157  m_hand_info.velocity[i] = m_impl->hand_info.velocity[i];
158  m_hand_info.load[i] = m_impl->hand_info.load[i];
159  m_hand_info.voltage[i] = m_impl->hand_info.voltage[i];
160  m_hand_info.temperature[i] = m_impl->hand_info.temperature[i];
161  m_hand_info.error_state[i] = m_impl->hand_info.error_state[i];
162  }
163 
164  return m_hand_info;
165 }
166 
170 int vpReflexTakktile2::getNumServos() const { return static_cast<int>(NUM_SERVOS); }
171 
175 int vpReflexTakktile2::getNumFingers() const { return static_cast<int>(NUM_FINGERS); }
176 
180 int vpReflexTakktile2::getNumSensorsPerFinger() const { return static_cast<int>(NUM_SENSORS_PER_FINGER); }
181 
187 {
188  vpColVector position(NUM_SERVOS);
189  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
190  position[i] = static_cast<double>(m_impl->hand_info.joint_angle[i]);
191  }
192  return position;
193 }
194 
200 {
201  vpColVector velocity(NUM_SERVOS);
202  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
203  velocity[i] = static_cast<double>(m_impl->hand_info.velocity[i]);
204  }
205  return velocity;
206 }
207 
217 {
218  if (targets.size() != NUM_SERVOS) {
219  vpException(vpException::dimensionError, "Wrong Takktile 2 position vector dimension (%d) instead of %d.",
220  targets.size(), NUM_SERVOS);
221  }
222  float targets_[NUM_SERVOS];
223  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
224  targets_[i] = static_cast<float>(targets[i]);
225  }
226  m_impl->set_angle_position(targets_);
227 }
228 
235 void vpReflexTakktile2::setTactileThreshold(int threshold) { m_impl->populate_tactile_thresholds(threshold); }
236 
244 void vpReflexTakktile2::setTactileThreshold(const std::vector<int> &thresholds)
245 {
246  if (thresholds.size() != NUM_FINGERS * NUM_SENSORS_PER_FINGER) {
247  vpException(vpException::dimensionError, "Wrong Takktile threshold vector dimension (%d) instead of %d.",
248  thresholds.size(), NUM_FINGERS * NUM_SENSORS_PER_FINGER);
249  }
250  int thresholds_[NUM_FINGERS * NUM_SENSORS_PER_FINGER];
251  for (size_t i = 0; i < NUM_FINGERS * NUM_SENSORS_PER_FINGER; i++) {
252  thresholds_[i] = thresholds[i];
253  }
254 
255  m_impl->set_tactile_thresholds(thresholds_);
256 }
257 
265 {
266  if (targets.size() != NUM_SERVOS) {
267  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
268  targets.size(), NUM_SERVOS);
269  }
270  float targets_[NUM_SERVOS];
271  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
272  targets_[i] = static_cast<float>(targets[i]);
273  }
274  m_impl->set_motor_speed(targets_);
275 }
276 
283 {
284  if (targets.size() != NUM_SERVOS) {
285  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
286  targets.size(), NUM_SERVOS);
287  }
288  float targets_[NUM_SERVOS];
289  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
290  targets_[i] = static_cast<float>(targets[i]);
291  }
292  m_impl->move_until_any_contact(targets_);
293 }
294 
301 {
302  if (targets.size() != NUM_SERVOS) {
303  vpException(vpException::dimensionError, "Wrong Takktile 2 velocity vector dimension (%d) instead of %d.",
304  targets.size(), NUM_SERVOS);
305  }
306  float targets_[NUM_SERVOS];
307  for (unsigned int i = 0; i < NUM_SERVOS; i++) {
308  targets_[i] = static_cast<float>(targets[i]);
309  }
310  m_impl->move_until_each_contact(targets_);
311 }
312 
317 {
319  reflex_hand2::ReflexHandState *state = &m_impl->rh->rx_state_;
320  m_impl->rh->setStateCallback(std::bind(&reflex_driver2::ReflexDriver::reflex_hand_state_cb, m_impl, state));
321 }
322 
327 void vpReflexTakktile2::wait(int milliseconds) { m_impl->wait(milliseconds); }
328 END_VISP_NAMESPACE
329 #endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
static double deg(double rad)
Definition: vpMath.h:119
std::vector< uint32_t > temperature
std::vector< float > joint_angle
std::vector< std::vector< bool > > contact
std::vector< std::vector< int > > pressure
std::vector< std::string > error_state
std::vector< float > load
std::vector< float > raw_angle
std::vector< float > proximal
std::vector< float > voltage
std::vector< float > distal_approx
std::vector< float > velocity
void setVelocityUntilAnyContact(const vpColVector &targets)
std::string m_finger_file_name
void setVelocityUntilEachContact(const vpColVector &targets)
std::string m_tactile_file_name
void setTactileThreshold(int threshold)
std::string m_network_interface
vpColVector getVelocity() const
void setPosition(const vpColVector &targets)
void setPositioningVelocity(const vpColVector &targets)
std::string m_motor_file_name
vpColVector getPosition() const
int getNumSensorsPerFinger() const
void wait(int milliseconds)