Visual Servoing Platform version 3.5.0
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
49class vpReflexTakktile2::Impl : public reflex_driver2::ReflexDriver
50{
51public:
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
85VISP_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
137void vpReflexTakktile2::calibrate() { m_impl->calibrate(); }
138
142void 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
173int vpReflexTakktile2::getNumServos() const { return static_cast<int>(NUM_SERVOS); }
174
178int vpReflexTakktile2::getNumFingers() const { return static_cast<int>(NUM_FINGERS); }
179
183int 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
238void vpReflexTakktile2::setTactileThreshold(int threshold) { m_impl->populate_tactile_thresholds(threshold); }
239
247void 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
330void vpReflexTakktile2::wait(int milliseconds) { m_impl->wait(milliseconds); }
331
332#endif
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
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
static double deg(double rad)
Definition: vpMath.h:103
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)