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};
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};
45const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
46 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
49constexpr unsigned int kIdx = 0;
52std::atomic<bool> g_running {
true};
58std::string g_gripper_name;
62std::string BoolStr(
bool v)
64 return v ?
"true" :
"false";
67std::string FmtFixed(
double v,
int prec)
69 std::ostringstream oss;
70 oss << std::fixed << std::setprecision(prec) << v;
74void LogInfo(
const std::string& msg)
76 std::cout <<
"[info] " << msg << std::endl;
79void LogWarn(
const std::string& msg)
81 std::cerr <<
"[warn] " << msg << std::endl;
84void LogError(
const std::string& msg)
86 std::cerr <<
"[error] " << msg << std::endl;
91 return std::any_of(status.
issues.begin(), status.
issues.end(),
97 LogInfo(
"Teleop pair " + std::to_string(idx) +
" status: initialized="
99 +
" engaged=" + BoolStr(status.
engaged) +
" stopped=" + BoolStr(status.
stopped)
100 +
" fault=" + BoolStr(status.
fault)
105 LogInfo(
"No teleop restriction. Safe to continue.");
108 const auto& issue = status.
primary;
110 LogInfo(std::string(
"No active teleop restriction. Last cleared issue: [")
111 + flexiv::tdk::TeleopIssueCodeStr[
static_cast<size_t>(issue.code)] +
"] "
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) {
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)] +
"] "
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());
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;
160const struct option kLongOptions[] = {
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'},
179 std::optional<flexiv::tdk::GripperRemoteControl>& gripper)
181 bool logged_idle =
false;
182 bool di1_stable =
false;
184 bool gripper_opened =
false;
185 std::string last_error;
187 while (g_running.load()) {
195 "ReadDigitalInputTask: teleop is not started, pause Engage "
196 "(call Init + Start to resume)");
202 teleop.
Engage(kIdx, digital_inputs[0]);
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;
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");
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");
232 }
catch (
const std::exception& e) {
233 LogWarn(std::string(
"DI1 gripper action failed: ") + e.what());
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();
249 std::this_thread::sleep_for(std::chrono::milliseconds(10));
251 LogInfo(
"ReadDigitalInputTask exiting.");
259 bool last_fault =
false;
260 std::string last_error;
261 while (g_running.load()) {
263 const bool fault = teleop.
fault(kIdx);
264 const bool fault_changed = (fault != last_fault);
266 if (fault_changed && fault) {
267 LogError(
"Fault detected. Press 'h' for details, 'F' to clear fault, then 'u' to "
270 }
else if (fault_changed) {
271 LogInfo(
"Fault cleared. Press 'u' to restart teleop.");
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();
280 std::this_thread::sleep_for(std::chrono::milliseconds(100));
282 LogInfo(
"FaultMonitorTask exiting.");
292 std::optional<flexiv::tdk::GripperRemoteControl>& gripper,
bool di_controls_gripper)
294 auto PrintCommandMenu = [has_gripper = gripper.has_value()]() {
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)
304 --- Teleop Engagement ---
308 --- Null Space Posture ---
309 i/I : Set local nullspace to Preferred/Home posture
311 --- Max Contact Wrench ---
312 p : Set default max contact wrench
314 --- Reinit and start ---
315 u : Recall Init and Start
318 --- TCP message latency ---
319 l : Print current message latency (disconnected / clock mismatch / over limit)
321 --- Teleop status ---
322 h : Print why teleop is restricted / paused and what to do next
324 --- Fault recovery ---
325 F : Clear fault (blocking, up to 30 s), then press 'u' to restart teleop
328 n : Print role() and robot_pair_sn()
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
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
343 --- Gripper States ---
344 t : Print latest gripper states
345 P : Print gripper params (valid command ranges)
351 quit : Stop teleop and exit the program
354 Any other key to show this help menu
364 auto apply_axis_lock = [&]() {
366 LogWarn(
"Axis lock is only available on the leader");
372 while (g_running.load()) {
374 std::string user_input {};
377 if (!std::getline(std::cin, user_input)) {
378 LogInfo(
"Console input closed.");
382 if (user_input.empty()) {
383 LogWarn(
"Empty command!");
388 if (user_input ==
"quit") {
389 LogInfo(
"Quit requested.");
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].");
401 && std::string(
"GDNocs").find(command) != std::string::npos) {
402 LogWarn(std::string(
"Command '") + command +
"' is only available on the leader");
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 "
408 + command +
"' is disabled.");
486 teleop.
Engage(kIdx,
true);
489 teleop.
Engage(kIdx,
false);
508 double latency_ms {};
511 LogInfo(
"pair " + std::to_string(kIdx) +
" message latency: "
512 + FmtFixed(latency_ms, 1) +
" ms (within limit)");
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");
520 LogWarn(
"pair " + std::to_string(kIdx) +
" latency over limit: "
521 + FmtFixed(latency_ms, 1) +
" ms");
529 LogInfo(
"Clearing fault, this may take up to 30 seconds...");
531 for (
size_t i = 0; i < results.size(); ++i) {
532 LogInfo(
"Teleop pair " + std::to_string(i)
533 +
" clear fault: " + BoolStr(results[i]));
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);
545 gripper->Enable(kIdx, g_gripper_name);
546 LogInfo(
"Follower gripper [" + g_gripper_name +
"] enabled");
549 gripper->Disable(kIdx);
550 LogInfo(
"Follower gripper disabled");
554 LogInfo(
"Follower gripper initialization triggered");
557 auto params = gripper->params(kIdx);
559 = std::clamp(0.5 * params.max_vel, params.min_vel, params.max_vel);
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");
569 auto params = gripper->params(kIdx);
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");
578 LogInfo(
"Stop command sent");
581 PrintGripperStates(*gripper);
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());
597 LogWarn(
"Invalid command!");
601 }
catch (
const std::exception& e) {
602 LogError(std::string(
"Command '") + user_input[0] +
"' failed: " + e.what());
605 }
catch (
const std::exception& status_e) {
606 LogError(std::string(
"Failed to query teleop status: ") + status_e.what());
610 LogInfo(
"Console thread exiting.");
613int main(
int argc,
char* argv[])
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;
620 while ((opt = getopt_long_only(argc, argv,
"l:f:r:c:A:n:D", kLongOptions,
nullptr)) != -1) {
623 follower_sn = std::string(optarg);
626 leader_sn = std::string(optarg);
629 teleop_role = std::string(optarg);
632 client_config_file = std::string(optarg);
635 lan_ip = std::string(optarg);
636 lan_interface_whitelist.push_back(lan_ip);
639 g_gripper_name = std::string(optarg);
642 enable_digital_input =
true;
649 if (follower_sn.empty() || leader_sn.empty() || teleop_role.empty()
650 || client_config_file.empty()) {
654 if (lan_interface_whitelist.empty()) {
655 LogWarn(
"LAN whitelist is not provided, RDK will search all network interfaces.");
658 if (teleop_role ==
"follower") {
660 }
else if (teleop_role ==
"leader") {
663 LogError(
"Valid inputs for [-r] are: follower, leader");
671 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
672 robot_sn_pairs.push_back({leader_sn, follower_sn});
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);
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
690 LogInfo(
"Gripper remote control is DISABLED (no [-n gripper_name] provided).");
695 tctw.SetMaxContactWrench(kIdx, kDefaultMaxContactWrench);
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.");
709 const bool start_pedal_thread
711 const bool di_controls_gripper = start_pedal_thread && gripper.has_value();
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));
717 std::optional<std::thread> pedal_thread;
718 if (start_pedal_thread) {
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)")
725 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw), std::ref(gripper));
728 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
732 console_thread.join();
733 g_running.store(
false);
735 fault_monitor_thread.join();
736 if (pedal_thread && pedal_thread->joinable()) {
737 pedal_thread->join();
742 }
catch (
const std::exception& e) {
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.
@ 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.
Network configuration for TDK Professional Edition (credential file from TDK Server).
std::string client_config_file
std::vector< std::string > lan_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