Example usage of Transparent Cartesian teleoperation under Local Area Network for controlling a follower robot using a leader robot with transparent force feedback. Supports both keyboard and digital input engage/disengage signal reading, with various axes lock modes, force scaling, and max contact wrench setting, etc.
Example usage of Transparent Cartesian teleoperation under Local Area Network for controlling a follower robot using a leader robot with transparent force feedback. Supports both keyboard and digital input engage/disengage signal reading, with various axes lock modes, force scaling, and max contact wrench setting, etc.
#include <getopt.h>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <optional>
#include <sstream>
#include <string>
#include <thread>
#include <vector>
#include <functional>
namespace {
std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
= {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
std::atomic<bool> g_running {true};
std::string BoolStr(bool v)
{
return v ? "true" : "false";
}
std::string FmtFixed(double v, int prec)
{
std::ostringstream oss;
oss << std::fixed << std::setprecision(prec) << v;
return oss.str();
}
std::string VecStr(const std::vector<double>& vec, int prec)
{
std::string out = "[";
for (size_t i = 0; i < vec.size(); ++i) {
out += (i == 0 ? "" : ", ") + FmtFixed(vec[i], prec);
}
return out + "]";
}
void LogInfo(const std::string& msg)
{
std::cout << "[info] " << msg << std::endl;
}
void LogWarn(const std::string& msg)
{
std::cerr << "[warn] " << msg << std::endl;
}
void LogError(const std::string& msg)
{
std::cerr << "[error] " << msg << std::endl;
}
}
{
LogInfo(
"Teleop pair " + std::to_string(idx) +
" status: initialized=" + BoolStr(status.
initialized)
+
" started=" + BoolStr(status.
started) +
" engaged=" + BoolStr(status.
engaged)
+
" stopped=" + BoolStr(status.
stopped) +
" fault=" + BoolStr(status.
fault)
LogInfo("No teleop restriction. Safe to continue.");
return;
}
const auto& issue = status.
primary;
LogWarn(std::string("[") + flexiv::tdk::TeleopIssueLevelStr[static_cast<size_t>(issue.level)]
+ "/" + flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(issue.side)] + "] "
+ issue.title + " | " + issue.description + " | " + issue.suggestion);
for (
const auto& extra : status.
issues) {
if (extra.code == issue.code && extra.side == issue.side
&& extra.joint_index == issue.joint_index) {
continue;
}
LogWarn(std::string(" also: [")
+ flexiv::tdk::TeleopIssueCodeStr[static_cast<size_t>(extra.code)] + "/"
+ flexiv::tdk::TeleopIssueSideStr[static_cast<size_t>(extra.side)] + "] "
+ extra.title);
}
}
void PrintHelp()
{
std::cout<<"Invalid program arguments!"<<std::endl;
std::cout<<" -l [necessary] serial number of leader robot."<<std::endl;
std::cout<<" -f [necessary] serial number of follower robot."<<std::endl;
std::cout<<" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
std::cout<<" -D [optional] Enable Digital Input reading task." << std::endl;
std::cout<<"Usage: sudo ./transparent_cartesian_teleop_lan -l <leader_sn> -f <follower_sn> [-A <lan_ipv4>] [-D]"<<std::endl;
}
const struct option kLongOptions[] = {
{"leader SN", required_argument, 0, 'l'},
{"follower SN", required_argument, 0, 'f'},
{"lan whitelist ip", required_argument, 0, 'A'},
{"enable digital input", no_argument, 0, 'D'},
{0, 0, 0, 0 }
};
{
bool logged_idle = false;
std::string last_error;
while (g_running.load()) {
try {
if (!logged_idle) {
LogWarn(
"ReadDigitalInputTask: teleop is not started, pause Engage "
"(call Init + Start to resume)");
logged_idle = true;
}
} else {
logged_idle = false;
}
last_error.clear();
} catch (const std::exception& e) {
if (last_error != e.what()) {
LogError(std::string("Exception in ReadDigitalInputTask: ") + e.what());
last_error = e.what();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
LogInfo("ReadDigitalInputTask exiting.");
return;
}
{
bool last_fault = false;
std::string last_error;
while (g_running.load()) {
try {
const bool fault_changed = (fault != last_fault);
last_fault = fault;
if (fault_changed && fault) {
LogError("Fault detected. Press 'h' for details, 'F' to clear fault, then 'B' to "
"restart teleop.");
} else if (fault_changed) {
LogInfo("Fault cleared. Press 'B' to restart teleop.");
}
last_error.clear();
} catch (const std::exception& e) {
if (last_error != e.what()) {
LogError(std::string("Exception in FaultMonitorTask: ") + e.what());
last_error = e.what();
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
LogInfo("FaultMonitorTask exiting.");
return;
}
{
auto PrintCommandMenu = []() {
std::cout << R"(
--- Axis Lock ---
x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
--- Teleop Engagement ---
r : Engage teleop
R : Disengage teleop
--- Wrench Feedback Scaling ---
t : Set wrench feedback scaling to 0.5
T : Set wrench feedback scaling to 2.0
c : Reset wrench feedback scaling to 1.0 default value
--- Axis Lock Presets ---
u : Unlock all axes (TCP coord)
U : Lock all axes (TCP coord)
--- Null Space Posture ---
i/I : Set local nullspace to Preferred/Home posture
o/O : Set remote nullspace to Preferred/Home posture
--- Max Contact Wrench ---
p : Set default remote max contact wrench
--- Repulsive Force ---
a : Set repulsive force to {5, 0, 0}
A : Set repulsive force to {-5, 0, 0}
C : Clear repulsive force
--- Start/Stop ---
b : Stop teleop
B : Start teleop
--- Is teleop stopped or not ---
s : Query if teleop stopped or not
--- Teleop status ---
h : Print why teleop is restricted / paused and what to do next
--- Fault recovery ---
F : Clear fault (blocking, up to 30 s), then press 'B' to restart teleop
--- Home all robots ---
H : Stop teleop and home all robots. Confirm the workspace is clear first.
--- Print action/states via instance ---
P : Print follower action and state
--- Exit ---
quit : Stop teleop and exit the program
--- Help ---
Any other key to show this help menu
)" << std::endl;
};
unsigned int index = 0;
while (g_running.load()) {
std::string userInput {};
if (!std::getline(std::cin, userInput)) {
LogInfo("Console input closed.");
break;
}
if (userInput.empty()) {
LogWarn("Empty command!");
PrintCommandMenu();
continue;
}
if (userInput == "quit") {
LogInfo("Quit requested.");
break;
}
try {
switch (userInput[0]) {
case 'x':
break;
case 'y':
break;
case 'z':
break;
case 'q':
break;
case 'w':
break;
case 'e':
break;
case 'X':
break;
case 'Y':
break;
case 'Z':
break;
case 'Q':
break;
case 'W':
break;
case 'E':
break;
case 'r':
break;
case 'R':
break;
case 't':
break;
case 'T':
break;
case 'u':
break;
case 'U':
break;
case 'i':
break;
case 'I':
break;
case 'o':
break;
case 'O':
break;
case 'p':
break;
case 'a':
break;
case 'A':
break;
case 'b':
break;
case 'B':
break;
case 'c':
break;
case 'C':
break;
case 's':
LogInfo(teleop.
stopped(0) ?
"Teleop pair 0 stopped" :
"Teleop pair 0 started");
break;
case 'h':
break;
case 'F': {
LogInfo("Clearing fault, this may take up to 30 seconds...");
for (size_t i = 0; i < results.size(); ++i) {
LogInfo("Teleop pair " + std::to_string(i)
+ " clear fault: leader=" + BoolStr(results[i].first)
+ " follower=" + BoolStr(results[i].second));
}
break;
}
case 'H':
LogInfo("Teleop stopped for homing");
break;
case 'P': {
const auto follower = teleop.
instances(index).second;
LogInfo("Follower q: " + VecStr(follower->states().q, 2));
LogInfo("Follower q_d: " + VecStr(follower->actions().q_d, 2));
break;
}
default:
LogWarn("Invalid command!");
PrintCommandMenu();
break;
}
} catch (const std::exception& e) {
LogError(std::string("Command '") + userInput[0] + "' failed: " + e.what());
try {
} catch (const std::exception& status_e) {
LogError(std::string("Failed to query teleop status: ") + status_e.what());
}
}
}
LogInfo("Console thread exiting.");
return;
}
int main(int argc, char* argv[])
{
std::string follower_sn {};
std::string leader_sn {};
std::string whitelist_ip {};
std::vector<std::string> network_interface_whitelist {};
int opt = 0;
int longIndex = 0;
bool enable_digital_input = false;
while ((opt = getopt_long_only(argc, argv, "f:l:A:D", kLongOptions, &longIndex)) != -1) {
switch (opt) {
case 'f':
follower_sn = std::string(optarg);
break;
case 'l':
leader_sn = std::string(optarg);
break;
case 'A':
whitelist_ip = std::string(optarg);
network_interface_whitelist.emplace_back(whitelist_ip);
break;
case 'D':
enable_digital_input = true;
break;
default:
PrintHelp();
return 1;
}
}
if (leader_sn.empty() || follower_sn.empty()) {
PrintHelp();
return 1;
}
if (network_interface_whitelist.empty()) {
LogWarn("network_interface_whitelist is not provided, will search all network interfaces.");
}
try {
{{leader_sn, follower_sn}}, network_interface_whitelist);
tctl.Start();
std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctl));
std::optional<std::thread> pedal_thread;
if (enable_digital_input) {
LogInfo("Starting ReadDigitalInputTask thread.");
pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
} else {
LogInfo("ReadDigitalInputTask thread NOT started (-D flag not provided).");
}
console_thread.join();
g_running.store(false);
fault_monitor_thread.join();
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
tctl.Stop();
} catch (const std::exception& e) {
LogError(e.what());
return 1;
}
return 0;
}
Teleoperation control interface to run Cartesian-space teleoperation for one or more pairs of robots ...
void SetFollowerNullSpacePosture(unsigned int idx, 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 GetTeleopStatus(unsigned int idx, TeleopStatus &status) const
[Non-blocking] Query why teleoperation is restricted, paused, or stopped, and what the operator shoul...
std::pair< std::shared_ptr< Robot >, std::shared_ptr< Robot > > instances(unsigned int idx) const
[Non-blocking] Pointers to the underlying rdk::Robot instances of the robot pair.
bool any_fault(void) const
[Non-blocking] Whether any of the connected robots is in fault state.
std::vector< std::pair< bool, bool > > ClearFault(unsigned int timeout_sec=30)
[Blocking] Try to clear minor or critical fault of the robot without a power cycle.
void SetFollowerMaxContactWrench(unsigned int idx, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the follower robot of specified robot pair....
void SetWrenchFeedbackScalingFactor(unsigned int idx, double factor=1.0)
[Non-blocking] Set the wrench feedback scaling factor.
void Stop()
[Blocking] Stop the teleoperation control loop and make all robots hold their pose.
void SetAxisLockCmd(unsigned int idx, const AxisLock &cmd)
[Non-blocking] Set the leader robot axis locking command.
void GetAxisLockState(unsigned int idx, AxisLock &data)
[Non-blocking] Get the leader robot axis locking status
void Init(bool limit_wrist_singular=true, ZeroFTSensor zero_ft_sensor=ZeroFTSensor::Enable)
[Blocking] Get all robots ready for teleoperation. The following actions will happen in sequence: a) ...
const std::pair< std::array< bool, kIOPorts >, 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 Start()
[Non-Blocking] Start the teleoperation control loop for all robot pairs.
void HomeAll()
[Blocking] Move all connected robots to their home posture simultaneously.
void SetRepulsiveForce(unsigned int idx, const std::array< double, 3 > &repulsive_force, bool in_world=true)
[Non-blocking] Set the repulsive force in World or Tcp frame of the follower robot.
bool stopped(unsigned int idx) const
[Non-blocking] Whether teleop process has stopped. After teleop is started, the teleop process may st...
void Engage(unsigned int idx, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot. TransparentCartesianTeleopLAN supports...
void SetLeaderNullSpacePosture(unsigned int idx, const std::vector< double > &ref_joint_positions)
[Blocking] Set reference joint positions used in the robot's null-space posture control module for th...
Header file containing various constant expressions, data structs, and enums.
@ COORD_WORLD
WORLD coordinate of local robot.
@ COORD_TCP
TCP coordinate of local robot.
Data for locking axis, including reference frame and axis to be locked for transparent cartesian tele...
std::array< bool, 3 > lock_ori_axis
Orientation axis lock, the corresponding axis order is . True for locking, false for floating.
std::array< bool, 3 > lock_trans_axis
Translation axis lock, the corresponding axis order is . True for locking, false for floating.
CoordType coord
Reference coordinate that the axis to be locked.
Snapshot of teleoperation health for one robot pair.
std::vector< TeleopIssue > issues
double latency_threshold_ms