16#include <spdlog/spdlog.h>
28std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
29 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
32std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
33 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
36const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
37 = {50.0, 50.0, 50.0, 40.0, 40.0, 40.0};
40std::atomic<bool> g_running {
true};
46constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
52 std::cout<<
"Invalid program arguments!"<<std::endl;
53 std::cout<<
" -l [necessary] serial number of leader robot."<<std::endl;
54 std::cout<<
" -f [necessary] serial number of follower robot."<<std::endl;
55 std::cout<<
" -r [necessary] Role of participants in teleop. can be [follower] or [leader]"<<std::endl;
56 std::cout<<
" -t [necessary] Role in the TCP connection, can be [server] or [client]."<<std::endl;
57 std::cout<<
" -i [necessary] Public IPV4 address of the machine that functions as TCP server."<<std::endl;
58 std::cout<<
" -p [necessary] Listening port of the TCP server machine."<<std::endl;
59 std::cout<<
" -W [optional] OS-level name(s) of the network interface(s) that connect to the internet." << std::endl;
60 std::cout<<
" -D [optional] Enable Digital Input reading task." << std::endl;
61 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;
65const struct option kLongOptions[] = {
67 {
"leader SN", required_argument, 0,
'l'},
68 {
"follower SN", required_argument, 0,
'f'},
69 {
"role", required_argument, 0,
'r'},
70 {
"tcp role", required_argument, 0,
't'},
71 {
"public ipv4 address", required_argument, 0,
'i'},
72 {
"port", required_argument, 0,
'p'},
73 {
"wan whitelist NIC name", optional_argument, 0,
'W'},
74 {
"enable digital input", no_argument, 0,
'D'},
84 while (g_running.load() && !teleop.
fault(0)) {
87 }
catch (
const std::exception& e) {
88 spdlog::error(
"Exception in ReadDigitalInputTask: {}", e.what());
90 std::this_thread::sleep_for(std::chrono::milliseconds(10));
92 spdlog::info(
"ReadDigitalInputTask exiting.");
101 auto PrintCommandMenu = []() {
103 --- Teleop Engagement ---
107 --- Null Space Posture ---
108 i/I : Set local nullspace to Preferred/Home posture
110 --- Max Contact Wrench ---
111 p : Set default max contact wrench
113 --- Reinit and start ---
114 u : Recall Init and Start
117 --- Tcp message latency ---
118 l : print current message latency in milliseconds
121 Any other key to show this help menu
125 while (g_running.load() && !teleop.
fault(0)) {
127 std::string user_input {};
129 std::getline(std::cin, user_input);
131 if (user_input.empty()) {
132 spdlog::warn(
"Empty command!");
138 switch (user_input[0]) {
140 teleop.
Engage(0, kJointGroup,
true);
143 teleop.
Engage(0, kJointGroup,
false);
162 double latency_ms {};
164 spdlog::info(
"Current message latency is: {}ms", latency_ms);
166 spdlog::warn(
"WAN teleop is disconnected.");
172 spdlog::warn(
"Invalid command!");
176 }
catch (
const std::exception& e) {
177 spdlog::error(
"Exception in ConsoleTask: {}", e.what());
178 g_running.store(
false);
182 spdlog::info(
"Console thread exiting.");
186int main(
int argc,
char* argv[])
188 std::string follower_sn, leader_sn, teleop_role, tcp_role, public_server_ip, nic_name;
189 unsigned int server_port = 0;
190 std::vector<std::string> wan_interface_whitelist {};
191 bool enable_digital_input =
false;
194 while ((opt = getopt_long_only(argc, argv,
"l:f:r:t:i:p:W:D", kLongOptions,
nullptr)) != -1) {
197 follower_sn = std::string(optarg);
200 leader_sn = std::string(optarg);
203 teleop_role = std::string(optarg);
206 tcp_role = std::string(optarg);
209 public_server_ip = std::string(optarg);
213 server_port = std::stoi(optarg);
215 spdlog::error(
"Invalid port number: {}", optarg);
220 nic_name = std::string(optarg);
221 wan_interface_whitelist.push_back(nic_name);
224 enable_digital_input =
true;
231 if (follower_sn.empty() || leader_sn.empty() || tcp_role.empty() || teleop_role.empty()
232 || public_server_ip.empty() || server_port == 0) {
236 if (wan_interface_whitelist.empty()) {
238 "WAN interface whitelist is not provided, will search all network interfaces.");
243 if (tcp_role ==
"server") {
244 is_tcp_server =
true;
245 }
else if (tcp_role ==
"client") {
246 is_tcp_server =
false;
248 spdlog::error(
"Valid inputs for [-t] are: server, client");
253 if (teleop_role ==
"follower") {
255 }
else if (teleop_role ==
"leader") {
258 spdlog::error(
"Valid inputs for [-r] are: follower, leader");
269 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
270 robot_sn_pairs.push_back({leader_sn, follower_sn});
283 tctw.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
286 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
289 std::optional<std::thread> pedal_thread;
291 if (teleop_role ==
"leader" && enable_digital_input) {
293 "Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
295 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw));
298 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
303 console_thread.join();
309 if (pedal_thread && pedal_thread->joinable()) {
310 pedal_thread->join();
316 }
catch (
const std::exception& e) {
317 spdlog::error(e.what());
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