Flexiv TDK APIs 1.7
Loading...
Searching...
No Matches
transparent_cartesian_teleop_wan.cpp
1
10
11#include <flexiv/tdk/data.hpp>
13
14#include <getopt.h>
15#include <algorithm>
16#include <array>
17#include <atomic>
18#include <chrono>
19#include <cmath>
20#include <iomanip>
21#include <iostream>
22#include <optional>
23#include <sstream>
24#include <string>
25#include <thread>
26#include <functional>
27
28namespace {
29
31std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
32 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
33
35std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
36 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
37
39const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
40 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
41
43std::atomic<bool> g_running {true};
44
47}
48
49std::string BoolStr(bool v)
50{
51 return v ? "true" : "false";
52}
53
54std::string FmtFixed(double v, int prec)
55{
56 std::ostringstream oss;
57 oss << std::fixed << std::setprecision(prec) << v;
58 return oss.str();
59}
60
61void LogInfo(const std::string& msg)
62{
63 std::cout << "[info] " << msg << std::endl;
64}
65
66void LogWarn(const std::string& msg)
67{
68 std::cerr << "[warn] " << msg << std::endl;
69}
70
71void LogError(const std::string& msg)
72{
73 std::cerr << "[error] " << msg << std::endl;
74}
75
76bool HasActiveIssue(const flexiv::tdk::TeleopStatus& status, flexiv::tdk::TeleopIssueCode code)
77{
78 return std::any_of(status.issues.begin(), status.issues.end(),
79 [code](const flexiv::tdk::TeleopIssue& issue) { return issue.code == code; });
80}
81
82void PrintTeleopStatus(unsigned int idx, const flexiv::tdk::TeleopStatus& status)
83{
84 LogInfo(
85 "Teleop pair " + std::to_string(idx) + " status: initialized=" + BoolStr(status.initialized)
86 + " started=" + BoolStr(status.started) + " engaged=" + BoolStr(status.engaged)
87 + " stopped=" + BoolStr(status.stopped) + " fault=" + BoolStr(status.fault)
88 + " motion_restricted=" + BoolStr(status.motion_restricted) + " latency="
89 + FmtFixed(status.latency_ms, 1) + "/" + FmtFixed(status.latency_threshold_ms, 1) + " ms");
91 LogInfo("No teleop restriction. Safe to continue.");
92 return;
93 }
94 const auto& issue = status.primary;
95 LogWarn(std::string("[") + flexiv::tdk::TeleopIssueLevelStr[static_cast<size_t>(issue.level)]
96 + "/" + flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(issue.side)] + "] "
97 + issue.title + " | " + issue.description + " | " + issue.suggestion);
98 for (const auto& extra : status.issues) {
99 if (extra.code == issue.code && extra.side == issue.side
100 && extra.joint_index == issue.joint_index) {
101 continue;
102 }
103 LogWarn(std::string(" also: [")
104 + flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(extra.code)] + "/"
105 + flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(extra.side)] + "] "
106 + extra.title);
107 }
108}
109
110void PrintHelp()
111{
112 // clang-format off
113 std::cout<<"Invalid program arguments!"<<std::endl;
114 std::cout<<" -l [necessary] serial number of leader robot."<<std::endl;
115 std::cout<<" -f [necessary] serial number of follower robot."<<std::endl;
116 std::cout<<" -r [necessary] Role of participants in teleop. can be [follower] or [leader]"<<std::endl;
117 std::cout<<" -t [necessary] Role in the TCP connection, can be [server] or [client]."<<std::endl;
118 std::cout<<" -i [necessary] Public IPV4 address of the machine that functions as TCP server."<<std::endl;
119 std::cout<<" -p [necessary] Listening port of the TCP server machine."<<std::endl;
120 std::cout<<" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
121 std::cout<<" -W [optional] WAN interface name, e.g. wlo1 or enp3s0. Repeatable." << std::endl;
122 std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
123 std::cout<<"Usage: sudo ./transparent_cartesian_teleop_wan -l <leader_sn> -f <follower_sn> -r leader|follower -t server|client -i <public_ip> -p <port> [-A <lan_ipv4>] [-W <iface>] [-D]"<<std::endl;
124 // clang-format on
125}
126
127const struct option kLongOptions[] = {
128 // clang-format off
129 {"leader SN", required_argument, 0, 'l'},
130 {"follower SN", required_argument, 0, 'f'},
131 {"role", required_argument, 0, 'r'},
132 {"tcp role", required_argument, 0, 't'},
133 {"public ipv4 address", required_argument, 0, 'i'},
134 {"port", required_argument, 0, 'p'},
135 {"lan whitelist ip", required_argument, 0, 'A'},
136 {"wan interface name", required_argument, 0, 'W'},
137 {"enable digital input", no_argument, 0, 'D'},
138 {0, 0, 0, 0 }
139 // clang-format on
140};
141
145void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
146{
147 bool logged_idle = false;
148 std::string last_error;
149 while (g_running.load()) {
150 try {
151 const auto status = teleop.GetTeleopStatus(0);
152 // Robot fault / Stop() drops teleop back to not-started. Engage() is
153 // invalid in that state and must not be polled every cycle.
154 if (!status.started || status.stopped) {
155 if (!logged_idle) {
156 LogWarn(
157 "ReadDigitalInputTask: teleop is not started, pause Engage "
158 "(call Init + Start to resume)");
159 logged_idle = true;
160 }
161 } else {
162 logged_idle = false;
163 teleop.Engage(0, teleop.digital_inputs(0)[0]);
164 }
165 last_error.clear();
166 } catch (const std::exception& e) {
167 if (last_error != e.what()) {
168 LogError(std::string("Exception in ReadDigitalInputTask: ") + e.what());
169 last_error = e.what();
170 }
171 }
172 std::this_thread::sleep_for(std::chrono::milliseconds(10));
173 }
174 LogInfo("ReadDigitalInputTask exiting.");
175 return;
176}
177
181void FaultMonitorTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
182{
183 bool last_fault = false;
184 std::string last_error;
185 while (g_running.load()) {
186 try {
187 const bool fault = teleop.fault(0);
188 const bool fault_changed = (fault != last_fault);
189 last_fault = fault;
190 if (fault_changed && fault) {
191 LogError("Fault detected. Press 'h' for details, 'F' to clear fault, then 'u' to "
192 "restart teleop.");
193 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
194 } else if (fault_changed) {
195 LogInfo("Fault cleared. Press 'u' to restart teleop.");
196 }
197 last_error.clear();
198 } catch (const std::exception& e) {
199 if (last_error != e.what()) {
200 LogError(std::string("Exception in FaultMonitorTask: ") + e.what());
201 last_error = e.what();
202 }
203 }
204 std::this_thread::sleep_for(std::chrono::milliseconds(100));
205 }
206 LogInfo("FaultMonitorTask exiting.");
207 return;
208}
209
213void ConsoleTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
214{
215 auto PrintCommandMenu = []() {
216 std::cout << R"(
217 --- Axis Lock (leader only) ---
218 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
219 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
220 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
221 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
222 a : Unlock all axes (TCP coord)
223 A : Lock all axes (TCP coord)
224
225 --- Teleop Engagement ---
226 r : Engage teleop
227 R : Disengage teleop
228
229 --- Null Space Posture ---
230 i/I : Set local nullspace to Preferred/Home posture
231
232 --- Max Contact Wrench ---
233 p : Set default max contact wrench
234
235 --- Reinit and start ---
236 u : Recall Init and Start
237 U : Stop teleop
238
239 --- TCP message latency ---
240 l : Print current message latency (disconnected / clock mismatch / over limit)
241
242 --- Teleop status ---
243 h : Print why teleop is restricted / paused and what to do next
244
245 --- Fault recovery ---
246 F : Clear fault (blocking, up to 30 s), then press 'u' to restart teleop
247
248 --- Identity ---
249 n : Print role() and robot_pair_sn()
250
251 --- Exit ---
252 quit : Stop teleop and exit the program
253
254 --- Help ---
255 Any other key to show this help menu
256 )" << std::endl;
257 };
258
259 unsigned int index = 0;
260 const bool is_leader = (teleop.role() == flexiv::tdk::Role::WAN_TELEOP_LEADER);
262 if (is_leader) {
263 teleop.GetAxisLockState(index, cmd);
264 }
265
266 auto apply_axis_lock = [&]() {
267 if (!is_leader) {
268 LogWarn("Axis lock is only available on the leader");
269 return;
270 }
271 teleop.SetAxisLockCmd(index, cmd);
272 };
273
274 while (g_running.load()) {
275
276 std::string user_input {};
277
278 // Get user input, exit when stdin is closed
279 if (!std::getline(std::cin, user_input)) {
280 LogInfo("Console input closed.");
281 break;
282 }
283
284 if (user_input.empty()) {
285 LogWarn("Empty command!");
286 PrintCommandMenu();
287 continue;
288 }
289
290 if (user_input == "quit") {
291 LogInfo("Quit requested.");
292 break;
293 }
294
295 try {
296 switch (user_input[0]) {
297 case 'x':
298 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
300 apply_axis_lock();
301 break;
302 case 'y':
303 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
305 apply_axis_lock();
306 break;
307 case 'z':
308 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
310 apply_axis_lock();
311 break;
312 case 'q':
313 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
315 apply_axis_lock();
316 break;
317 case 'w':
318 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
320 apply_axis_lock();
321 break;
322 case 'e':
323 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
325 apply_axis_lock();
326 break;
327 case 'X':
328 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
330 apply_axis_lock();
331 break;
332 case 'Y':
333 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
335 apply_axis_lock();
336 break;
337 case 'Z':
338 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
340 apply_axis_lock();
341 break;
342 case 'Q':
343 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
345 apply_axis_lock();
346 break;
347 case 'W':
348 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
350 apply_axis_lock();
351 break;
352 case 'E':
353 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
355 apply_axis_lock();
356 break;
357 case 'a':
358 cmd.lock_ori_axis = {false, false, false};
359 cmd.lock_trans_axis = {false, false, false};
361 apply_axis_lock();
362 break;
363 case 'A':
364 cmd.lock_ori_axis = {true, true, true};
365 cmd.lock_trans_axis = {true, true, true};
367 apply_axis_lock();
368 break;
369 case 'r':
370 teleop.Engage(0, true);
371 break;
372 case 'R':
373 teleop.Engage(0, false);
374 break;
375 case 'i':
376 teleop.SetNullSpacePosture(0, kPreferredJntPos);
377 break;
378 case 'I':
379 teleop.SetNullSpacePosture(0, kHomeJntPos);
380 break;
381 case 'p':
382 teleop.SetMaxContactWrench(0, kDefaultMaxContactWrench);
383 break;
384 case 'u':
385 teleop.Init();
386 teleop.Start();
387 break;
388 case 'U':
389 teleop.Stop();
390 break;
391 case 'l': {
392 double latency_ms {};
393 const bool ok = teleop.CheckTeleopConnectionLatency(0, latency_ms);
394 if (ok) {
395 LogInfo("pair 0 message latency: " + FmtFixed(latency_ms, 1)
396 + " ms (within limit)");
397 } else if (HasActiveIssue(teleop.GetTeleopStatus(0),
399 LogWarn("pair 0 disconnected");
400 } else if (latency_ms < 0.0) {
401 LogWarn(
402 "pair 0 clock mismatch: latency " + FmtFixed(latency_ms, 1) + " ms");
403 } else {
404 LogWarn("pair 0 latency over limit: " + FmtFixed(latency_ms, 1) + " ms");
405 }
406 break;
407 }
408 case 'h':
409 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
410 break;
411 case 'F': {
412 LogInfo("Clearing fault, this may take up to 30 seconds...");
413 const auto results = teleop.ClearFault();
414 for (size_t i = 0; i < results.size(); ++i) {
415 LogInfo("Teleop pair " + std::to_string(i)
416 + " clear fault: " + BoolStr(results[i]));
417 }
418 break;
419 }
420 case 'n': {
421 const auto pair = teleop.robot_pair_sn(0);
422 LogInfo(std::string("pair 0 role=")
423 + flexiv::tdk::RoleTypeStr[static_cast<size_t>(teleop.role())]
424 + " leader_sn=" + pair.first + " follower_sn=" + pair.second);
425 break;
426 }
427
428 default:
429 LogWarn("Invalid command!");
430 PrintCommandMenu();
431 break;
432 }
433 } catch (const std::exception& e) {
434 LogError(std::string("Command '") + user_input[0] + "' failed: " + e.what());
435 try {
436 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
437 } catch (const std::exception& status_e) {
438 LogError(std::string("Failed to query teleop status: ") + status_e.what());
439 }
440 }
441 }
442 LogInfo("Console thread exiting.");
443 return;
444}
445
446int main(int argc, char* argv[])
447{
448 std::string follower_sn, leader_sn, teleop_role, tcp_role, public_server_ip, lan_ip, wan_iface;
449 unsigned int server_port = 0;
450 std::vector<std::string> lan_interface_whitelist {};
451 std::vector<std::string> wan_interface_whitelist {};
452 bool enable_digital_input = false;
453
454 int opt = 0;
455 while ((opt = getopt_long_only(argc, argv, "l:f:r:t:i:p:A:W:D", kLongOptions, nullptr)) != -1) {
456 switch (opt) {
457 case 'f':
458 follower_sn = std::string(optarg);
459 break;
460 case 'l':
461 leader_sn = std::string(optarg);
462 break;
463 case 'r':
464 teleop_role = std::string(optarg);
465 break;
466 case 't':
467 tcp_role = std::string(optarg);
468 break;
469 case 'i':
470 public_server_ip = std::string(optarg);
471 break;
472 case 'p': {
473 const std::string port_str(optarg);
474 unsigned long port = 0;
475 size_t parsed = 0;
476 try {
477 port = std::stoul(port_str, &parsed);
478 } catch (...) {
479 parsed = 0;
480 }
481 if (port_str.empty() || port_str[0] == '-' || parsed != port_str.size()
482 || port < 1 || port > 65535) {
483 LogError("Invalid port number: " + port_str + ", valid range is 1-65535");
484 return 1;
485 }
486 server_port = static_cast<unsigned int>(port);
487 break;
488 }
489 case 'A':
490 lan_ip = std::string(optarg);
491 lan_interface_whitelist.push_back(lan_ip);
492 break;
493 case 'W':
494 wan_iface = std::string(optarg);
495 wan_interface_whitelist.push_back(wan_iface);
496 break;
497 case 'D':
498 enable_digital_input = true;
499 break;
500 default:
501 PrintHelp();
502 return 1;
503 }
504 }
505 if (follower_sn.empty() || leader_sn.empty() || tcp_role.empty() || teleop_role.empty()
506 || public_server_ip.empty() || server_port == 0) {
507 PrintHelp();
508 return 1;
509 }
510 if (lan_interface_whitelist.empty()) {
511 LogWarn("LAN whitelist is not provided, will search all network interfaces.");
512 }
513 if (wan_interface_whitelist.empty()) {
514 LogWarn("WAN interface whitelist is not provided, will search all network interfaces.");
515 }
516
517 // Whether this is a TCP server or client
518 bool is_tcp_server;
519 if (tcp_role == "server") {
520 is_tcp_server = true;
521 } else if (tcp_role == "client") {
522 is_tcp_server = false;
523 } else {
524 LogError("Valid inputs for [-t] are: server, client");
525 return 1;
526 }
527
528 // Whether this is leader or follower
529 if (teleop_role == "follower") {
531 } else if (teleop_role == "leader") {
533 } else {
534 LogError("Valid inputs for [-r] are: follower, leader");
535 return 1;
536 }
537
538 // Network configuration
539 flexiv::tdk::NetworkCfgStd network_cfg;
540 network_cfg.is_tcp_server = is_tcp_server;
541 network_cfg.public_ipv4_address = public_server_ip;
542 network_cfg.listening_port = server_port;
543 network_cfg.lan_interface_whitelist = lan_interface_whitelist;
544 network_cfg.wan_interface_whitelist = wan_interface_whitelist;
545
546 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
547 robot_sn_pairs.push_back({leader_sn, follower_sn});
548 try {
549
550 // Allocate tdk object (Standard Edition TCP peer-to-peer)
551 flexiv::tdk::TransparentCartesianTeleopWAN tctw(robot_sn_pairs, kRole, network_cfg);
552 const auto pair_sn = tctw.robot_pair_sn(0);
553 LogInfo(std::string("This instance role=")
554 + flexiv::tdk::RoleTypeStr[static_cast<size_t>(tctw.role())]
555 + " leader_sn=" + pair_sn.first + " follower_sn=" + pair_sn.second);
556
557 // Init high transparency teleop
558 tctw.Init();
559
560 // Start high transparency teleop
561 tctw.Start();
562
563 // Set max contact wrench
564 tctw.SetMaxContactWrench(0, kDefaultMaxContactWrench);
565
566 // Start console and fault monitor threads
567 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
568 std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctw));
569
570 // Start pedal_thread only if role is leader AND -D is given
571 std::optional<std::thread> pedal_thread;
572 if (teleop_role == "leader" && enable_digital_input) {
573 LogInfo(
574 "Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
575 "flag.");
576 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw));
577 } else {
578 LogInfo(
579 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
580 "provided).");
581 }
582
583 // Wait for console_thread to finish
584 console_thread.join();
585
586 // Stop all threads, notify other threads exit
587 g_running = false;
588
589 // Wait for fault monitor and digital reading task exit
590 fault_monitor_thread.join();
591 if (pedal_thread && pedal_thread->joinable()) {
592 pedal_thread->join();
593 }
594
595 // Exit high transparency teleop
596 tctw.Stop();
597
598 } catch (const std::exception& e) {
599 LogError(e.what());
600 return 1;
601 }
602
603 return 0;
604}
Teleoperation control interface that represents leader or follower robots in transparent teleoperatio...
void Stop()
[Blocking] Stop the teleoperation control loop and make all robots hold their pose.
void SetAxisLockCmd(unsigned int idx, const AxisLock &cmd)
[Non-blocking] Set the leader robot axis locking command.
void GetAxisLockState(unsigned int idx, AxisLock &data)
[Non-blocking] Get the leader robot axis locking status
void Engage(unsigned int idx, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot in the specified robot pair....
flexiv::tdk::Role role() const
[Non-blocking] Role of this teleop instance.
void Start()
[Non-Blocking] Start the teleoperation control loop for current roles (leaders or followers) specifie...
void SetMaxContactWrench(unsigned int idx, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the current role in the robot pairs....
bool CheckTeleopConnectionLatency(unsigned int idx, double &latency_ms) const
Non-blocking check of WAN connection status and average message latency.
bool fault(unsigned int idx) const
[Non-blocking] Whether the current role is in fault state.
std::vector< bool > ClearFault(unsigned int timeout_sec=30)
[Blocking] Try to clear minor or critical fault for current role (leaders or followers) without a pow...
void GetTeleopStatus(unsigned int idx, TeleopStatus &status) const
[Non-blocking] Query why teleoperation is restricted, paused, or stopped, and what the operator shoul...
void Init(bool limit_wrist_singular=true, ZeroFTSensor zero_ft_sensor=ZeroFTSensor::Enable)
[Blocking] Get current role ready for teleoperation. The following actions will happen in sequence: a...
const 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 SetNullSpacePosture(unsigned int idx, const std::vector< double > &ref_joint_positions)
[Blocking] Set reference joint positions used in the robot's null-space posture control module for th...
std::pair< std::string, std::string > robot_pair_sn(unsigned int idx) const
[Non-blocking] Serial number pair {leader_sn, follower_sn} of the specified robot pair.
Header file containing various constant expressions, data structs, and enums.
Role
Roles of participants in transparent cartesian teleop.
Definition data.hpp:121
@ WAN_TELEOP_LEADER
The leader robot operated by a human during teleoperation over WAN.
Definition data.hpp:124
@ WAN_TELEOP_FOLLOWER
Definition data.hpp:125
TeleopIssueCode
Issue code and possible reasons for a teleoperation restriction or interruption.
Definition data.hpp:286
@ NETWORK_DISCONNECTED
Connection to the peer is not established.
Definition data.hpp:293
@ COORD_WORLD
WORLD coordinate of local robot.
Definition data.hpp:223
@ COORD_TCP
TCP coordinate of local robot.
Definition data.hpp:222
Data for locking axis, including reference frame and axis to be locked for transparent cartesian tele...
Definition data.hpp:250
std::array< bool, 3 > lock_ori_axis
Orientation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:266
std::array< bool, 3 > lock_trans_axis
Translation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:260
CoordType coord
Reference coordinate that the axis to be locked.
Definition data.hpp:254
TCP Server and Client Configuration TDK Standard Edition. In a teleoperation-over-WAN setup,...
Definition data.hpp:53
std::string public_ipv4_address
Definition data.hpp:68
std::vector< std::string > lan_interface_whitelist
Definition data.hpp:82
unsigned int listening_port
Definition data.hpp:74
std::vector< std::string > wan_interface_whitelist
Definition data.hpp:90
One restriction or interruption that the operator should understand.
Definition data.hpp:339
TeleopIssueCode code
Definition data.hpp:341
Snapshot of teleoperation health for one robot pair.
Definition data.hpp:394
std::vector< TeleopIssue > issues
Definition data.hpp:430