Flexiv TDK APIs 1.7
Loading...
Searching...
No Matches
transparent_cartesian_teleop_lan.cpp
1
10
11#include <flexiv/tdk/data.hpp>
13
14#include <getopt.h>
15#include <array>
16#include <atomic>
17#include <chrono>
18#include <cmath>
19#include <iomanip>
20#include <iostream>
21#include <optional>
22#include <sstream>
23#include <string>
24#include <thread>
25#include <vector>
26#include <functional>
27
28namespace {
30std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
31 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
32
34std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
35 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
36
38const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
39 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
40
42std::atomic<bool> g_running {true};
43
44std::string BoolStr(bool v)
45{
46 return v ? "true" : "false";
47}
48
49std::string FmtFixed(double v, int prec)
50{
51 std::ostringstream oss;
52 oss << std::fixed << std::setprecision(prec) << v;
53 return oss.str();
54}
55
56std::string VecStr(const std::vector<double>& vec, int prec)
57{
58 std::string out = "[";
59 for (size_t i = 0; i < vec.size(); ++i) {
60 out += (i == 0 ? "" : ", ") + FmtFixed(vec[i], prec);
61 }
62 return out + "]";
63}
64
65void LogInfo(const std::string& msg)
66{
67 std::cout << "[info] " << msg << std::endl;
68}
69
70void LogWarn(const std::string& msg)
71{
72 std::cerr << "[warn] " << msg << std::endl;
73}
74
75void LogError(const std::string& msg)
76{
77 std::cerr << "[error] " << msg << std::endl;
78}
79
80} // namespace
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<<" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
117 std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
118 std::cout<<"Usage: sudo ./transparent_cartesian_teleop_lan -l <leader_sn> -f <follower_sn> [-A <lan_ipv4>] [-D]"<<std::endl;
119 // clang-format on
120}
121
122const struct option kLongOptions[] = {
123 // clang-format off
124 {"leader SN", required_argument, 0, 'l'},
125 {"follower SN", required_argument, 0, 'f'},
126 {"lan whitelist ip", required_argument, 0, 'A'},
127 {"enable digital input", no_argument, 0, 'D'},
128 {0, 0, 0, 0 }
129 // clang-format on
130};
131
135void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
136{
137 bool logged_idle = false;
138 std::string last_error;
139 while (g_running.load()) {
140 try {
141 const auto status = teleop.GetTeleopStatus(0);
142 // Robot fault / Stop() drops teleop back to not-started. Engage() is
143 // invalid in that state and must not be polled every cycle.
144 if (!status.started || status.stopped) {
145 if (!logged_idle) {
146 LogWarn(
147 "ReadDigitalInputTask: teleop is not started, pause Engage "
148 "(call Init + Start to resume)");
149 logged_idle = true;
150 }
151 } else {
152 logged_idle = false;
153 teleop.Engage(0, teleop.digital_inputs(0).first[0]);
154 }
155 last_error.clear();
156 } catch (const std::exception& e) {
157 if (last_error != e.what()) {
158 LogError(std::string("Exception in ReadDigitalInputTask: ") + e.what());
159 last_error = e.what();
160 }
161 }
162 std::this_thread::sleep_for(std::chrono::milliseconds(10));
163 }
164 LogInfo("ReadDigitalInputTask exiting.");
165 return;
166}
167
171void FaultMonitorTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
172{
173 bool last_fault = false;
174 std::string last_error;
175 while (g_running.load()) {
176 try {
177 const bool fault = teleop.any_fault();
178 const bool fault_changed = (fault != last_fault);
179 last_fault = fault;
180 if (fault_changed && fault) {
181 LogError("Fault detected. Press 'h' for details, 'F' to clear fault, then 'B' to "
182 "restart teleop.");
183 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
184 } else if (fault_changed) {
185 LogInfo("Fault cleared. Press 'B' to restart teleop.");
186 }
187 last_error.clear();
188 } catch (const std::exception& e) {
189 if (last_error != e.what()) {
190 LogError(std::string("Exception in FaultMonitorTask: ") + e.what());
191 last_error = e.what();
192 }
193 }
194 std::this_thread::sleep_for(std::chrono::milliseconds(100));
195 }
196 LogInfo("FaultMonitorTask exiting.");
197 return;
198}
199
203void ConsoleTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
204{
205 auto PrintCommandMenu = []() {
206 std::cout << R"(
207 --- Axis Lock ---
208 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
209 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
210 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
211 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
212
213 --- Teleop Engagement ---
214 r : Engage teleop
215 R : Disengage teleop
216
217 --- Wrench Feedback Scaling ---
218 t : Set wrench feedback scaling to 0.5
219 T : Set wrench feedback scaling to 2.0
220 c : Reset wrench feedback scaling to 1.0 default value
221
222 --- Axis Lock Presets ---
223 u : Unlock all axes (TCP coord)
224 U : Lock all axes (TCP coord)
225
226 --- Null Space Posture ---
227 i/I : Set local nullspace to Preferred/Home posture
228 o/O : Set remote nullspace to Preferred/Home posture
229
230 --- Max Contact Wrench ---
231 p : Set default remote max contact wrench
232
233 --- Repulsive Force ---
234 a : Set repulsive force to {5, 0, 0}
235 A : Set repulsive force to {-5, 0, 0}
236 C : Clear repulsive force
237
238 --- Start/Stop ---
239 b : Stop teleop
240 B : Start teleop
241
242 --- Is teleop stopped or not ---
243 s : Query if teleop stopped or not
244
245 --- Teleop status ---
246 h : Print why teleop is restricted / paused and what to do next
247
248 --- Fault recovery ---
249 F : Clear fault (blocking, up to 30 s), then press 'B' to restart teleop
250
251 --- Home all robots ---
252 H : Stop teleop and home all robots. Confirm the workspace is clear first.
253
254 --- Print action/states via instance ---
255 P : Print follower action and state
256
257 --- Exit ---
258 quit : Stop teleop and exit the program
259
260 --- Help ---
261 Any other key to show this help menu
262 )" << std::endl;
263 };
264
265 unsigned int index = 0;
267 teleop.GetAxisLockState(index, cmd);
268
269 while (g_running.load()) {
270
271 std::string userInput {};
272
273 // Get user input, exit when stdin is closed
274 if (!std::getline(std::cin, userInput)) {
275 LogInfo("Console input closed.");
276 break;
277 }
278
279 if (userInput.empty()) {
280 LogWarn("Empty command!");
281 PrintCommandMenu();
282 continue;
283 }
284
285 if (userInput == "quit") {
286 LogInfo("Quit requested.");
287 break;
288 }
289
290 try {
291 switch (userInput[0]) {
292 case 'x':
293 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
295 teleop.SetAxisLockCmd(index, cmd);
296 break;
297 case 'y':
298 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
300 teleop.SetAxisLockCmd(index, cmd);
301 break;
302 case 'z':
303 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
305 teleop.SetAxisLockCmd(index, cmd);
306 break;
307 case 'q':
308 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
310 teleop.SetAxisLockCmd(index, cmd);
311 break;
312 case 'w':
313 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
315 teleop.SetAxisLockCmd(index, cmd);
316 break;
317 case 'e':
318 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
320 teleop.SetAxisLockCmd(index, cmd);
321 break;
322
323 case 'X':
324 cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
326 teleop.SetAxisLockCmd(index, cmd);
327 break;
328 case 'Y':
329 cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
331 teleop.SetAxisLockCmd(index, cmd);
332 break;
333 case 'Z':
334 cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
336 teleop.SetAxisLockCmd(index, cmd);
337 break;
338 case 'Q':
339 cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
341 teleop.SetAxisLockCmd(index, cmd);
342 break;
343 case 'W':
344 cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
346 teleop.SetAxisLockCmd(index, cmd);
347 break;
348 case 'E':
349 cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
351 teleop.SetAxisLockCmd(index, cmd);
352 break;
353
354 case 'r':
355 teleop.Engage(index, true);
356 break;
357 case 'R':
358 teleop.Engage(index, false);
359 break;
360
361 case 't':
362 teleop.SetWrenchFeedbackScalingFactor(index, 0.5);
363 break;
364 case 'T':
365 teleop.SetWrenchFeedbackScalingFactor(index, 2);
366 break;
367
368 case 'u':
369 cmd.lock_ori_axis = {false, false, false};
370 cmd.lock_trans_axis = {false, false, false};
372 teleop.SetAxisLockCmd(index, cmd);
373 break;
374 case 'U':
375 cmd.lock_ori_axis = {true, true, true};
376 cmd.lock_trans_axis = {true, true, true};
378 teleop.SetAxisLockCmd(index, cmd);
379 break;
380 case 'i':
381 teleop.SetLeaderNullSpacePosture(index, kPreferredJntPos);
382 break;
383 case 'I':
384 teleop.SetLeaderNullSpacePosture(index, kHomeJntPos);
385 break;
386 case 'o':
387 teleop.SetFollowerNullSpacePosture(index, kPreferredJntPos);
388 break;
389 case 'O':
390 teleop.SetFollowerNullSpacePosture(index, kHomeJntPos);
391 break;
392 case 'p':
393 teleop.SetFollowerMaxContactWrench(index, kDefaultMaxContactWrench);
394 break;
395
396 case 'a':
397 teleop.SetRepulsiveForce(index, {5, 0, 0});
398 break;
399 case 'A':
400 teleop.SetRepulsiveForce(index, {-5, 0, 0});
401 break;
402 case 'b':
403 teleop.Stop();
404 break;
405 case 'B':
406 teleop.Init();
407 teleop.Start();
408 break;
409 case 'c':
410 teleop.SetWrenchFeedbackScalingFactor(index, 1);
411 break;
412 case 'C':
413 teleop.SetRepulsiveForce(index, {0, 0, 0});
414 break;
415 case 's':
416 LogInfo(teleop.stopped(0) ? "Teleop pair 0 stopped" : "Teleop pair 0 started");
417 break;
418 case 'h':
419 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
420 break;
421 case 'F': {
422 LogInfo("Clearing fault, this may take up to 30 seconds...");
423 const auto results = teleop.ClearFault();
424 for (size_t i = 0; i < results.size(); ++i) {
425 LogInfo("Teleop pair " + std::to_string(i)
426 + " clear fault: leader=" + BoolStr(results[i].first)
427 + " follower=" + BoolStr(results[i].second));
428 }
429 break;
430 }
431 case 'H':
432 teleop.Stop();
433 teleop.HomeAll();
434 LogInfo("Teleop stopped for homing");
435 break;
436 case 'P': {
437 const auto follower = teleop.instances(index).second;
438 LogInfo("Follower q: " + VecStr(follower->states().q, 2));
439 LogInfo("Follower q_d: " + VecStr(follower->actions().q_d, 2));
440 break;
441 }
442 default:
443 LogWarn("Invalid command!");
444 PrintCommandMenu();
445 break;
446 }
447 } catch (const std::exception& e) {
448 LogError(std::string("Command '") + userInput[0] + "' failed: " + e.what());
449 try {
450 PrintTeleopStatus(0, teleop.GetTeleopStatus(0));
451 } catch (const std::exception& status_e) {
452 LogError(std::string("Failed to query teleop status: ") + status_e.what());
453 }
454 }
455 }
456 LogInfo("Console thread exiting.");
457 return;
458}
459
460int main(int argc, char* argv[])
461{
462 std::string follower_sn {};
463 std::string leader_sn {};
464 std::string whitelist_ip {};
465 std::vector<std::string> network_interface_whitelist {};
466 int opt = 0;
467 int longIndex = 0;
468 bool enable_digital_input = false;
469
470 while ((opt = getopt_long_only(argc, argv, "f:l:A:D", kLongOptions, &longIndex)) != -1) {
471 switch (opt) {
472 case 'f':
473 follower_sn = std::string(optarg);
474 break;
475 case 'l':
476 leader_sn = std::string(optarg);
477 break;
478 case 'A':
479 whitelist_ip = std::string(optarg);
480 network_interface_whitelist.emplace_back(whitelist_ip);
481 break;
482 case 'D':
483 enable_digital_input = true;
484 break;
485 default:
486 PrintHelp();
487 return 1;
488 }
489 }
490 if (leader_sn.empty() || follower_sn.empty()) {
491 PrintHelp();
492 return 1;
493 }
494 if (network_interface_whitelist.empty()) {
495 LogWarn("network_interface_whitelist is not provided, will search all network interfaces.");
496 }
497
498 try {
499
500 // Create teleop control interface
502 {{leader_sn, follower_sn}}, network_interface_whitelist);
503
504 // Run initialization sequence
505 tctl.Init();
506
507 // Start control loop
508 tctl.Start();
509
510 // Start console, fault monitor and pedal input threads
511 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
512 std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctl));
513
514 // Start pedal_thread based on the new flag or the role
515 std::optional<std::thread> pedal_thread;
516
517 if (enable_digital_input) {
518 LogInfo("Starting ReadDigitalInputTask thread.");
519 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
520 } else {
521 LogInfo("ReadDigitalInputTask thread NOT started (-D flag not provided).");
522 }
523
524 // Wait for threads to finish
525 console_thread.join();
526
527 // Stop all threads, notify other threads exit
528 g_running.store(false);
529
530 // Wait for fault monitor and digital reading task exit
531 fault_monitor_thread.join();
532 if (pedal_thread && pedal_thread->joinable()) {
533 pedal_thread->join();
534 }
535
536 // Exit high transparency teleop
537 tctl.Stop();
538
539 } catch (const std::exception& e) {
540 LogError(e.what());
541 return 1;
542 }
543
544 return 0;
545}
Teleoperation control interface to run Cartesian-space teleoperation for one or more pairs of robots ...
void SetFollowerNullSpacePosture(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...
void GetTeleopStatus(unsigned int idx, TeleopStatus &status) const
[Non-blocking] Query why teleoperation is restricted, paused, or stopped, and what the operator shoul...
std::pair< std::shared_ptr< Robot >, std::shared_ptr< Robot > > instances(unsigned int idx) const
[Non-blocking] Pointers to the underlying rdk::Robot instances of the robot pair.
bool any_fault(void) const
[Non-blocking] Whether any of the connected robots is in fault state.
std::vector< std::pair< bool, bool > > ClearFault(unsigned int timeout_sec=30)
[Blocking] Try to clear minor or critical fault of the robot without a power cycle.
void SetFollowerMaxContactWrench(unsigned int idx, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the follower robot of specified robot pair....
void SetWrenchFeedbackScalingFactor(unsigned int idx, double factor=1.0)
[Non-blocking] Set the wrench feedback scaling factor.
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 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) ...
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 Start()
[Non-Blocking] Start the teleoperation control loop for all robot pairs.
void HomeAll()
[Blocking] Move all connected robots to their home posture simultaneously.
void SetRepulsiveForce(unsigned int idx, 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.
bool stopped(unsigned int idx) const
[Non-blocking] Whether teleop process has stopped. After teleop is started, the teleop process may st...
void Engage(unsigned int idx, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot. TransparentCartesianTeleopLAN supports...
void SetLeaderNullSpacePosture(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...
Header file containing various constant expressions, data structs, and enums.
@ 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
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