Example usage of Transparent Cartesian teleoperation over WAN (TDK Professional Edition, TDK Server credential). Controls a follower robot from a leader robot with transparent force feedback. Supports keyboard and digital input engage/disengage, message latency query, nullspace posture tuning, max contact wrench setting, and teleop status query. Optionally enables GripperRemoteControl when a follower gripper device name is provided.
Example usage of Transparent Cartesian teleoperation over WAN (TDK Professional Edition, TDK Server credential). Controls a follower robot from a leader robot with transparent force feedback. Supports keyboard and digital input engage/disengage, message latency query, nullspace posture tuning, max contact wrench setting, and teleop status query. Optionally enables GripperRemoteControl when a follower gripper device name is provided.
#include <getopt.h>
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <functional>
#include <iomanip>
#include <iostream>
#include <optional>
#include <sstream>
#include <string>
#include <thread>
#include <utility>
#include <vector>
namespace {
std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
= {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
constexpr unsigned int kIdx = 0;
std::atomic<bool> g_running {true};
std::string g_gripper_name;
}
std::string BoolStr(bool v)
{
return v ? "true" : "false";
}
std::string FmtFixed(double v, int prec)
{
std::ostringstream oss;
oss << std::fixed << std::setprecision(prec) << v;
return oss.str();
}
void LogInfo(const std::string& msg)
{
std::cout << "[info] " << msg << std::endl;
}
void LogWarn(const std::string& msg)
{
std::cerr << "[warn] " << msg << std::endl;
}
void LogError(const std::string& msg)
{
std::cerr << "[error] " << msg << std::endl;
}
{
return std::any_of(status.
issues.begin(), status.
issues.end(),
}
{
LogInfo("Teleop pair " + std::to_string(idx) + " status: initialized="
+
" engaged=" + BoolStr(status.
engaged) +
" stopped=" + BoolStr(status.
stopped)
+
" fault=" + BoolStr(status.
fault)
+ " ms");
LogInfo("No teleop restriction. Safe to continue.");
return;
}
const auto& issue = status.
primary;
if (!issue.active) {
LogInfo(std::string("No active teleop restriction. Last cleared issue: [")
+ flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(issue.code)] + "] "
+ issue.title);
return;
}
LogWarn(std::string("[")
+ flexiv::tdk::TeleopIssueLevelStr[static_cast<size_t>(issue.level)] + "/"
+ flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(issue.side)] + "] "
+ issue.title + " | " + issue.description + " | " + issue.suggestion);
for (
const auto& extra : status.
issues) {
if (extra.code == issue.code && extra.side == issue.side
&& extra.joint_index == issue.joint_index) {
continue;
}
LogWarn(std::string(" also: [")
+ flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(extra.code)] + "/"
+ flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(extra.side)] + "] "
+ extra.title);
}
}
{
try {
auto states = gripper.
states(kIdx);
LogInfo("Gripper states: width = " + FmtFixed(states.width, 4) + " m, force = "
+ FmtFixed(states.force, 2) + " N, is_moving = " + BoolStr(states.is_moving));
} catch (const std::exception& e) {
LogWarn(std::string("Gripper states not available yet: ") + e.what());
}
}
void PrintHelp()
{
std::cout<<"Invalid program arguments!"<<std::endl;
std::cout<<" -l [necessary] serial number of leader robot."<<std::endl;
std::cout<<" -f [necessary] serial number of follower robot."<<std::endl;
std::cout<<" -r [necessary] Role of participants in teleop. can be [follower] or [leader]"<<std::endl;
std::cout<<" -c [necessary] Path to client.conf from the TDK Server credential package."<<std::endl;
std::cout<<" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
std::cout<<" -n [optional] Follower gripper device name configured in Elements -> Device." << std::endl;
std::cout<<" To enable GripperRemoteControl, both leader and follower sides must provide the same gripper name." << std::endl;
std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
std::cout<<" Leader DI0 engages arm teleop; with [-n], DI1 toggles the follower gripper" << std::endl;
std::cout<<" and the console gripper motion commands (o/c/s) are disabled." << std::endl;
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;
}
const struct option kLongOptions[] = {
{"leader SN", required_argument, 0, 'l'},
{"follower SN", required_argument, 0, 'f'},
{"role", required_argument, 0, 'r'},
{"client config", required_argument, 0, 'c'},
{"lan whitelist ip", required_argument, 0, 'A'},
{"gripper name", required_argument, 0, 'n'},
{"enable digital input", no_argument, 0, 'D'},
{0, 0, 0, 0 }
};
std::optional<flexiv::tdk::GripperRemoteControl>& gripper)
{
bool logged_idle = false;
bool di1_stable = false;
int di1_counter = 0;
bool gripper_opened = false;
std::string last_error;
while (g_running.load()) {
try {
if (!logged_idle) {
LogWarn(
"ReadDigitalInputTask: teleop is not started, pause Engage "
"(call Init + Start to resume)");
logged_idle = true;
}
} else {
logged_idle = false;
teleop.
Engage(kIdx, digital_inputs[0]);
if (gripper.has_value() && digital_inputs.size() > 1) {
const bool di1_raw = digital_inputs[1];
if (di1_raw != di1_stable) {
if (++di1_counter >= 3) {
di1_stable = di1_raw;
di1_counter = 0;
if (di1_stable) {
try {
auto params = gripper->params(kIdx);
if (!gripper_opened) {
double width = std::clamp(
0.5 * params.max_width, params.min_width, params.max_width);
double velocity = std::clamp(
0.5 * params.max_vel, params.min_vel, params.max_vel);
double force_limit = std::clamp(
0.5 * params.max_force, params.min_force, params.max_force);
gripper->Move(kIdx, width, velocity, force_limit);
gripper_opened = true;
LogInfo("DI1 pressed: opening gripper to width = "
+ FmtFixed(width, 4) + " m");
} else {
double force
= std::clamp(40.0, params.min_force, params.max_force);
gripper->Grasp(kIdx, force);
gripper_opened = false;
LogInfo("DI1 pressed: closing gripper with force = "
+ FmtFixed(force, 2) + " N");
}
} catch (const std::exception& e) {
LogWarn(std::string("DI1 gripper action failed: ") + e.what());
}
}
}
} else {
di1_counter = 0;
}
}
}
last_error.clear();
} catch (const std::exception& e) {
if (last_error != e.what()) {
LogError(std::string("Exception in ReadDigitalInputTask: ") + e.what());
last_error = e.what();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
LogInfo("ReadDigitalInputTask exiting.");
}
{
bool last_fault = false;
std::string last_error;
while (g_running.load()) {
try {
const bool fault = teleop.
fault(kIdx);
const bool fault_changed = (fault != last_fault);
last_fault = fault;
if (fault_changed && fault) {
LogError("Fault detected. Press 'h' for details, 'F' to clear fault, then 'u' to "
"restart teleop.");
} else if (fault_changed) {
LogInfo("Fault cleared. Press 'u' to restart teleop.");
}
last_error.clear();
} catch (const std::exception& e) {
if (last_error != e.what()) {
LogError(std::string("Exception in FaultMonitorTask: ") + e.what());
last_error = e.what();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
LogInfo("FaultMonitorTask exiting.");
}
std::optional<flexiv::tdk::GripperRemoteControl>& gripper, bool di_controls_gripper)
{
auto PrintCommandMenu = [has_gripper = gripper.has_value()]() {
std::cout << R"(
--- Axis Lock (leader only) ---
x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
a : Unlock all axes (TCP coord)
A : Lock all axes (TCP coord)
--- Teleop Engagement ---
r : Engage teleop
R : Disengage teleop
--- Null Space Posture ---
i/I : Set local nullspace to Preferred/Home posture
--- Max Contact Wrench ---
p : Set default max contact wrench
--- Reinit and start ---
u : Recall Init and Start
U : Stop teleop
--- TCP message latency ---
l : Print current message latency (disconnected / clock mismatch / over limit)
--- Teleop status ---
h : Print why teleop is restricted / paused and what to do next
--- Fault recovery ---
F : Clear fault (blocking, up to 30 s), then press 'u' to restart teleop
--- Identity ---
n : Print role() and robot_pair_sn()
)" << std::endl;
if (has_gripper) {
std::cout << R"(
--- Gripper Lifecycle (Leader only, via RPC) ---
G : Enable gripper on follower robot
D : Disable gripper on follower robot
N : Trigger initialization of the enabled gripper
--- Gripper Motion (Leader only, via reliable topic, disabled when -D is enabled) ---
o : Open gripper (Move to max width)
c : Close gripper (Grasp with moderate force)
s : Stop and hold gripper
--- Gripper States ---
t : Print latest gripper states
P : Print gripper params (valid command ranges)
)" << std::endl;
}
std::cout << R"(
--- Exit ---
quit : Stop teleop and exit the program
--- Help ---
Any other key to show this help menu
)" << std::endl;
};
if (is_leader) {
}
auto apply_axis_lock = [&]() {
if (!is_leader) {
LogWarn("Axis lock is only available on the leader");
return;
}
};
while (g_running.load()) {
std::string user_input {};
if (!std::getline(std::cin, user_input)) {
LogInfo("Console input closed.");
break;
}
if (user_input.empty()) {
LogWarn("Empty command!");
PrintCommandMenu();
continue;
}
if (user_input == "quit") {
LogInfo("Quit requested.");
break;
}
try {
const char command = user_input[0];
if (std::string("GDNocstP").find(command) != std::string::npos
&& !gripper.has_value()) {
LogWarn("Gripper commands require [-n gripper_name].");
continue;
}
&& std::string("GDNocs").find(command) != std::string::npos) {
LogWarn(std::string("Command '") + command + "' is only available on the leader");
continue;
}
if (di_controls_gripper && std::string("ocs").find(command) != std::string::npos) {
LogWarn(std::string("Gripper motion is controlled by DI1 (-D enabled); console "
"command '")
+ command + "' is disabled.");
continue;
}
switch (command) {
case 'x':
apply_axis_lock();
break;
case 'y':
apply_axis_lock();
break;
case 'z':
apply_axis_lock();
break;
case 'q':
apply_axis_lock();
break;
case 'w':
apply_axis_lock();
break;
case 'e':
apply_axis_lock();
break;
case 'X':
apply_axis_lock();
break;
case 'Y':
apply_axis_lock();
break;
case 'Z':
apply_axis_lock();
break;
case 'Q':
apply_axis_lock();
break;
case 'W':
apply_axis_lock();
break;
case 'E':
apply_axis_lock();
break;
case 'a':
apply_axis_lock();
break;
case 'A':
apply_axis_lock();
break;
case 'r':
break;
case 'R':
break;
case 'i':
break;
case 'I':
break;
case 'p':
break;
case 'u':
break;
case 'U':
break;
case 'l': {
double latency_ms {};
if (ok) {
LogInfo("pair " + std::to_string(kIdx) + " message latency: "
+ FmtFixed(latency_ms, 1) + " ms (within limit)");
LogWarn("pair " + std::to_string(kIdx) + " disconnected");
} else if (latency_ms < 0.0) {
LogWarn("pair " + std::to_string(kIdx) + " clock mismatch: latency "
+ FmtFixed(latency_ms, 1) + " ms");
} else {
LogWarn("pair " + std::to_string(kIdx) + " latency over limit: "
+ FmtFixed(latency_ms, 1) + " ms");
}
break;
}
case 'h':
break;
case 'F': {
LogInfo("Clearing fault, this may take up to 30 seconds...");
for (size_t i = 0; i < results.size(); ++i) {
LogInfo("Teleop pair " + std::to_string(i)
+ " clear fault: " + BoolStr(results[i]));
}
break;
}
case 'n': {
LogInfo("pair " + std::to_string(kIdx) + " role="
+ flexiv::tdk::RoleTypeStr[
static_cast<size_t>(teleop.
role())]
+ " leader_sn=" + pair.first + " follower_sn=" + pair.second);
break;
}
case 'G':
gripper->Enable(kIdx, g_gripper_name);
LogInfo("Follower gripper [" + g_gripper_name + "] enabled");
break;
case 'D':
gripper->Disable(kIdx);
LogInfo("Follower gripper disabled");
break;
case 'N':
gripper->Init(kIdx);
LogInfo("Follower gripper initialization triggered");
break;
case 'o': {
auto params = gripper->params(kIdx);
double velocity
= std::clamp(0.5 * params.max_vel, params.min_vel, params.max_vel);
double force_limit
= std::clamp(0.5 * params.max_force, params.min_force, params.max_force);
gripper->Move(kIdx, params.max_width, velocity, force_limit);
LogInfo("Open command sent: width = " + FmtFixed(params.max_width, 4)
+ " m, velocity = " + FmtFixed(velocity, 4)
+ " m/s, force_limit = " + FmtFixed(force_limit, 2) + " N");
break;
}
case 'c': {
auto params = gripper->params(kIdx);
double force
= std::clamp(0.5 * params.max_force, params.min_force, params.max_force);
gripper->Grasp(kIdx, force);
LogInfo("Grasp command sent: force = " + FmtFixed(force, 2) + " N");
break;
}
case 's':
gripper->Stop(kIdx);
LogInfo("Stop command sent");
break;
case 't':
PrintGripperStates(*gripper);
break;
case 'P': {
try {
auto params = gripper->params(kIdx);
LogInfo("Gripper params: width = [" + FmtFixed(params.min_width, 4)
+ ", " + FmtFixed(params.max_width, 4) + "] m, velocity = ["
+ FmtFixed(params.min_vel, 4) + ", " + FmtFixed(params.max_vel, 4)
+ "] m/s, force = [" + FmtFixed(params.min_force, 2) + ", "
+ FmtFixed(params.max_force, 2) + "] N");
} catch (const std::exception& e) {
LogWarn(std::string("Gripper params not available yet: ") + e.what());
}
break;
}
default:
LogWarn("Invalid command!");
PrintCommandMenu();
break;
}
} catch (const std::exception& e) {
LogError(std::string("Command '") + user_input[0] + "' failed: " + e.what());
try {
} catch (const std::exception& status_e) {
LogError(std::string("Failed to query teleop status: ") + status_e.what());
}
}
}
LogInfo("Console thread exiting.");
}
int main(int argc, char* argv[])
{
std::string follower_sn, leader_sn, teleop_role, client_config_file, lan_ip;
std::vector<std::string> lan_interface_whitelist {};
bool enable_digital_input = false;
int opt = 0;
while ((opt = getopt_long_only(argc, argv, "l:f:r:c:A:n:D", kLongOptions, nullptr)) != -1) {
switch (opt) {
case 'f':
follower_sn = std::string(optarg);
break;
case 'l':
leader_sn = std::string(optarg);
break;
case 'r':
teleop_role = std::string(optarg);
break;
case 'c':
client_config_file = std::string(optarg);
break;
case 'A':
lan_ip = std::string(optarg);
lan_interface_whitelist.push_back(lan_ip);
break;
case 'n':
g_gripper_name = std::string(optarg);
break;
case 'D':
enable_digital_input = true;
break;
default:
PrintHelp();
return 1;
}
}
if (follower_sn.empty() || leader_sn.empty() || teleop_role.empty()
|| client_config_file.empty()) {
PrintHelp();
return 1;
}
if (lan_interface_whitelist.empty()) {
LogWarn("LAN whitelist is not provided, RDK will search all network interfaces.");
}
if (teleop_role == "follower") {
} else if (teleop_role == "leader") {
} else {
LogError("Valid inputs for [-r] are: follower, leader");
return 1;
}
std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
robot_sn_pairs.push_back({leader_sn, follower_sn});
try {
const auto pair_sn = tctw.robot_pair_sn(kIdx);
LogInfo(std::string("This instance role=")
+ flexiv::tdk::RoleTypeStr[static_cast<size_t>(tctw.role())]
+ " leader_sn=" + pair_sn.first + " follower_sn=" + pair_sn.second);
std::optional<flexiv::tdk::GripperRemoteControl> gripper;
if (!g_gripper_name.empty()) {
gripper.emplace(tctw, network_cfg);
LogInfo("Gripper remote control is ENABLED, target gripper device: [" + g_gripper_name
+ "]");
} else {
LogInfo("Gripper remote control is DISABLED (no [-n gripper_name] provided).");
}
tctw.Init();
tctw.Start();
tctw.SetMaxContactWrench(kIdx, kDefaultMaxContactWrench);
try {
gripper->EnableLocal(kIdx, g_gripper_name);
} catch (const std::exception& e) {
LogError(std::string("Failed to auto-enable gripper [") + g_gripper_name
+ "] on follower: " + e.what()
+ ". The leader can still retry via the 'G' console command.");
}
}
const bool start_pedal_thread
const bool di_controls_gripper = start_pedal_thread && gripper.has_value();
std::thread console_thread(
std::bind(ConsoleTask, std::ref(tctw), std::ref(gripper), di_controls_gripper));
std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctw));
std::optional<std::thread> pedal_thread;
if (start_pedal_thread) {
LogInfo(std::string(
"Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
"flag. DI0: arm teleop engagement; DI1: ")
+ (gripper.has_value() ? "toggles follower gripper open/close"
: "unused (gripper feature disabled, no -n)")
+ ".");
pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw), std::ref(gripper));
} else {
LogInfo(
"ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
"provided).");
}
console_thread.join();
g_running.store(false);
fault_monitor_thread.join();
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
tctw.Stop();
} catch (const std::exception& e) {
LogError(e.what());
return 1;
}
return 0;
}
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