Flexiv TDK APIs 1.7
Loading...
Searching...
No Matches
device_teleop_lan.cpp
1
16
17#include <flexiv/tdk/data.hpp>
19
20#include <array>
21#include <atomic>
22#include <chrono>
23#include <csignal>
24#include <iostream>
25#include <string>
26#include <thread>
27#include <vector>
28
29#include <getopt.h>
30
31namespace {
32std::atomic<bool> g_stop {false};
33
35const std::array<double, flexiv::tdk::kCartDoF> kExampleMaxContactWrench
36 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
37
38void OnSignal(int)
39{
40 g_stop.store(true);
41}
42
43void LogWarn(const std::string& msg)
44{
45 std::cerr << "[warn] " << msg << std::endl;
46}
47
48void LogError(const std::string& msg)
49{
50 std::cerr << "[error] " << msg << std::endl;
51}
52
53void LogInfo(const std::string& msg)
54{
55 std::cout << "[info] " << msg << std::endl;
56}
57
58void PrintHelp()
59{
60 // clang-format off
61 std::cout << "Usage: sudo ./device_teleop_lan -r <serial_num> [-r <serial_num> ...] [-A <lan_ipv4>] [-t <seconds>] [-n]" << std::endl;
62 std::cout << " -r --robot-sn Serial number of a robot to hold. Repeat for each additional robot." << std::endl;
63 std::cout << " -A --lan-ip [optional] LAN IPv4 address of the NIC connected to the robots. Repeatable." << std::endl;
64 std::cout << " -t --seconds [optional] How long to hold the pose. Default: 30." << std::endl;
65 std::cout << " -n --no-zero-ft [optional] Skip force/torque sensor zeroing during Init()." << std::endl;
66 std::cout << "The robots do not move. Ctrl+C stops the control loop and holds the pose." << std::endl;
67 // clang-format on
68}
69
70const struct option kLongOptions[] = {
71 // clang-format off
72 {"robot-sn", required_argument, 0, 'r'},
73 {"lan-ip", required_argument, 0, 'A'},
74 {"seconds", required_argument, 0, 't'},
75 {"no-zero-ft", no_argument, 0, 'n'},
76 {0, 0, 0, 0}
77 // clang-format on
78};
79}
80
81int main(int argc, char* argv[])
82{
83 std::vector<std::string> robot_sn_vec;
84 std::vector<std::string> network_interface_whitelist;
85 int hold_seconds = 30;
86 bool zero_ft = true;
87
88 int opt = 0;
89 while ((opt = getopt_long(argc, argv, "r:A:t:n", kLongOptions, nullptr)) != -1) {
90 switch (opt) {
91 case 'r':
92 robot_sn_vec.emplace_back(optarg);
93 break;
94 case 'A':
95 network_interface_whitelist.emplace_back(optarg);
96 break;
97 case 't':
98 try {
99 hold_seconds = std::stoi(optarg);
100 } catch (const std::exception&) {
101 PrintHelp();
102 return 1;
103 }
104 break;
105 case 'n':
106 zero_ft = false;
107 break;
108 default:
109 PrintHelp();
110 return 1;
111 }
112 }
113 if (robot_sn_vec.empty() || hold_seconds <= 0) {
114 PrintHelp();
115 return 1;
116 }
117 if (network_interface_whitelist.empty()) {
118 LogWarn("network_interface_whitelist is not provided, will search all network interfaces.");
119 }
120
121 std::signal(SIGINT, OnSignal);
122 std::signal(SIGTERM, OnSignal);
123
124 try {
125 flexiv::tdk::DeviceTeleopLan teleop(robot_sn_vec, network_interface_whitelist);
126 try {
127 teleop.Init(
128 zero_ft ? flexiv::tdk::ZeroFTSensor::Enable : flexiv::tdk::ZeroFTSensor::Disable);
129
130 // Init() has already switched the control mode, so the contact limit can be set
131 // before Start(). A real device should choose this from its own safety requirements.
132 for (unsigned int i = 0; i < robot_sn_vec.size(); ++i) {
133 teleop.SetRobotMaxContactWrench(i, kExampleMaxContactWrench);
134 }
135
136 // One command object per robot, in the same order as robot_sn_vec. These objects must
137 // stay alive for the whole session; the library reads them from its realtime threads.
138 std::vector<std::shared_ptr<flexiv::tdk::MotionControlCmds>> cmds;
139 std::vector<std::array<double, flexiv::tdk::kPoseSize>> hold_poses;
140 const std::array<double, flexiv::tdk::kCartDoF> zero_twist {};
141 cmds.reserve(robot_sn_vec.size());
142 hold_poses.reserve(robot_sn_vec.size());
143 for (unsigned int i = 0; i < robot_sn_vec.size(); ++i) {
144 auto pose = teleop.instance(i)->states().tcp_pose;
145 auto cmd = std::make_shared<flexiv::tdk::MotionControlCmds>();
146 cmd->write(pose, zero_twist, zero_twist);
147 cmds.push_back(cmd);
148 hold_poses.push_back(pose);
149 LogInfo("Holding robot [" + robot_sn_vec[i] + "] at its current TCP pose");
150 }
151
152 teleop.Start(cmds);
153 LogInfo("Control loop started. Updating the same command objects from this thread.");
154
155 const auto deadline
156 = std::chrono::steady_clock::now() + std::chrono::seconds(hold_seconds);
157 auto next_status = std::chrono::steady_clock::now();
158 while (!g_stop.load() && std::chrono::steady_clock::now() < deadline) {
159 for (unsigned int i = 0; i < cmds.size(); ++i) {
160 cmds[i]->write(hold_poses[i], zero_twist, zero_twist);
161 }
162 if (std::chrono::steady_clock::now() >= next_status) {
163 for (unsigned int i = 0; i < robot_sn_vec.size(); ++i) {
164 if (teleop.fault(i)) {
165 LogWarn("Robot [" + robot_sn_vec[i]
166 + "] is in fault. Other robots keep holding.");
167 }
168 }
169 next_status = std::chrono::steady_clock::now() + std::chrono::seconds(1);
170 }
171 std::this_thread::sleep_for(std::chrono::milliseconds(10));
172 }
173
174 // Stop once, then let teleop leave scope. A second Stop() from the destructor is safe.
175 LogInfo("Stopping teleoperation.");
176 teleop.Stop();
177 } catch (...) {
178 try {
179 teleop.Stop();
180 } catch (const std::exception& stop_error) {
181 LogError(stop_error.what());
182 }
183 throw;
184 }
185 } catch (const std::exception& e) {
186 LogError(e.what());
187 return 1;
188 }
189
190 LogInfo("Teleoperation exiting.");
191 return 0;
192}
Teleoperation control interface to use device to teleop one or more robots connected to the same LAN.
Header file containing various constant expressions, data structs, and enums.