Example usage of Transparent Cartesian teleoperation cross Wide Area Network for controlling a follower robot using a leader robot with transparent force feedback. Supports both keyboard and digital input engage/disengage signal reading, with message latency query, nullspace posture tuning, and max contact wrench setting, etc.
Example usage of Transparent Cartesian teleoperation cross Wide Area Network for controlling a follower robot using a leader robot with transparent force feedback. Supports both keyboard and digital input engage/disengage signal reading, with message latency query, nullspace posture tuning, and max contact wrench setting, etc.
#include <spdlog/spdlog.h>
#include <getopt.h>
#include <atomic>
#include <chrono>
#include <iostream>
#include <thread>
#include <optional>
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
= {50.0, 50.0, 50.0, 40.0, 40.0, 40.0};
std::atomic<bool> g_running {true};
constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
}
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<<" -t [necessary] Role in the TCP connection, can be [server] or [client]."<<std::endl;
std::cout<<" -i [necessary] Public IPV4 address of the machine that functions as TCP server."<<std::endl;
std::cout<<" -p [necessary] Listening port of the TCP server machine."<<std::endl;
std::cout<<" -W [optional] OS-level name(s) of the network interface(s) that connect to the internet." << std::endl;
std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
std::cout<<"Usage: sudo ./transparent_cartesian_teleop_wan [-l leader_robot_serial_number] [-f follower_robot_serial_number] [-r leader/follower] [-t server/client] [-i server_public_ip] [-p server_port] [-A lan_interface_ip] [-W wan_interface_ip] [-D]"<<std::endl;
}
const struct option kLongOptions[] = {
{"leader SN", required_argument, 0, 'l'},
{"follower SN", required_argument, 0, 'f'},
{"role", required_argument, 0, 'r'},
{"tcp role", required_argument, 0, 't'},
{"public ipv4 address", required_argument, 0, 'i'},
{"port", required_argument, 0, 'p'},
{"wan whitelist NIC name", optional_argument, 0, 'W'},
{"enable digital input", no_argument, 0, 'D'},
{0, 0, 0, 0 }
};
{
while (g_running.load() && !teleop.
fault(0)) {
try {
} catch (const std::exception& e) {
spdlog::error("Exception in ReadDigitalInputTask: {}", e.what());
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
spdlog::info("ReadDigitalInputTask exiting.");
return;
}
{
auto PrintCommandMenu = []() {
std::cout << R"(
--- 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 in milliseconds
--- Help ---
Any other key to show this help menu
)" << std::endl;
};
while (g_running.load() && !teleop.
fault(0)) {
std::string user_input {};
std::getline(std::cin, user_input);
if (user_input.empty()) {
spdlog::warn("Empty command!");
PrintCommandMenu();
continue;
}
try {
switch (user_input[0]) {
case 'r':
teleop.
Engage(0, kJointGroup,
true);
break;
case 'R':
teleop.
Engage(0, kJointGroup,
false);
break;
case 'i':
break;
case 'I':
break;
case 'p':
break;
case 'u':
break;
case 'U':
break;
case 'l': {
double latency_ms {};
spdlog::info("Current message latency is: {}ms", latency_ms);
} else {
spdlog::warn("WAN teleop is disconnected.");
}
break;
}
default:
spdlog::warn("Invalid command!");
PrintCommandMenu();
break;
}
} catch (const std::exception& e) {
spdlog::error("Exception in ConsoleTask: {}", e.what());
g_running.store(false);
return;
}
}
spdlog::info("Console thread exiting.");
return;
}
int main(int argc, char* argv[])
{
std::string follower_sn, leader_sn, teleop_role, tcp_role, public_server_ip, nic_name;
unsigned int server_port = 0;
std::vector<std::string> wan_interface_whitelist {};
bool enable_digital_input = false;
int opt = 0;
while ((opt = getopt_long_only(argc, argv, "l:f:r:t:i:p:W: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 't':
tcp_role = std::string(optarg);
break;
case 'i':
public_server_ip = std::string(optarg);
break;
case 'p':
try {
server_port = std::stoi(optarg);
} catch (...) {
spdlog::error("Invalid port number: {}", optarg);
return 1;
}
break;
case 'W':
nic_name = std::string(optarg);
wan_interface_whitelist.push_back(nic_name);
break;
case 'D':
enable_digital_input = true;
break;
default:
PrintHelp();
return 1;
}
}
if (follower_sn.empty() || leader_sn.empty() || tcp_role.empty() || teleop_role.empty()
|| public_server_ip.empty() || server_port == 0) {
PrintHelp();
return 1;
}
if (wan_interface_whitelist.empty()) {
spdlog::warn(
"WAN interface whitelist is not provided, will search all network interfaces.");
}
bool is_tcp_server;
if (tcp_role == "server") {
is_tcp_server = true;
} else if (tcp_role == "client") {
is_tcp_server = false;
} else {
spdlog::error("Valid inputs for [-t] are: server, client");
return 1;
}
if (teleop_role == "follower") {
} else if (teleop_role == "leader") {
} else {
spdlog::error("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 {
tctw.Init();
tctw.Start();
tctw.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
std::optional<std::thread> pedal_thread;
if (teleop_role == "leader" && enable_digital_input) {
spdlog::info(
"Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
"flag.");
pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw));
} else {
spdlog::info(
"ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
"provided).");
}
console_thread.join();
g_running = false;
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
tctw.Stop();
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
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 SetMaxContactWrench(unsigned int idx, JointGroup group, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the current role in the robot pairs....
void Start()
[Non-Blocking] Start the teleoperation control loop for current roles (leaders or followers) specifie...
bool CheckTeleopConnectionLatency(unsigned int idx, double &latency_ms) const
Non-blocking check of TCP connection status and average message latency.
bool fault(unsigned int idx) const
[Non-blocking] Whether the current role is in fault state.
void SetNullSpacePosture(unsigned int idx, JointGroup group, 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 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 Engage(unsigned int idx, JointGroup group, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot in the specified robot pair....
Header file containing various constant expressions, data structs, and enums.
Role
Roles of participants in transparent cartesian teleop.
TCP Server and Client Configuration TDK Standard Edition. In TDK standard edition,...
std::string public_ipv4_address
unsigned int listening_port
std::vector< std::string > wan_interface_whitelist