32std::atomic<bool> g_stop {
false};
35const std::array<double, flexiv::tdk::kCartDoF> kExampleMaxContactWrench
36 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
43void LogWarn(
const std::string& msg)
45 std::cerr <<
"[warn] " << msg << std::endl;
48void LogError(
const std::string& msg)
50 std::cerr <<
"[error] " << msg << std::endl;
53void LogInfo(
const std::string& msg)
55 std::cout <<
"[info] " << msg << std::endl;
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;
70const struct option kLongOptions[] = {
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'},
81int main(
int argc,
char* argv[])
83 std::vector<std::string> robot_sn_vec;
84 std::vector<std::string> network_interface_whitelist;
85 int hold_seconds = 30;
89 while ((opt = getopt_long(argc, argv,
"r:A:t:n", kLongOptions,
nullptr)) != -1) {
92 robot_sn_vec.emplace_back(optarg);
95 network_interface_whitelist.emplace_back(optarg);
99 hold_seconds = std::stoi(optarg);
100 }
catch (
const std::exception&) {
113 if (robot_sn_vec.empty() || hold_seconds <= 0) {
117 if (network_interface_whitelist.empty()) {
118 LogWarn(
"network_interface_whitelist is not provided, will search all network interfaces.");
121 std::signal(SIGINT, OnSignal);
122 std::signal(SIGTERM, OnSignal);
128 zero_ft ? flexiv::tdk::ZeroFTSensor::Enable : flexiv::tdk::ZeroFTSensor::Disable);
132 for (
unsigned int i = 0; i < robot_sn_vec.size(); ++i) {
133 teleop.SetRobotMaxContactWrench(i, kExampleMaxContactWrench);
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);
148 hold_poses.push_back(pose);
149 LogInfo(
"Holding robot [" + robot_sn_vec[i] +
"] at its current TCP pose");
153 LogInfo(
"Control loop started. Updating the same command objects from this thread.");
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);
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.");
169 next_status = std::chrono::steady_clock::now() + std::chrono::seconds(1);
171 std::this_thread::sleep_for(std::chrono::milliseconds(10));
175 LogInfo(
"Stopping teleoperation.");
180 }
catch (
const std::exception& stop_error) {
181 LogError(stop_error.what());
185 }
catch (
const std::exception& e) {
190 LogInfo(
"Teleoperation exiting.");
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.