17void LogWarn(
const std::string& msg)
19 std::cerr <<
"[warn] " << msg << std::endl;
22void LogError(
const std::string& msg)
24 std::cerr <<
"[error] " << msg << std::endl;
28const std::vector<double> kJointStiffnessRatio = {0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02};
29constexpr double kLastJointShapedInertia = 0.05;
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;
42const struct option kLongOptions[] = {
44 {
"first-sn", required_argument, 0,
'1'},
45 {
"second-sn", required_argument, 0,
'2'},
46 {
"lan-ip", required_argument, 0,
'A'},
51int main(
int argc,
char* argv[])
54 std::string first_sn, second_sn;
55 std::vector<std::string> network_interface_whitelist {};
57 while ((opt = getopt_long(argc, argv,
"1:2:A:", kLongOptions,
nullptr)) != -1) {
60 first_sn = std::string(optarg);
63 second_sn = std::string(optarg);
66 network_interface_whitelist.emplace_back(optarg);
73 if (first_sn.empty() || second_sn.empty()) {
77 if (network_interface_whitelist.empty()) {
78 LogWarn(
"network_interface_whitelist is not provided, will search all network interfaces.");
86 unsigned int robot_pair_idx = 0;
92 joint_teleop.SetSoftLimit(robot_pair_idx, 20.0);
95 joint_teleop.SyncPose(robot_pair_idx, {});
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);
104 joint_teleop.Start();
107 joint_teleop.SetJointImpedance(robot_pair_idx, kJointStiffnessRatio);
110 bool last_pedal_input =
false;
111 while (!joint_teleop.any_fault()) {
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;
118 std::this_thread::sleep_for(std::chrono::milliseconds(100));
124 }
catch (
const std::exception& e) {
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) ...