Flexiv TDK APIs 2.1
Loading...
Searching...
No Matches
transparent_cartesian_teleop_wan.cpp

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.

Note
This program is provided only as an example. Users must adapt it to their own application requirements, safety procedures, and software architecture before deployment.
Author
Flexiv
#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()
{
// clang-format off
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;
// clang-format on
}
const struct option kLongOptions[] = {
// clang-format off
{"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 }
// clang-format on
};
void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
{
while (g_running.load() && !teleop.fault(0)) {
try {
teleop.Engage(0, kJointGroup, teleop.digital_inputs(0)[0]);
} 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':
teleop.SetNullSpacePosture(0, kJointGroup, kPreferredJntPos);
break;
case 'I':
teleop.SetNullSpacePosture(0, kJointGroup, kHomeJntPos);
break;
case 'p':
teleop.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
break;
case 'u':
teleop.Init();
teleop.Start();
break;
case 'U':
teleop.Stop();
break;
case 'l': {
double latency_ms {};
if (teleop.CheckTeleopConnectionLatency(0, 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.");
}
// Whether this is a TCP server or client
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;
}
// Whether this is leader or follower
if (teleop_role == "follower") {
} else if (teleop_role == "leader") {
} else {
spdlog::error("Valid inputs for [-r] are: follower, leader");
return 1;
}
// Network configuration
network_cfg.is_tcp_server = is_tcp_server;
network_cfg.public_ipv4_address = public_server_ip;
network_cfg.listening_port = server_port;
network_cfg.wan_interface_whitelist = wan_interface_whitelist;
std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
robot_sn_pairs.push_back({leader_sn, follower_sn});
try {
// Allocate tdk object
flexiv::tdk::TransparentCartesianTeleopWAN tctw(robot_sn_pairs, kRole, network_cfg);
// Init high transparency teleop
tctw.Init();
// Start high transparency teleop
tctw.Start();
// Set max contact wrench
tctw.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
// Start console_thread
std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
// Start pedal_thread based on the new flag or the role
std::optional<std::thread> pedal_thread;
// Modified condition: Start if -D is given OR if role is leader (original behavior)
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).");
}
// Wait for console_thread to finish
console_thread.join();
// Stop all threads, notify other threads exit
g_running = false;
// Wait for digital reading task exit
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
// Exit high transparency teleop
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.
Definition data.hpp:95
@ WAN_TELEOP_LEADER
Definition data.hpp:98
@ WAN_TELEOP_FOLLOWER
Definition data.hpp:99
TCP Server and Client Configuration TDK Standard Edition. In TDK standard edition,...
Definition data.hpp:56
std::string public_ipv4_address
Definition data.hpp:71
unsigned int listening_port
Definition data.hpp:77
std::vector< std::string > wan_interface_whitelist
Definition data.hpp:87