Flexiv TDK APIs 2.1
Loading...
Searching...
No Matches
transparent_cartesian_teleop_wan.cpp
1
12
13#include <flexiv/tdk/data.hpp>
15
16#include <spdlog/spdlog.h>
17
18#include <getopt.h>
19#include <atomic>
20#include <chrono>
21#include <iostream>
22#include <thread>
23#include <optional>
24
25namespace {
26
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};
30
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};
34
36const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
37 = {50.0, 50.0, 50.0, 40.0, 40.0, 40.0};
38
40std::atomic<bool> g_running {true};
41
44
46constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
47}
48
49void PrintHelp()
50{
51 // clang-format off
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;
62 // clang-format on
63}
64
65const struct option kLongOptions[] = {
66 // clang-format off
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'},
75 {0, 0, 0, 0 }
76 // clang-format on
77};
78
82void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
83{
84 while (g_running.load() && !teleop.fault(0)) {
85 try {
86 teleop.Engage(0, kJointGroup, teleop.digital_inputs(0)[0]);
87 } catch (const std::exception& e) {
88 spdlog::error("Exception in ReadDigitalInputTask: {}", e.what());
89 }
90 std::this_thread::sleep_for(std::chrono::milliseconds(10));
91 }
92 spdlog::info("ReadDigitalInputTask exiting.");
93 return;
94}
95
99void ConsoleTask(flexiv::tdk::TransparentCartesianTeleopWAN& teleop)
100{
101 auto PrintCommandMenu = []() {
102 std::cout << R"(
103 --- Teleop Engagement ---
104 r : Engage teleop
105 R : Disengage teleop
106
107 --- Null Space Posture ---
108 i/I : Set local nullspace to Preferred/Home posture
109
110 --- Max Contact Wrench ---
111 p : Set default max contact wrench
112
113 --- Reinit and start ---
114 u : Recall Init and Start
115 U : Stop teleop
116
117 --- Tcp message latency ---
118 l : print current message latency in milliseconds
119
120 --- Help ---
121 Any other key to show this help menu
122 )" << std::endl;
123 };
124
125 while (g_running.load() && !teleop.fault(0)) {
126
127 std::string user_input {};
128
129 std::getline(std::cin, user_input);
130
131 if (user_input.empty()) {
132 spdlog::warn("Empty command!");
133 PrintCommandMenu();
134 continue;
135 }
136
137 try {
138 switch (user_input[0]) {
139 case 'r':
140 teleop.Engage(0, kJointGroup, true);
141 break;
142 case 'R':
143 teleop.Engage(0, kJointGroup, false);
144 break;
145 case 'i':
146 teleop.SetNullSpacePosture(0, kJointGroup, kPreferredJntPos);
147 break;
148 case 'I':
149 teleop.SetNullSpacePosture(0, kJointGroup, kHomeJntPos);
150 break;
151 case 'p':
152 teleop.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
153 break;
154 case 'u':
155 teleop.Init();
156 teleop.Start();
157 break;
158 case 'U':
159 teleop.Stop();
160 break;
161 case 'l': {
162 double latency_ms {};
163 if (teleop.CheckTeleopConnectionLatency(0, latency_ms)) {
164 spdlog::info("Current message latency is: {}ms", latency_ms);
165 } else {
166 spdlog::warn("WAN teleop is disconnected.");
167 }
168 break;
169 }
170
171 default:
172 spdlog::warn("Invalid command!");
173 PrintCommandMenu();
174 break;
175 }
176 } catch (const std::exception& e) {
177 spdlog::error("Exception in ConsoleTask: {}", e.what());
178 g_running.store(false);
179 return;
180 }
181 }
182 spdlog::info("Console thread exiting.");
183 return;
184}
185
186int main(int argc, char* argv[])
187{
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;
192
193 int opt = 0;
194 while ((opt = getopt_long_only(argc, argv, "l:f:r:t:i:p:W:D", kLongOptions, nullptr)) != -1) {
195 switch (opt) {
196 case 'f':
197 follower_sn = std::string(optarg);
198 break;
199 case 'l':
200 leader_sn = std::string(optarg);
201 break;
202 case 'r':
203 teleop_role = std::string(optarg);
204 break;
205 case 't':
206 tcp_role = std::string(optarg);
207 break;
208 case 'i':
209 public_server_ip = std::string(optarg);
210 break;
211 case 'p':
212 try {
213 server_port = std::stoi(optarg);
214 } catch (...) {
215 spdlog::error("Invalid port number: {}", optarg);
216 return 1;
217 }
218 break;
219 case 'W':
220 nic_name = std::string(optarg);
221 wan_interface_whitelist.push_back(nic_name);
222 break;
223 case 'D':
224 enable_digital_input = true;
225 break;
226 default:
227 PrintHelp();
228 return 1;
229 }
230 }
231 if (follower_sn.empty() || leader_sn.empty() || tcp_role.empty() || teleop_role.empty()
232 || public_server_ip.empty() || server_port == 0) {
233 PrintHelp();
234 return 1;
235 }
236 if (wan_interface_whitelist.empty()) {
237 spdlog::warn(
238 "WAN interface whitelist is not provided, will search all network interfaces.");
239 }
240
241 // Whether this is a TCP server or client
242 bool is_tcp_server;
243 if (tcp_role == "server") {
244 is_tcp_server = true;
245 } else if (tcp_role == "client") {
246 is_tcp_server = false;
247 } else {
248 spdlog::error("Valid inputs for [-t] are: server, client");
249 return 1;
250 }
251
252 // Whether this is leader or follower
253 if (teleop_role == "follower") {
255 } else if (teleop_role == "leader") {
257 } else {
258 spdlog::error("Valid inputs for [-r] are: follower, leader");
259 return 1;
260 }
261
262 // Network configuration
263 flexiv::tdk::NetworkCfgStd network_cfg;
264 network_cfg.is_tcp_server = is_tcp_server;
265 network_cfg.public_ipv4_address = public_server_ip;
266 network_cfg.listening_port = server_port;
267 network_cfg.wan_interface_whitelist = wan_interface_whitelist;
268
269 std::vector<std::pair<std::string, std::string>> robot_sn_pairs {};
270 robot_sn_pairs.push_back({leader_sn, follower_sn});
271 try {
272
273 // Allocate tdk object
274 flexiv::tdk::TransparentCartesianTeleopWAN tctw(robot_sn_pairs, kRole, network_cfg);
275
276 // Init high transparency teleop
277 tctw.Init();
278
279 // Start high transparency teleop
280 tctw.Start();
281
282 // Set max contact wrench
283 tctw.SetMaxContactWrench(0, kJointGroup, kDefaultMaxContactWrench);
284
285 // Start console_thread
286 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctw)));
287
288 // Start pedal_thread based on the new flag or the role
289 std::optional<std::thread> pedal_thread;
290 // Modified condition: Start if -D is given OR if role is leader (original behavior)
291 if (teleop_role == "leader" && enable_digital_input) {
292 spdlog::info(
293 "Starting ReadDigitalInputTask thread as role is 'leader' and requested by -D "
294 "flag.");
295 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctw));
296 } else {
297 spdlog::info(
298 "ReadDigitalInputTask thread NOT started (role is not 'leader' or -D flag not "
299 "provided).");
300 }
301
302 // Wait for console_thread to finish
303 console_thread.join();
304
305 // Stop all threads, notify other threads exit
306 g_running = false;
307
308 // Wait for digital reading task exit
309 if (pedal_thread && pedal_thread->joinable()) {
310 pedal_thread->join();
311 }
312
313 // Exit high transparency teleop
314 tctw.Stop();
315
316 } catch (const std::exception& e) {
317 spdlog::error(e.what());
318 return 1;
319 }
320
321 return 0;
322}
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