Flexiv TDK APIs 2.1
Loading...
Searching...
No Matches
transparent_cartesian_teleop_lan.cpp
1
12
13#include <flexiv/tdk/data.hpp>
15
16#include <spdlog/spdlog.h>
17
18#include <getopt.h>
19#include <atomic>
20#include <chrono>
21#include <iostream>
22#include <thread>
23#include <optional>
24
25namespace {
27std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
28 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
29
31std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
32 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
33
35const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
36 = {5.0, 5.0, 5.0, 40.0, 40.0, 40.0};
37
39std::atomic<bool> g_running {true};
40
42constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
43
44} // namespace
45
46void PrintHelp()
47{
48 // clang-format off
49 std::cout<<"Invalid program arguments!"<<std::endl;
50 std::cout<<" -l [necessary] serial number of leader robot."<<std::endl;
51 std::cout<<" -f [necessary] serial number of follower robot."<<std::endl;
52 std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
53 std::cout<<"Usage: sudo ./transparent_cartesian_teleop_lan [-l leader_robot_sn] [-f follower_robot_sn] [-D]"<<std::endl;
54 // clang-format on
55}
56
57const struct option kLongOptions[] = {
58 // clang-format off
59 {"leader SN", required_argument, 0, 'l'},
60 {"follower SN", required_argument, 0, 'f'},
61 {"enable digital input", no_argument, 0, 'D'},
62 {0, 0, 0, 0 }
63 // clang-format on
64};
65
69void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
70{
71 while (g_running.load() && !teleop.any_fault()) {
72 try {
73 teleop.Engage(0, kJointGroup, teleop.digital_inputs(0).first[0]);
74 } catch (const std::exception& e) {
75 spdlog::error("Exception in ReadDigitalInputTask: {}", e.what());
76 }
77 std::this_thread::sleep_for(std::chrono::milliseconds(10));
78 }
79 spdlog::info("ReadDigitalInputTask exiting.");
80 return;
81}
85void ConsoleTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
86{
87 auto PrintCommandMenu = []() {
88 std::cout << R"(
89 --- Axis Lock ---
90 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
91 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
92 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
93 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
94
95 --- Teleop Engagement ---
96 r : Engage teleop
97 R : Disengage teleop
98
99 --- Wrench Feedback Scaling ---
100 t : Set wrench feedback scaling to 0.5
101 T : Set wrench feedback scaling to 2.0
102 c : Reset wrench feedback scaling to 1.0 default value
103
104 --- Axis Lock Presets ---
105 u : Unlock all axes (TCP coord)
106 U : Lock all axes (TCP coord)
107
108 --- Null Space Posture ---
109 i/I : Set local nullspace to Preferred/Home posture
110 o/O : Set remote nullspace to Preferred/Home posture
111
112 --- Max Contact Wrench ---
113 p : Set default remote max contact wrench
114
115 --- Repulsive Force ---
116 a : Set repulsive force to {5, 0, 0}
117 A : Set repulsive force to {-5, 0, 0}
118 C : Clear force
119
120 --- Start/Stop ---
121 b : Stop teleop
122 B : Start teleop
123
124
125 --- Is teleop stopped or not ---
126 s : Query if teleop stopped or not
127
128 --- Help ---
129 Any other key to show this help menu
130 )" << std::endl;
131 };
132
133 unsigned int index = 0;
135 teleop.GetAxisLockState(index, kJointGroup, cmd);
136
137 while (g_running.load() && !teleop.any_fault()) {
138
139 std::string userInput {};
140
141 // Get user input
142 std::getline(std::cin, userInput);
143
144 if (userInput.empty()) {
145 spdlog::warn("Empty command!");
146 PrintCommandMenu();
147 continue;
148 }
149
150 try {
151 switch (userInput[0]) {
152 case 'x':
153 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
155 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
156 break;
157 case 'y':
158 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
160 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
161 break;
162 case 'z':
163 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
165 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
166 break;
167 case 'q':
168 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
170 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
171 break;
172 case 'w':
173 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
175 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
176 break;
177 case 'e':
178 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
180 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
181 break;
182
183 case 'X':
184 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
186 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
187 break;
188 case 'Y':
189 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
191 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
192 break;
193 case 'Z':
194 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
196 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
197 break;
198 case 'Q':
199 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
201 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
202 break;
203 case 'W':
204 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
206 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
207 break;
208 case 'E':
209 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
211 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
212 break;
213
214 case 'r':
215 teleop.Engage(index, kJointGroup, true);
216 break;
217 case 'R':
218 teleop.Engage(index, kJointGroup, false);
219 break;
220
221 case 't':
222 teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 0.5);
223 break;
224 case 'T':
225 teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 2);
226 break;
227
228 case 'u':
229 cmd.lock_ori_axis = {false, false, false};
230 cmd.lock_trans_axis = {false, false, false};
232 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
233 break;
234 case 'U':
235 cmd.lock_ori_axis = {true, true, true};
236 cmd.lock_trans_axis = {true, true, true};
238 teleop.SetAxisLockCmd(index, kJointGroup, cmd);
239 break;
240 case 'i':
241 teleop.SetLeaderNullSpacePosture(index, kJointGroup, kPreferredJntPos);
242 break;
243 case 'I':
244 teleop.SetLeaderNullSpacePosture(index, kJointGroup, kHomeJntPos);
245 break;
246 case 'o':
247 teleop.SetFollowerNullSpacePosture(index, kJointGroup, kPreferredJntPos);
248 break;
249 case 'O':
250 teleop.SetFollowerNullSpacePosture(index, kJointGroup, kHomeJntPos);
251 break;
252 case 'p':
253 teleop.SetFollowerMaxContactWrench(index, kJointGroup, kDefaultMaxContactWrench);
254 break;
255
256 case 'a':
257 teleop.SetRepulsiveForce(index, kJointGroup, {5, 0, 0});
258 break;
259 case 'A':
260 teleop.SetRepulsiveForce(index, kJointGroup, {-5, 0, 0});
261 break;
262 case 'b':
263 teleop.Stop();
264 break;
265 case 'B':
266 teleop.Init();
267 teleop.Start();
268 break;
269 case 'c':
270 teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 1);
271 break;
272 case 'C':
273 teleop.SetRepulsiveForce(index, kJointGroup, {0, 0, 0});
274 break;
275 case 's':
276 teleop.stopped(0) ? spdlog::info("Teleop pair 0 stopped")
277 : spdlog::info("Teleop pair 0 started");
278 break;
279 default:
280 spdlog::warn("Invalid command!");
281 PrintCommandMenu();
282 break;
283 }
284 } catch (const std::exception& e) {
285 spdlog::error("Exception in ConsoleTask: {}", e.what());
286 g_running.store(false);
287 return;
288 }
289 }
290 spdlog::info("Console thread exiting.");
291 return;
292}
293
294int main(int argc, char* argv[])
295{
296 std::string follower_sn {};
297 std::string leader_sn {};
298 int opt = 0;
299 int longIndex = 0;
300 bool enable_digital_input = false;
301
302 while ((opt = getopt_long_only(argc, argv, "f:l:D", kLongOptions, &longIndex)) != -1) {
303 switch (opt) {
304 case 'f':
305 follower_sn = std::string(optarg);
306 break;
307 case 'l':
308 leader_sn = std::string(optarg);
309 break;
310 case 'D':
311 enable_digital_input = true;
312 break;
313 default:
314 PrintHelp();
315 return 1;
316 }
317 }
318 if (leader_sn.empty() || follower_sn.empty()) {
319 PrintHelp();
320 return 1;
321 }
322 try {
323
324 // Create teleop control interface
325 flexiv::tdk::TransparentCartesianTeleopLAN tctl({{leader_sn, follower_sn}});
326
327 // Run initialization sequence
328 tctl.Init();
329
330 // Start control loop
331 tctl.Start();
332
333 // Start console and pedal input threads
334 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
335
336 // Start pedal_thread based on the new flag or the role
337 std::optional<std::thread> pedal_thread;
338
339 if (enable_digital_input) {
340 spdlog::info("Starting ReadDigitalInputTask thread.");
341 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
342 } else {
343 spdlog::info("ReadDigitalInputTask thread NOT started (-D flag not provided).");
344 }
345
346 // Wait for threads to finish
347 console_thread.join();
348
349 // Stop all threads, notify other threads exit
350 g_running.store(false);
351
352 // Wait for digital reading task exit
353 if (pedal_thread && pedal_thread->joinable()) {
354 pedal_thread->join();
355 }
356
357 // Exit high transparency teleop
358 tctl.Stop();
359
360 } catch (const std::exception& e) {
361 spdlog::error(e.what());
362 return 1;
363 }
364
365 return 0;
366}
Teleoperation control interface to run Cartesian-space teleoperation for one or more pairs of robots ...
bool any_fault(void) const
[Non-blocking] Whether any of the connected robots is in fault state.
void GetAxisLockState(unsigned int idx, JointGroup group, AxisLock &data)
[Non-blocking] Get the leader robot axis locking status
void Stop()
[Blocking] Stop the teleoperation control loop and make all robots hold their pose.
void Init(bool limit_wrist_singular=true, ZeroFTSensor zero_ft_sensor=ZeroFTSensor::Enable)
[Blocking] Get all robots ready for teleoperation. The following actions will happen in sequence: a) ...
void Engage(unsigned int idx, JointGroup group, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot. TransparentCartesianTeleopLAN supports...
const std::pair< std::array< bool, kIOPorts >, std::array< bool, kIOPorts > > digital_inputs(unsigned int idx) const
[Non-blocking] Current reading from all digital input ports (16 on the control box + 2 inside the wri...
void SetRepulsiveForce(unsigned int idx, JointGroup group, const std::array< double, 3 > &repulsive_force, bool in_world=true)
[Non-blocking] Set the repulsive force in World or Tcp frame of the follower robot.
void SetAxisLockCmd(unsigned int idx, JointGroup group, const AxisLock &cmd)
[Non-blocking] Set the leader robot axis locking command.
void Start()
[Non-Blocking] Start the teleoperation control loop for all robot pairs.
bool stopped(unsigned int idx) const
[Non-blocking] Whether teleop process has stopped. After teleop is started, the teleop process may st...
void SetWrenchFeedbackScalingFactor(unsigned int idx, JointGroup group, double factor=1.0)
[Non-blocking] Set the wrench feedback scaling factor.
void SetFollowerMaxContactWrench(unsigned int idx, JointGroup group, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the follower robot of specified robot pair....
void SetFollowerNullSpacePosture(unsigned int idx, JointGroup group, const std::vector< double > &ref_joint_positions)
[Blocking] Set reference joint positions used in the robot's null-space posture control module for th...
void SetLeaderNullSpacePosture(unsigned int idx, JointGroup group, const std::vector< double > &ref_joint_positions)
[Blocking] Set reference joint positions used in the robot's null-space posture control module for th...
Header file containing various constant expressions, data structs, and enums.
@ COORD_WORLD
Definition data.hpp:197
Data for locking axis, including reference frame and axis to be locked for transparent cartesian tele...
Definition data.hpp:224
std::array< bool, 3 > lock_ori_axis
Orientation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:240
std::array< bool, 3 > lock_trans_axis
Translation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:234
CoordType coord
Reference coordinate that the axis to be locked.
Definition data.hpp:228