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};
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};
39const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
40 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
43std::atomic<bool> g_running {
true};
49std::string BoolStr(
bool v)
51 return v ?
"true" :
"false";
54std::string FmtFixed(
double v,
int prec)
56 std::ostringstream oss;
57 oss << std::fixed << std::setprecision(prec) << v;
61void LogInfo(
const std::string& msg)
63 std::cout <<
"[info] " << msg << std::endl;
66void LogWarn(
const std::string& msg)
68 std::cerr <<
"[warn] " << msg << std::endl;
71void LogError(
const std::string& msg)
73 std::cerr <<
"[error] " << msg << std::endl;
78 return std::any_of(status.
issues.begin(), status.
issues.end(),
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)
91 LogInfo(
"No teleop restriction. Safe to continue.");
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) {
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)] +
"] "
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;
127const struct option kLongOptions[] = {
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'},
147 bool logged_idle =
false;
148 std::string last_error;
149 while (g_running.load()) {
157 "ReadDigitalInputTask: teleop is not started, pause Engage "
158 "(call Init + Start to resume)");
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();
172 std::this_thread::sleep_for(std::chrono::milliseconds(10));
174 LogInfo(
"ReadDigitalInputTask exiting.");
183 bool last_fault =
false;
184 std::string last_error;
185 while (g_running.load()) {
187 const bool fault = teleop.
fault(0);
188 const bool fault_changed = (fault != last_fault);
190 if (fault_changed && fault) {
191 LogError(
"Fault detected. Press 'h' for details, 'F' to clear fault, then 'u' to "
194 }
else if (fault_changed) {
195 LogInfo(
"Fault cleared. Press 'u' to restart teleop.");
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();
204 std::this_thread::sleep_for(std::chrono::milliseconds(100));
206 LogInfo(
"FaultMonitorTask exiting.");
215 auto PrintCommandMenu = []() {
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)
225 --- Teleop Engagement ---
229 --- Null Space Posture ---
230 i/I : Set local nullspace to Preferred/Home posture
232 --- Max Contact Wrench ---
233 p : Set default max contact wrench
235 --- Reinit and start ---
236 u : Recall Init and Start
239 --- TCP message latency ---
240 l : Print current message latency (disconnected / clock mismatch / over limit)
242 --- Teleop status ---
243 h : Print why teleop is restricted / paused and what to do next
245 --- Fault recovery ---
246 F : Clear fault (blocking, up to 30 s), then press 'u' to restart teleop
249 n : Print role() and robot_pair_sn()
252 quit : Stop teleop and exit the program
255 Any other key to show this help menu
259 unsigned int index = 0;
266 auto apply_axis_lock = [&]() {
268 LogWarn(
"Axis lock is only available on the leader");
274 while (g_running.load()) {
276 std::string user_input {};
279 if (!std::getline(std::cin, user_input)) {
280 LogInfo(
"Console input closed.");
284 if (user_input.empty()) {
285 LogWarn(
"Empty command!");
290 if (user_input ==
"quit") {
291 LogInfo(
"Quit requested.");
296 switch (user_input[0]) {
392 double latency_ms {};
395 LogInfo(
"pair 0 message latency: " + FmtFixed(latency_ms, 1)
396 +
" ms (within limit)");
399 LogWarn(
"pair 0 disconnected");
400 }
else if (latency_ms < 0.0) {
402 "pair 0 clock mismatch: latency " + FmtFixed(latency_ms, 1) +
" ms");
404 LogWarn(
"pair 0 latency over limit: " + FmtFixed(latency_ms, 1) +
" ms");
412 LogInfo(
"Clearing fault, this may take up to 30 seconds...");
414 for (
size_t i = 0; i < results.size(); ++i) {
415 LogInfo(
"Teleop pair " + std::to_string(i)
416 +
" clear fault: " + BoolStr(results[i]));
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);
429 LogWarn(
"Invalid command!");
433 }
catch (
const std::exception& e) {
434 LogError(std::string(
"Command '") + user_input[0] +
"' failed: " + e.what());
437 }
catch (
const std::exception& status_e) {
438 LogError(std::string(
"Failed to query teleop status: ") + status_e.what());
442 LogInfo(
"Console thread exiting.");
446int main(
int argc,
char* argv[])
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;
455 while ((opt = getopt_long_only(argc, argv,
"l:f:r:t:i:p:A:W:D", kLongOptions,
nullptr)) != -1) {
458 follower_sn = std::string(optarg);
461 leader_sn = std::string(optarg);
464 teleop_role = std::string(optarg);
467 tcp_role = std::string(optarg);
470 public_server_ip = std::string(optarg);
473 const std::string port_str(optarg);
474 unsigned long port = 0;
477 port = std::stoul(port_str, &parsed);
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");
486 server_port =
static_cast<unsigned int>(port);
490 lan_ip = std::string(optarg);
491 lan_interface_whitelist.push_back(lan_ip);
494 wan_iface = std::string(optarg);
495 wan_interface_whitelist.push_back(wan_iface);
498 enable_digital_input =
true;
505 if (follower_sn.empty() || leader_sn.empty() || tcp_role.empty() || teleop_role.empty()
506 || public_server_ip.empty() || server_port == 0) {
510 if (lan_interface_whitelist.empty()) {
511 LogWarn(
"LAN whitelist is not provided, will search all network interfaces.");
513 if (wan_interface_whitelist.empty()) {
514 LogWarn(
"WAN interface whitelist is not provided, will search all network interfaces.");
519 if (tcp_role ==
"server") {
520 is_tcp_server =
true;
521 }
else if (tcp_role ==
"client") {
522 is_tcp_server =
false;
524 LogError(
"Valid inputs for [-t] are: server, client");
529 if (teleop_role ==
"follower") {
531 }
else if (teleop_role ==
"leader") {
534 LogError(
"Valid inputs for [-r] are: follower, leader");
546 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
547 robot_sn_pairs.push_back({leader_sn, follower_sn});
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);
564 tctw.SetMaxContactWrench(0, kDefaultMaxContactWrench);
567 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
568 std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctw));
571 std::optional<std::thread> pedal_thread;
572 if (teleop_role ==
"leader" && enable_digital_input) {
574 "Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
576 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw));
579 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
584 console_thread.join();
590 fault_monitor_thread.join();
591 if (pedal_thread && pedal_thread->joinable()) {
592 pedal_thread->join();
598 }
catch (
const std::exception& e) {
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.
@ WAN_TELEOP_LEADER
The leader robot operated by a human during teleoperation over WAN.
TeleopIssueCode
Issue code and possible reasons for a teleoperation restriction or interruption.
@ NETWORK_DISCONNECTED
Connection to the peer is not established.
@ COORD_WORLD
WORLD coordinate of local robot.
@ COORD_TCP
TCP coordinate of local robot.
Data for locking axis, including reference frame and axis to be locked for transparent cartesian tele...
std::array< bool, 3 > lock_ori_axis
Orientation axis lock, the corresponding axis order is . True for locking, false for floating.
std::array< bool, 3 > lock_trans_axis
Translation axis lock, the corresponding axis order is . True for locking, false for floating.
CoordType coord
Reference coordinate that the axis to be locked.
TCP Server and Client Configuration TDK Standard Edition. In a teleoperation-over-WAN setup,...
std::string public_ipv4_address
std::vector< std::string > lan_interface_whitelist
unsigned int listening_port
std::vector< std::string > wan_interface_whitelist
One restriction or interruption that the operator should understand.
Snapshot of teleoperation health for one robot pair.
std::vector< TeleopIssue > issues
double latency_threshold_ms