Flexiv TDK APIs 1.7
Loading...
Searching...
No Matches
joint_teleop_under_lan.cpp
1
7
9
10#include <getopt.h>
11#include <chrono>
12#include <iostream>
13#include <string>
14#include <thread>
15#include <vector>
16
17void LogWarn(const std::string& msg)
18{
19 std::cerr << "[warn] " << msg << std::endl;
20}
21
22void LogError(const std::string& msg)
23{
24 std::cerr << "[error] " << msg << std::endl;
25}
26
27namespace {
28const std::vector<double> kJointStiffnessRatio = {0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02};
29constexpr double kLastJointShapedInertia = 0.05;
30}
31
32void PrintHelp()
33{
34 // clang-format off
35 std::cout << "Usage: sudo ./joint_teleop_under_lan -1 <serial_num> -2 <serial_num> [-A <lan_ipv4>]" << std::endl;
36 std::cout << " -1 --first-sn Serial number of the first robot." << std::endl;
37 std::cout << " -2 --second-sn Serial number of the second robot." << std::endl;
38 std::cout << " -A --lan-ip [optional] LAN IPv4 address of the NIC connected to the robot. Repeatable." << std::endl;
39 // clang-format on
40}
41
42const struct option kLongOptions[] = {
43 // clang-format off
44 {"first-sn", required_argument, 0, '1'},
45 {"second-sn", required_argument, 0, '2'},
46 {"lan-ip", required_argument, 0, 'A'},
47 {0, 0, 0, 0}
48 // clang-format on
49};
50
51int main(int argc, char* argv[])
52{
53 // Parse program arguments
54 std::string first_sn, second_sn;
55 std::vector<std::string> network_interface_whitelist {};
56 int opt = 0;
57 while ((opt = getopt_long(argc, argv, "1:2:A:", kLongOptions, nullptr)) != -1) {
58 switch (opt) {
59 case '1':
60 first_sn = std::string(optarg);
61 break;
62 case '2':
63 second_sn = std::string(optarg);
64 break;
65 case 'A':
66 network_interface_whitelist.emplace_back(optarg);
67 break;
68 default:
69 PrintHelp();
70 return 1;
71 }
72 }
73 if (first_sn.empty() || second_sn.empty()) {
74 PrintHelp();
75 return 1;
76 }
77 if (network_interface_whitelist.empty()) {
78 LogWarn("network_interface_whitelist is not provided, will search all network interfaces.");
79 }
80
81 try {
82 // Create teleop control interface
83 flexiv::tdk::JointTeleopLAN joint_teleop({{first_sn, second_sn}}, network_interface_whitelist);
84
85 // Only control 1 pair of robots
86 unsigned int robot_pair_idx = 0;
87
88 // Run initialization sequence
89 joint_teleop.Init();
90
91 // Set 20 degrees soft limit
92 joint_teleop.SetSoftLimit(robot_pair_idx, 20.0);
93
94 // Sync pose, first robot stays still, second robot moves to its pose
95 joint_teleop.SyncPose(robot_pair_idx, {});
96
97 // Enable inertia shaping for the last joint
98 std::vector<std::pair<bool, double>> shaped_joint_inertia;
99 shaped_joint_inertia.resize(joint_teleop.DoF(robot_pair_idx), {false, 1.0});
100 shaped_joint_inertia.back() = {true, kLastJointShapedInertia};
101 joint_teleop.SetInertiaShaping(robot_pair_idx, shaped_joint_inertia);
102
103 // Start control loop
104 joint_teleop.Start();
105
106 // Set impedance properties
107 joint_teleop.SetJointImpedance(robot_pair_idx, kJointStiffnessRatio);
108
109 // Block until faulted
110 bool last_pedal_input = false;
111 while (!joint_teleop.any_fault()) {
112 // Activate by pedal
113 bool pedal_input = joint_teleop.digital_inputs(robot_pair_idx).first[0];
114 if (pedal_input != last_pedal_input) {
115 joint_teleop.Activate(robot_pair_idx, pedal_input);
116 last_pedal_input = pedal_input;
117 }
118 std::this_thread::sleep_for(std::chrono::milliseconds(100));
119 }
120
121 // Fault occurred, stop the teleoperation
122 joint_teleop.Stop();
123
124 } catch (const std::exception& e) {
125 LogError(e.what());
126 return 1;
127 }
128
129 return 0;
130}
Teleoperation control interface to run joint-space robot-robot teleoperation for one or more pairs of...
void Init()
[Blocking] Get all robots ready for teleoperation. The following actions will happen in sequence: a) ...