Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#ifndef DOXYGEN_SHOULD_SKIP_THIS
46class vpReflexTakktile2::Impl : public reflex_driver2::ReflexDriver
47{
48public:
49 Impl() {}
50
51 ~Impl() {}
52};
53
54#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
55
57{
58 proximal.resize(NUM_FINGERS);
59 distal_approx.resize(NUM_FINGERS);
60
61 pressure.resize(NUM_FINGERS);
62 contact.resize(NUM_FINGERS);
63 for (size_t i = 0; i < NUM_FINGERS; i++) {
64 pressure[i].resize(NUM_SENSORS_PER_FINGER);
65 contact[i].resize(NUM_SENSORS_PER_FINGER);
66 }
67
68 joint_angle.resize(NUM_DYNAMIXELS);
69 raw_angle.resize(NUM_DYNAMIXELS);
70 velocity.resize(NUM_DYNAMIXELS);
71 load.resize(NUM_DYNAMIXELS);
72 voltage.resize(NUM_DYNAMIXELS);
73 temperature.resize(NUM_DYNAMIXELS);
74 error_state.resize(NUM_DYNAMIXELS);
75}
76
82VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpReflexTakktile2::HandInfo &hand)
83{
84 for (size_t i = 0; i < NUM_FINGERS; i++) {
85 os << "Finger " << i + 1 << ": " << std::endl;
86
87 os << "\tProximal: " << hand.proximal[i] << std::endl;
88 os << "\tDistal Approx: " << hand.distal_approx[i] << std::endl;
89
90 os << "\tPressures: ";
91 for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
92 os << hand.pressure[i][j] << ", ";
93 }
94 os << std::endl;
95
96 os << "\tContact: ";
97 for (size_t j = 0; j < NUM_SENSORS_PER_FINGER; j++) {
98 os << hand.contact[i][j] << ", ";
99 }
100 os << std::endl;
101
102 os << "\tJoint Angle: " << hand.joint_angle[i] << " rad" << std::endl;
103 os << "\tJoint Angle: " << vpMath::deg(static_cast<double>(hand.joint_angle[i])) << " deg" << std::endl;
104 os << "\tVelocity: " << hand.velocity[i] << " rad/s" << std::endl;
105 os << "\tVelocity: " << vpMath::deg(static_cast<double>(hand.velocity[i])) << " deg/s" << std::endl;
106 os << "\tError State: " << hand.error_state[i] << std::endl;
107 }
108
109 os << "Preshape: " << std::endl;
110 os << "\tJoint Angle: " << hand.joint_angle[3] << std::endl;
111 os << "\tVelocity: " << hand.velocity[3] << std::endl;
112 os << "\tError State: " << hand.error_state[3] << std::endl;
113
114 return os;
115}
116
125
130
134void vpReflexTakktile2::calibrate() { m_impl->calibrate(); }
135
139void 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
170int vpReflexTakktile2::getNumServos() const { return static_cast<int>(NUM_SERVOS); }
171
175int vpReflexTakktile2::getNumFingers() const { return static_cast<int>(NUM_FINGERS); }
176
180int 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
235void vpReflexTakktile2::setTactileThreshold(int threshold) { m_impl->populate_tactile_thresholds(threshold); }
236
244void 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
327void vpReflexTakktile2::wait(int milliseconds) { m_impl->wait(milliseconds); }
328
329#endif
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ dimensionError
Bad dimension.
Definition vpException.h:83
static double deg(double rad)
Definition vpMath.h:106
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 > 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)