Flexiv TDK APIs 1.7
Loading...
Searching...
No Matches
transparent_cartesian_teleop_wan_pro.cpp
1
13
14#include <flexiv/tdk/data.hpp>
17
18#include <getopt.h>
19#include <algorithm>
20#include <array>
21#include <atomic>
22#include <chrono>
23#include <cmath>
24#include <functional>
25#include <iomanip>
26#include <iostream>
27#include <optional>
28#include <sstream>
29#include <string>
30#include <thread>
31#include <utility>
32#include <vector>
33
34namespace {
35
37std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
38 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
39
41std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
42 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
43
45const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
46 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
47
49constexpr unsigned int kIdx = 0;
50
52std::atomic<bool> g_running {true};
53
56
58std::string g_gripper_name;
59
60}
61
62std::string BoolStr(bool v)
63{
64 return v ? "true" : "false";
65}
66
67std::string FmtFixed(double v, int prec)
68{
69 std::ostringstream oss;
70 oss << std::fixed << std::setprecision(prec) << v;
71 return oss.str();
72}
73
74void LogInfo(const std::string& msg)
75{
76 std::cout << "[info] " << msg << std::endl;
77}
78
79void LogWarn(const std::string& msg)
80{
81 std::cerr << "[warn] " << msg << std::endl;
82}
83
84void LogError(const std::string& msg)
85{
86 std::cerr << "[error] " << msg << std::endl;
87}
88
89bool HasActiveIssue(const flexiv::tdk::TeleopStatus& status, flexiv::tdk::TeleopIssueCode code)
90{
91 return std::any_of(status.issues.begin(), status.issues.end(),
92 [code](const flexiv::tdk::TeleopIssue& issue) { return issue.code == code; });
93}
94
95void PrintTeleopStatus(unsigned int idx, const flexiv::tdk::TeleopStatus& status)
96{
97 LogInfo("Teleop pair " + std::to_string(idx) + " status: initialized="
98 + BoolStr(status.initialized) + " started=" + BoolStr(status.started)
99 + " engaged=" + BoolStr(status.engaged) + " stopped=" + BoolStr(status.stopped)
100 + " fault=" + BoolStr(status.fault)
101 + " motion_restricted=" + BoolStr(status.motion_restricted) + " latency="
102 + FmtFixed(status.latency_ms, 1) + "/" + FmtFixed(status.latency_threshold_ms, 1)
103 + " ms");
105 LogInfo("No teleop restriction. Safe to continue.");
106 return;
107 }
108 const auto& issue = status.primary;
109 if (!issue.active) {
110 LogInfo(std::string("No active teleop restriction. Last cleared issue: [")
111 + flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(issue.code)] + "] "
112 + issue.title);
113 return;
114 }
115 LogWarn(std::string("[")
116 + flexiv::tdk::TeleopIssueLevelStr[static_cast<size_t>(issue.level)] + "/"
117 + flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(issue.side)] + "] "
118 + issue.title + " | " + issue.description + " | " + issue.suggestion);
119 for (const auto& extra : status.issues) {
120 if (extra.code == issue.code && extra.side == issue.side
121 && extra.joint_index == issue.joint_index) {
122 continue;
123 }
124 LogWarn(std::string(" also: [")
125 + flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(extra.code)] + "/"
126 + flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(extra.side)] + "] "
127 + extra.title);
128 }
129}
130
131void PrintGripperStates(flexiv::tdk::GripperRemoteControl& gripper)
132{
133 try {
134 auto states = gripper.states(kIdx);
135 LogInfo("Gripper states: width = " + FmtFixed(states.width, 4) + " m, force = "
136 + FmtFixed(states.force, 2) + " N, is_moving = " + BoolStr(states.is_moving));
137 } catch (const std::exception& e) {
138 LogWarn(std::string("Gripper states not available yet: ") + e.what());
139 }
140}
141
142void PrintHelp()
143{
144 // clang-format off
145 std::cout<<"Invalid program arguments!"<<std::endl;
146 std::cout<<" -l [necessary] serial number of leader robot."<<std::endl;
147 std::cout<<" -f [necessary] serial number of follower robot."<<std::endl;
148 std::cout<<" -r [necessary] Role of participants in teleop. can be [follower] or [leader]"<<std::endl;
149 std::cout<<" -c [necessary] Path to client.conf from the TDK Server credential package."<<std::endl;
150 std::cout<<" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
151 std::cout<<" -n [optional] Follower gripper device name configured in Elements -> Device." << std::endl;
152 std::cout<<" To enable GripperRemoteControl, both leader and follower sides must provide the same gripper name." << std::endl;
153 std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
154 std::cout<<" Leader DI0 engages arm teleop; with [-n], DI1 toggles the follower gripper" << std::endl;
155 std::cout<<" and the console gripper motion commands (o/c/s) are disabled." << std::endl;
156 std::cout<<"Usage: sudo ./transparent_cartesian_teleop_wan_pro -l <leader_sn> -f <follower_sn> -r leader|follower -c <client.conf> [-A <lan_ipv4>] [-n gripper_name] [-D]"<<std::endl;
157 // clang-format on
158}
159
160const struct option kLongOptions[] = {
161 // clang-format off
162 {"leader SN", required_argument, 0, 'l'},
163 {"follower SN", required_argument, 0, 'f'},
164 {"role", required_argument, 0, 'r'},
165 {"client config", required_argument, 0, 'c'},
166 {"lan whitelist ip", required_argument, 0, 'A'},
167 {"gripper name", required_argument, 0, 'n'},
168 {"enable digital input", no_argument, 0, 'D'},
169 {0, 0, 0, 0 }
170 // clang-format on
171};
172
178void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop,
179 std::optional<flexiv::tdk::GripperRemoteControl>& gripper)
180{
181 bool logged_idle = false;
182 bool di1_stable = false;
183 int di1_counter = 0;
184 bool gripper_opened = false;
185 std::string last_error;
186
187 while (g_running.load()) {
188 try {
189 const auto status = teleop.GetTeleopStatus(kIdx);
190 // Robot fault / Stop() drops teleop back to not-started. Engage() is
191 // invalid in that state and must not be polled every cycle.
192 if (!status.started || status.stopped) {
193 if (!logged_idle) {
194 LogWarn(
195 "ReadDigitalInputTask: teleop is not started, pause Engage "
196 "(call Init + Start to resume)");
197 logged_idle = true;
198 }
199 } else {
200 logged_idle = false;
201 const auto digital_inputs = teleop.digital_inputs(kIdx);
202 teleop.Engage(kIdx, digital_inputs[0]);
203
204 if (gripper.has_value() && digital_inputs.size() > 1) {
205 const bool di1_raw = digital_inputs[1];
206 if (di1_raw != di1_stable) {
207 if (++di1_counter >= 3) {
208 di1_stable = di1_raw;
209 di1_counter = 0;
210 if (di1_stable) {
211 try {
212 auto params = gripper->params(kIdx);
213 if (!gripper_opened) {
214 double width = std::clamp(
215 0.5 * params.max_width, params.min_width, params.max_width);
216 double velocity = std::clamp(
217 0.5 * params.max_vel, params.min_vel, params.max_vel);
218 double force_limit = std::clamp(
219 0.5 * params.max_force, params.min_force, params.max_force);
220 gripper->Move(kIdx, width, velocity, force_limit);
221 gripper_opened = true;
222 LogInfo("DI1 pressed: opening gripper to width = "
223 + FmtFixed(width, 4) + " m");
224 } else {
225 double force
226 = std::clamp(40.0, params.min_force, params.max_force);
227 gripper->Grasp(kIdx, force);
228 gripper_opened = false;
229 LogInfo("DI1 pressed: closing gripper with force = "
230 + FmtFixed(force, 2) + " N");
231 }
232 } catch (const std::exception& e) {
233 LogWarn(std::string("DI1 gripper action failed: ") + e.what());
234 }
235 }
236 }
237 } else {
238 di1_counter = 0;
239 }
240 }
241 }
242 last_error.clear();
243 } catch (const std::exception& e) {
244 if (last_error != e.what()) {
245 LogError(std::string("Exception in ReadDigitalInputTask: ") + e.what());
246 last_error = e.what();
247 }
248 }
249 std::this_thread::sleep_for(std::chrono::milliseconds(10));
250 }
251 LogInfo("ReadDigitalInputTask exiting.");
252}
253
257void FaultMonitorTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
258{
259 bool last_fault = false;
260 std::string last_error;
261 while (g_running.load()) {
262 try {
263 const bool fault = teleop.fault(kIdx);
264 const bool fault_changed = (fault != last_fault);
265 last_fault = fault;
266 if (fault_changed && fault) {
267 LogError("Fault detected. Press 'h' for details, 'F' to clear fault, then 'u' to "
268 "restart teleop.");
269 PrintTeleopStatus(kIdx, teleop.GetTeleopStatus(kIdx));
270 } else if (fault_changed) {
271 LogInfo("Fault cleared. Press 'u' to restart teleop.");
272 }
273 last_error.clear();
274 } catch (const std::exception& e) {
275 if (last_error != e.what()) {
276 LogError(std::string("Exception in FaultMonitorTask: ") + e.what());
277 last_error = e.what();
278 }
279 }
280 std::this_thread::sleep_for(std::chrono::milliseconds(100));
281 }
282 LogInfo("FaultMonitorTask exiting.");
283}
284
291void ConsoleTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop,
292 std::optional<flexiv::tdk::GripperRemoteControl>& gripper, bool di_controls_gripper)
293{
294 auto PrintCommandMenu = [has_gripper = gripper.has_value()]() {
295 std::cout << R"(
296 --- Axis Lock (leader only) ---
297 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
298 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
299 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
300 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
301 a : Unlock all axes (TCP coord)
302 A : Lock all axes (TCP coord)
303
304 --- Teleop Engagement ---
305 r : Engage teleop
306 R : Disengage teleop
307
308 --- Null Space Posture ---
309 i/I : Set local nullspace to Preferred/Home posture
310
311 --- Max Contact Wrench ---
312 p : Set default max contact wrench
313
314 --- Reinit and start ---
315 u : Recall Init and Start
316 U : Stop teleop
317
318 --- TCP message latency ---
319 l : Print current message latency (disconnected / clock mismatch / over limit)
320
321 --- Teleop status ---
322 h : Print why teleop is restricted / paused and what to do next
323
324 --- Fault recovery ---
325 F : Clear fault (blocking, up to 30 s), then press 'u' to restart teleop
326
327 --- Identity ---
328 n : Print role() and robot_pair_sn()
329 )" << std::endl;
330
331 if (has_gripper) {
332 std::cout << R"(
333 --- Gripper Lifecycle (Leader only, via RPC) ---
334 G : Enable gripper on follower robot
335 D : Disable gripper on follower robot
336 N : Trigger initialization of the enabled gripper
337
338 --- Gripper Motion (Leader only, via reliable topic, disabled when -D is enabled) ---
339 o : Open gripper (Move to max width)
340 c : Close gripper (Grasp with moderate force)
341 s : Stop and hold gripper
342
343 --- Gripper States ---
344 t : Print latest gripper states
345 P : Print gripper params (valid command ranges)
346 )" << std::endl;
347 }
348
349 std::cout << R"(
350 --- Exit ---
351 quit : Stop teleop and exit the program
352
353 --- Help ---
354 Any other key to show this help menu
355 )" << std::endl;
356 };
357
358 const bool is_leader = (teleop.role() == flexiv::tdk::Role::WAN_TELEOP_LEADER);
360 if (is_leader) {
361 teleop.GetAxisLockState(kIdx, cmd);
362 }
363
364 auto apply_axis_lock = [&]() {
365 if (!is_leader) {
366 LogWarn("Axis lock is only available on the leader");
367 return;
368 }
369 teleop.SetAxisLockCmd(kIdx, cmd);
370 };
371
372 while (g_running.load()) {
373
374 std::string user_input {};
375
376 // Get user input, exit when stdin is closed
377 if (!std::getline(std::cin, user_input)) {
378 LogInfo("Console input closed.");
379 break;
380 }
381
382 if (user_input.empty()) {
383 LogWarn("Empty command!");
384 PrintCommandMenu();
385 continue;
386 }
387
388 if (user_input == "quit") {
389 LogInfo("Quit requested.");
390 break;
391 }
392
393 try {
394 const char command = user_input[0];
395 if (std::string("GDNocstP").find(command) != std::string::npos
396 && !gripper.has_value()) {
397 LogWarn("Gripper commands require [-n gripper_name].");
398 continue;
399 }
400 if (gripper.has_value() && g_role != flexiv::tdk::Role::WAN_TELEOP_LEADER
401 && std::string("GDNocs").find(command) != std::string::npos) {
402 LogWarn(std::string("Command '") + command + "' is only available on the leader");
403 continue;
404 }
405 if (di_controls_gripper && std::string("ocs").find(command) != std::string::npos) {
406 LogWarn(std::string("Gripper motion is controlled by DI1 (-D enabled); console "
407 "command '")
408 + command + "' is disabled.");
409 continue;
410 }
411
412 switch (command) {
413 case 'x':
414 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
416 apply_axis_lock();
417 break;
418 case 'y':
419 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
421 apply_axis_lock();
422 break;
423 case 'z':
424 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
426 apply_axis_lock();
427 break;
428 case 'q':
429 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
431 apply_axis_lock();
432 break;
433 case 'w':
434 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
436 apply_axis_lock();
437 break;
438 case 'e':
439 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
441 apply_axis_lock();
442 break;
443 case 'X':
444 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
446 apply_axis_lock();
447 break;
448 case 'Y':
449 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
451 apply_axis_lock();
452 break;
453 case 'Z':
454 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
456 apply_axis_lock();
457 break;
458 case 'Q':
459 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
461 apply_axis_lock();
462 break;
463 case 'W':
464 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
466 apply_axis_lock();
467 break;
468 case 'E':
469 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
471 apply_axis_lock();
472 break;
473 case 'a':
474 cmd.lock_ori_axis = {false, false, false};
475 cmd.lock_trans_axis = {false, false, false};
477 apply_axis_lock();
478 break;
479 case 'A':
480 cmd.lock_ori_axis = {true, true, true};
481 cmd.lock_trans_axis = {true, true, true};
483 apply_axis_lock();
484 break;
485 case 'r':
486 teleop.Engage(kIdx, true);
487 break;
488 case 'R':
489 teleop.Engage(kIdx, false);
490 break;
491 case 'i':
492 teleop.SetNullSpacePosture(kIdx, kPreferredJntPos);
493 break;
494 case 'I':
495 teleop.SetNullSpacePosture(kIdx, kHomeJntPos);
496 break;
497 case 'p':
498 teleop.SetMaxContactWrench(kIdx, kDefaultMaxContactWrench);
499 break;
500 case 'u':
501 teleop.Init();
502 teleop.Start();
503 break;
504 case 'U':
505 teleop.Stop();
506 break;
507 case 'l': {
508 double latency_ms {};
509 const bool ok = teleop.CheckTeleopConnectionLatency(kIdx, latency_ms);
510 if (ok) {
511 LogInfo("pair " + std::to_string(kIdx) + " message latency: "
512 + FmtFixed(latency_ms, 1) + " ms (within limit)");
513 } else if (HasActiveIssue(teleop.GetTeleopStatus(kIdx),
515 LogWarn("pair " + std::to_string(kIdx) + " disconnected");
516 } else if (latency_ms < 0.0) {
517 LogWarn("pair " + std::to_string(kIdx) + " clock mismatch: latency "
518 + FmtFixed(latency_ms, 1) + " ms");
519 } else {
520 LogWarn("pair " + std::to_string(kIdx) + " latency over limit: "
521 + FmtFixed(latency_ms, 1) + " ms");
522 }
523 break;
524 }
525 case 'h':
526 PrintTeleopStatus(kIdx, teleop.GetTeleopStatus(kIdx));
527 break;
528 case 'F': {
529 LogInfo("Clearing fault, this may take up to 30 seconds...");
530 const auto results = teleop.ClearFault();
531 for (size_t i = 0; i < results.size(); ++i) {
532 LogInfo("Teleop pair " + std::to_string(i)
533 + " clear fault: " + BoolStr(results[i]));
534 }
535 break;
536 }
537 case 'n': {
538 const auto pair = teleop.robot_pair_sn(kIdx);
539 LogInfo("pair " + std::to_string(kIdx) + " role="
540 + flexiv::tdk::RoleTypeStr[static_cast<size_t>(teleop.role())]
541 + " leader_sn=" + pair.first + " follower_sn=" + pair.second);
542 break;
543 }
544 case 'G':
545 gripper->Enable(kIdx, g_gripper_name);
546 LogInfo("Follower gripper [" + g_gripper_name + "] enabled");
547 break;
548 case 'D':
549 gripper->Disable(kIdx);
550 LogInfo("Follower gripper disabled");
551 break;
552 case 'N':
553 gripper->Init(kIdx);
554 LogInfo("Follower gripper initialization triggered");
555 break;
556 case 'o': {
557 auto params = gripper->params(kIdx);
558 double velocity
559 = std::clamp(0.5 * params.max_vel, params.min_vel, params.max_vel);
560 double force_limit
561 = std::clamp(0.5 * params.max_force, params.min_force, params.max_force);
562 gripper->Move(kIdx, params.max_width, velocity, force_limit);
563 LogInfo("Open command sent: width = " + FmtFixed(params.max_width, 4)
564 + " m, velocity = " + FmtFixed(velocity, 4)
565 + " m/s, force_limit = " + FmtFixed(force_limit, 2) + " N");
566 break;
567 }
568 case 'c': {
569 auto params = gripper->params(kIdx);
570 double force
571 = std::clamp(0.5 * params.max_force, params.min_force, params.max_force);
572 gripper->Grasp(kIdx, force);
573 LogInfo("Grasp command sent: force = " + FmtFixed(force, 2) + " N");
574 break;
575 }
576 case 's':
577 gripper->Stop(kIdx);
578 LogInfo("Stop command sent");
579 break;
580 case 't':
581 PrintGripperStates(*gripper);
582 break;
583 case 'P': {
584 try {
585 auto params = gripper->params(kIdx);
586 LogInfo("Gripper params: width = [" + FmtFixed(params.min_width, 4)
587 + ", " + FmtFixed(params.max_width, 4) + "] m, velocity = ["
588 + FmtFixed(params.min_vel, 4) + ", " + FmtFixed(params.max_vel, 4)
589 + "] m/s, force = [" + FmtFixed(params.min_force, 2) + ", "
590 + FmtFixed(params.max_force, 2) + "] N");
591 } catch (const std::exception& e) {
592 LogWarn(std::string("Gripper params not available yet: ") + e.what());
593 }
594 break;
595 }
596 default:
597 LogWarn("Invalid command!");
598 PrintCommandMenu();
599 break;
600 }
601 } catch (const std::exception& e) {
602 LogError(std::string("Command '") + user_input[0] + "' failed: " + e.what());
603 try {
604 PrintTeleopStatus(kIdx, teleop.GetTeleopStatus(kIdx));
605 } catch (const std::exception& status_e) {
606 LogError(std::string("Failed to query teleop status: ") + status_e.what());
607 }
608 }
609 }
610 LogInfo("Console thread exiting.");
611}
612
613int main(int argc, char* argv[])
614{
615 std::string follower_sn, leader_sn, teleop_role, client_config_file, lan_ip;
616 std::vector<std::string> lan_interface_whitelist {};
617 bool enable_digital_input = false;
618
619 int opt = 0;
620 while ((opt = getopt_long_only(argc, argv, "l:f:r:c:A:n:D", kLongOptions, nullptr)) != -1) {
621 switch (opt) {
622 case 'f':
623 follower_sn = std::string(optarg);
624 break;
625 case 'l':
626 leader_sn = std::string(optarg);
627 break;
628 case 'r':
629 teleop_role = std::string(optarg);
630 break;
631 case 'c':
632 client_config_file = std::string(optarg);
633 break;
634 case 'A':
635 lan_ip = std::string(optarg);
636 lan_interface_whitelist.push_back(lan_ip);
637 break;
638 case 'n':
639 g_gripper_name = std::string(optarg);
640 break;
641 case 'D':
642 enable_digital_input = true;
643 break;
644 default:
645 PrintHelp();
646 return 1;
647 }
648 }
649 if (follower_sn.empty() || leader_sn.empty() || teleop_role.empty()
650 || client_config_file.empty()) {
651 PrintHelp();
652 return 1;
653 }
654 if (lan_interface_whitelist.empty()) {
655 LogWarn("LAN whitelist is not provided, RDK will search all network interfaces.");
656 }
657
658 if (teleop_role == "follower") {
660 } else if (teleop_role == "leader") {
662 } else {
663 LogError("Valid inputs for [-r] are: follower, leader");
664 return 1;
665 }
666
667 flexiv::tdk::NetworkCfgPro network_cfg;
668 network_cfg.client_config_file = client_config_file;
669 network_cfg.lan_interface_whitelist = lan_interface_whitelist;
670
671 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
672 robot_sn_pairs.push_back({leader_sn, follower_sn});
673 try {
674 flexiv::tdk::TransparentCartesianTeleopWAN tctw(robot_sn_pairs, g_role, network_cfg);
675
676 const auto pair_sn = tctw.robot_pair_sn(kIdx);
677 LogInfo(std::string("This instance role=")
678 + flexiv::tdk::RoleTypeStr[static_cast<size_t>(tctw.role())]
679 + " leader_sn=" + pair_sn.first + " follower_sn=" + pair_sn.second);
680
681 // Allocate the gripper remote control object on top of the arm teleop object only if
682 // [-n] is provided. It reuses the role, robot serial number pairs and rdk::Robot
683 // connection of the arm teleop. Without [-n] the program runs as basic arm teleop only.
684 std::optional<flexiv::tdk::GripperRemoteControl> gripper;
685 if (!g_gripper_name.empty()) {
686 gripper.emplace(tctw, network_cfg);
687 LogInfo("Gripper remote control is ENABLED, target gripper device: [" + g_gripper_name
688 + "]");
689 } else {
690 LogInfo("Gripper remote control is DISABLED (no [-n gripper_name] provided).");
691 }
692
693 tctw.Init();
694 tctw.Start();
695 tctw.SetMaxContactWrench(kIdx, kDefaultMaxContactWrench);
696
697 // Follower side: enable the configured gripper locally so the leader can see states and
698 // send commands without waiting for the Enable RPC.
699 if (g_role == flexiv::tdk::Role::WAN_TELEOP_FOLLOWER && gripper.has_value()) {
700 try {
701 gripper->EnableLocal(kIdx, g_gripper_name);
702 } catch (const std::exception& e) {
703 LogError(std::string("Failed to auto-enable gripper [") + g_gripper_name
704 + "] on follower: " + e.what()
705 + ". The leader can still retry via the 'G' console command.");
706 }
707 }
708
709 const bool start_pedal_thread
710 = (g_role == flexiv::tdk::Role::WAN_TELEOP_LEADER && enable_digital_input);
711 const bool di_controls_gripper = start_pedal_thread && gripper.has_value();
712
713 std::thread console_thread(
714 std::bind(ConsoleTask, std::ref(tctw), std::ref(gripper), di_controls_gripper));
715 std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctw));
716
717 std::optional<std::thread> pedal_thread;
718 if (start_pedal_thread) {
719 LogInfo(std::string(
720 "Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
721 "flag. DI0: arm teleop engagement; DI1: ")
722 + (gripper.has_value() ? "toggles follower gripper open/close"
723 : "unused (gripper feature disabled, no -n)")
724 + ".");
725 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw), std::ref(gripper));
726 } else {
727 LogInfo(
728 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
729 "provided).");
730 }
731
732 console_thread.join();
733 g_running.store(false);
734
735 fault_monitor_thread.join();
736 if (pedal_thread && pedal_thread->joinable()) {
737 pedal_thread->join();
738 }
739
740 tctw.Stop();
741
742 } catch (const std::exception& e) {
743 LogError(e.what());
744 return 1;
745 }
746
747 return 0;
748}
Remote gripper control interface for the TransparentCartesianTeleopWAN teleoperation scenario....
GripperStates states(unsigned int idx)
[Blocking on leader side] Latest gripper states. On the follower side the states are read directly fr...
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
@ UNKNOWN
Unknown role.
Definition data.hpp:122
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
Network configuration for TDK Professional Edition (credential file from TDK Server).
Definition data.hpp:101
std::string client_config_file
Definition data.hpp:105
std::vector< std::string > lan_interface_whitelist
Definition data.hpp:113
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