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