30std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
31 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
34std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
35 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
38const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
39 = {25.0, 25.0, 25.0, 10.0, 10.0, 10.0};
42std::atomic<bool> g_running {
true};
44std::string BoolStr(
bool v)
46 return v ?
"true" :
"false";
49std::string FmtFixed(
double v,
int prec)
51 std::ostringstream oss;
52 oss << std::fixed << std::setprecision(prec) << v;
56std::string VecStr(
const std::vector<double>& vec,
int prec)
58 std::string out =
"[";
59 for (
size_t i = 0; i < vec.size(); ++i) {
60 out += (i == 0 ?
"" :
", ") + FmtFixed(vec[i], prec);
65void LogInfo(
const std::string& msg)
67 std::cout <<
"[info] " << msg << std::endl;
70void LogWarn(
const std::string& msg)
72 std::cerr <<
"[warn] " << msg << std::endl;
75void LogError(
const std::string& msg)
77 std::cerr <<
"[error] " << msg << std::endl;
85 "Teleop pair " + std::to_string(idx) +
" status: initialized=" + BoolStr(status.
initialized)
86 +
" started=" + BoolStr(status.
started) +
" engaged=" + BoolStr(status.
engaged)
87 +
" stopped=" + BoolStr(status.
stopped) +
" fault=" + BoolStr(status.
fault)
91 LogInfo(
"No teleop restriction. Safe to continue.");
94 const auto& issue = status.
primary;
95 LogWarn(std::string(
"[") + flexiv::tdk::TeleopIssueLevelStr[
static_cast<size_t>(issue.level)]
96 +
"/" + flexiv::tdk::TeleopIssueSideStr[
static_cast<size_t>(issue.side)] +
"] "
97 + issue.title +
" | " + issue.description +
" | " + issue.suggestion);
98 for (
const auto& extra : status.
issues) {
99 if (extra.code == issue.code && extra.side == issue.side
100 && extra.joint_index == issue.joint_index) {
103 LogWarn(std::string(
" also: [")
104 + flexiv::tdk::TeleopIssueCodeStr[
static_cast<size_t>(extra.code)] +
"/"
105 + flexiv::tdk::TeleopIssueSideStr[
static_cast<size_t>(extra.side)] +
"] "
113 std::cout<<
"Invalid program arguments!"<<std::endl;
114 std::cout<<
" -l [necessary] serial number of leader robot."<<std::endl;
115 std::cout<<
" -f [necessary] serial number of follower robot."<<std::endl;
116 std::cout<<
" -A [optional] LAN IPv4 address of the NIC connected to the robot." << std::endl;
117 std::cout<<
" -D [optional] Enable Digital Input reading task." << std::endl;
118 std::cout<<
"Usage: sudo ./transparent_cartesian_teleop_lan -l <leader_sn> -f <follower_sn> [-A <lan_ipv4>] [-D]"<<std::endl;
122const struct option kLongOptions[] = {
124 {
"leader SN", required_argument, 0,
'l'},
125 {
"follower SN", required_argument, 0,
'f'},
126 {
"lan whitelist ip", required_argument, 0,
'A'},
127 {
"enable digital input", no_argument, 0,
'D'},
137 bool logged_idle =
false;
138 std::string last_error;
139 while (g_running.load()) {
147 "ReadDigitalInputTask: teleop is not started, pause Engage "
148 "(call Init + Start to resume)");
156 }
catch (
const std::exception& e) {
157 if (last_error != e.what()) {
158 LogError(std::string(
"Exception in ReadDigitalInputTask: ") + e.what());
159 last_error = e.what();
162 std::this_thread::sleep_for(std::chrono::milliseconds(10));
164 LogInfo(
"ReadDigitalInputTask exiting.");
173 bool last_fault =
false;
174 std::string last_error;
175 while (g_running.load()) {
178 const bool fault_changed = (fault != last_fault);
180 if (fault_changed && fault) {
181 LogError(
"Fault detected. Press 'h' for details, 'F' to clear fault, then 'B' to "
184 }
else if (fault_changed) {
185 LogInfo(
"Fault cleared. Press 'B' to restart teleop.");
188 }
catch (
const std::exception& e) {
189 if (last_error != e.what()) {
190 LogError(std::string(
"Exception in FaultMonitorTask: ") + e.what());
191 last_error = e.what();
194 std::this_thread::sleep_for(std::chrono::milliseconds(100));
196 LogInfo(
"FaultMonitorTask exiting.");
205 auto PrintCommandMenu = []() {
208 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
209 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
210 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
211 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
213 --- Teleop Engagement ---
217 --- Wrench Feedback Scaling ---
218 t : Set wrench feedback scaling to 0.5
219 T : Set wrench feedback scaling to 2.0
220 c : Reset wrench feedback scaling to 1.0 default value
222 --- Axis Lock Presets ---
223 u : Unlock all axes (TCP coord)
224 U : Lock all axes (TCP coord)
226 --- Null Space Posture ---
227 i/I : Set local nullspace to Preferred/Home posture
228 o/O : Set remote nullspace to Preferred/Home posture
230 --- Max Contact Wrench ---
231 p : Set default remote max contact wrench
233 --- Repulsive Force ---
234 a : Set repulsive force to {5, 0, 0}
235 A : Set repulsive force to {-5, 0, 0}
236 C : Clear repulsive force
242 --- Is teleop stopped or not ---
243 s : Query if teleop stopped or not
245 --- Teleop status ---
246 h : Print why teleop is restricted / paused and what to do next
248 --- Fault recovery ---
249 F : Clear fault (blocking, up to 30 s), then press 'B' to restart teleop
251 --- Home all robots ---
252 H : Stop teleop and home all robots. Confirm the workspace is clear first.
254 --- Print action/states via instance ---
255 P : Print follower action and state
258 quit : Stop teleop and exit the program
261 Any other key to show this help menu
265 unsigned int index = 0;
269 while (g_running.load()) {
271 std::string userInput {};
274 if (!std::getline(std::cin, userInput)) {
275 LogInfo(
"Console input closed.");
279 if (userInput.empty()) {
280 LogWarn(
"Empty command!");
285 if (userInput ==
"quit") {
286 LogInfo(
"Quit requested.");
291 switch (userInput[0]) {
355 teleop.
Engage(index,
true);
358 teleop.
Engage(index,
false);
416 LogInfo(teleop.
stopped(0) ?
"Teleop pair 0 stopped" :
"Teleop pair 0 started");
422 LogInfo(
"Clearing fault, this may take up to 30 seconds...");
424 for (
size_t i = 0; i < results.size(); ++i) {
425 LogInfo(
"Teleop pair " + std::to_string(i)
426 +
" clear fault: leader=" + BoolStr(results[i].first)
427 +
" follower=" + BoolStr(results[i].second));
434 LogInfo(
"Teleop stopped for homing");
437 const auto follower = teleop.
instances(index).second;
438 LogInfo(
"Follower q: " + VecStr(follower->states().q, 2));
439 LogInfo(
"Follower q_d: " + VecStr(follower->actions().q_d, 2));
443 LogWarn(
"Invalid command!");
447 }
catch (
const std::exception& e) {
448 LogError(std::string(
"Command '") + userInput[0] +
"' failed: " + e.what());
451 }
catch (
const std::exception& status_e) {
452 LogError(std::string(
"Failed to query teleop status: ") + status_e.what());
456 LogInfo(
"Console thread exiting.");
460int main(
int argc,
char* argv[])
462 std::string follower_sn {};
463 std::string leader_sn {};
464 std::string whitelist_ip {};
465 std::vector<std::string> network_interface_whitelist {};
468 bool enable_digital_input =
false;
470 while ((opt = getopt_long_only(argc, argv,
"f:l:A:D", kLongOptions, &longIndex)) != -1) {
473 follower_sn = std::string(optarg);
476 leader_sn = std::string(optarg);
479 whitelist_ip = std::string(optarg);
480 network_interface_whitelist.emplace_back(whitelist_ip);
483 enable_digital_input =
true;
490 if (leader_sn.empty() || follower_sn.empty()) {
494 if (network_interface_whitelist.empty()) {
495 LogWarn(
"network_interface_whitelist is not provided, will search all network interfaces.");
502 {{leader_sn, follower_sn}}, network_interface_whitelist);
511 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
512 std::thread fault_monitor_thread(FaultMonitorTask, std::ref(tctl));
515 std::optional<std::thread> pedal_thread;
517 if (enable_digital_input) {
518 LogInfo(
"Starting ReadDigitalInputTask thread.");
519 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
521 LogInfo(
"ReadDigitalInputTask thread NOT started (-D flag not provided).");
525 console_thread.join();
528 g_running.store(
false);
531 fault_monitor_thread.join();
532 if (pedal_thread && pedal_thread->joinable()) {
533 pedal_thread->join();
539 }
catch (
const std::exception& e) {
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