Visual Servoing Platform version 3.5.0
takktile2-control.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
46#include <string>
47#include <iostream>
48
49#include <visp3/robot/vpReflexTakktile2.h>
50
51int main(int argc, char *argv[])
52{
53 std::string opt_network_interface = "eth0";
54 std::string opt_finger_file_name = "yaml/finger_calibrate.yaml";
55 std::string opt_tactile_file_name = "yaml/tactile_calibrate.yaml";
56 std::string opt_motor_file_name = "yaml/motor_constants.yaml";
57
58 for (int i = 0; i < argc; i++) {
59 if (std::string(argv[i]) == "--network")
60 opt_network_interface = std::string(argv[i + 1]);
61 else if (std::string(argv[i]) == "--finger")
62 opt_finger_file_name = std::string(argv[i + 1]);
63 else if (std::string(argv[i]) == "--tactile")
64 opt_tactile_file_name = atoi(argv[i + 1]);
65 else if (std::string(argv[i]) == "--motor")
66 opt_motor_file_name = atoi(argv[i + 1]);
67 else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
68 std::cout << "\nUsage: " << argv[0]
69 << " [--network <interface name>] "
70 " [--finger <calib file name>]"
71 " [--tactile <calib file name>]"
72 " [--motor <constants file name>]"
73 " [--help] [-h]\n"
74 << std::endl;
75 std::cout << "Options:" << std::endl;
76 std::cout << " --network <interface name>" << std::endl;
77 std::cout << "\tNetwork interface name. Default: " << opt_network_interface << std::endl << std::endl;
78 std::cout << " --finger <calib file name>" << std::endl;
79 std::cout << "\tFinger calibration file name. Default: " << opt_finger_file_name << std::endl << std::endl;
80 std::cout << " --tactile <calib file name>" << std::endl;
81 std::cout << "\tTactile calibration file name. Default: " << opt_tactile_file_name << std::endl << std::endl;
82 std::cout << " --motor <constants file name>" << std::endl;
83 std::cout << "\tMotor constants file name. Default: " << opt_motor_file_name << std::endl << std::endl;
84 std::cout << " --help, -h" << std::endl;
85 std::cout << "\tPrint this helper." << std::endl;
86
87 return 0;
88 }
89 }
90#ifdef VISP_HAVE_TAKKTILE2
91 vpReflexTakktile2 reflex;
92 reflex.setNetworkInterface(opt_network_interface);
93 reflex.setFingerConfigFile(opt_finger_file_name);
94 reflex.setTactileConfigFile(opt_tactile_file_name);
95 reflex.setMotorConfigFile(opt_motor_file_name);
96
97 reflex.open();
98
99 /****************************************************/
100 reflex.calibrate();
101 reflex.wait(100);
102
103 std::cout << "Calibration complete" << std::endl;
104 reflex.wait(500);
105
106 /****************************************************/
107 // Demonstration of position control
108
109 std::cout << "Moving fingers..." << std::endl;
110
111 std::vector<vpColVector> finger_positions = {
112 {0.0, 0.0, 0.0, 0.0}, {0.5, 0.5, 0.5, 0.0}, {1.0, 1.0, 1.0, 0.0}, {0.5, 0.5, 0.5, 0.0}, {0.0, 0.0, 0.0, 0.0}};
113
114 for (size_t i = 0; i < finger_positions.size(); i++) {
115 reflex.setPosition(finger_positions[i]);
116 reflex.wait(500);
117 }
118
119 std::cout << "Finger movement complete" << std::endl;
120 reflex.wait(500);
121
122 /****************************************************/
123 // Demonstration of preshape joint
124
125 std::cout << "Moving preshape joint..." << std::endl;
126
127 std::vector<vpColVector> preshape_positions = {
128 {0.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.5}, {0.0, 0.0, 0.0, 1.5}, {0.0, 0.0, 0.0, 0.5}, {0.0, 0.0, 0.0, 0.0}};
129
130 for (size_t i = 0; i < preshape_positions.size(); i++) {
131 reflex.setPosition(preshape_positions[i]);
132 reflex.wait(500);
133 }
134
135 std::cout << "Preshape movement complete" << std::endl;
136 reflex.wait(500);
137
138 /****************************************************/
139 // Demonstration of changing velocity
140
141 std::cout << "Changing to faster speed..." << std::endl;
142 vpColVector faster_velocities = {7.0, 7.0, 7.0, 7.0};
143 reflex.setPositioningVelocity(faster_velocities);
144
145 for (size_t i = 0; i < finger_positions.size(); i++) {
146 reflex.setPosition(finger_positions[i]);
147 reflex.wait(500);
148 }
149
150 std::cout << "Changing to slower speed..." << std::endl;
151 vpColVector slower_velocities = {0.5, 0.5, 0.5, 0.5};
152 reflex.setPositioningVelocity(slower_velocities);
153
154 for (size_t i = 0; i < finger_positions.size(); i++) {
155 reflex.setPosition(finger_positions[i]);
156 reflex.wait(500);
157 }
158
159 std::cout << "Done changing speeds" << std::endl;
160 reflex.wait(500);
161
162 /****************************************************/
163 // Demonstration of blended control
164
165 std::cout << "Starting blended control..." << std::endl;
166 vpColVector mixed_speeds = {0.5, 1.0, 1.5, 0.0};
167 reflex.setPositioningVelocity(mixed_speeds);
168
169 for (int i = 0; i < 5; i++) {
170 reflex.setPosition(finger_positions[i]);
171 reflex.wait(500);
172 }
173
174 reflex.setPositioningVelocity(slower_velocities);
175
176 std::cout << "Blended control complete" << std::endl;
177 reflex.wait(500);
178
179 /****************************************************/
180 // Demonstration of tactile feedback and setting sensor thresholds
181
182 std::cout << "Starting tactile feedback..." << std::endl
183 << "All fingers will stop moving when any one makes contact" << std::endl;
184 vpColVector move_speeds = {1.25, 1.25, 1.25, 0};
185
186 reflex.setTactileThreshold(15);
187
188 reflex.setVelocityUntilAnyContact(move_speeds);
189 reflex.wait(1000);
190
191 reflex.setPosition(finger_positions[0]);
192 reflex.wait(500);
193
194 /****************************************************/
195 std::cout << "Now each finger will only stop moving once it makes contact" << std::endl;
196
197 std::vector<int> thresholds = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 20, 20, 20, 20, 20, 20, 20,
198 20, 20, 20, 20, 20, 20, 20, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30};
199
200 reflex.setTactileThreshold(thresholds);
201
202 reflex.setVelocityUntilEachContact(move_speeds);
203 reflex.wait(1000);
204
205 reflex.setPosition(finger_positions[0]);
206 reflex.wait(500);
207
208 /****************************************************/
209 reflex.disableTorque();
210 std::cout << "All torques disabled" << std::endl;
211
212 std::cout << "The end" << std::endl;
213
214#else
215 std::cout << "ViSP is not built to support Right Hand Reflex Takktile2 hand" << std::endl;
216#endif
217
218 return EXIT_SUCCESS;
219}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void setVelocityUntilAnyContact(const vpColVector &targets)
void setVelocityUntilEachContact(const vpColVector &targets)
void setTactileThreshold(int threshold)
void setMotorConfigFile(const std::string &motor_file_name)
void setPosition(const vpColVector &targets)
void setTactileConfigFile(const std::string &tactile_file_name)
void setPositioningVelocity(const vpColVector &targets)
void setNetworkInterface(const std::string &network_interface="eth0")
void setFingerConfigFile(const std::string &finger_file_name)
void wait(int milliseconds)