16#include <spdlog/spdlog.h>
27std::vector<double> kPreferredJntPos = {60 * M_PI / 180.0, -60 * M_PI / 180.0, -85 * M_PI / 180.0,
28 115 * M_PI / 180.0, 70 * M_PI / 180.0, 0 * M_PI / 180.0, 0 * M_PI / 180.0};
31std::vector<double> kHomeJntPos = {0 * M_PI / 180.0, -40 * M_PI / 180.0, 0 * M_PI / 180.0,
32 90 * M_PI / 180.0, 0 * M_PI / 180.0, 40 * M_PI / 180.0, 0 * M_PI / 180.0};
35const std::array<double, flexiv::tdk::kCartDoF> kDefaultMaxContactWrench
36 = {5.0, 5.0, 5.0, 40.0, 40.0, 40.0};
39std::atomic<bool> g_running {
true};
42constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
49 std::cout<<
"Invalid program arguments!"<<std::endl;
50 std::cout<<
" -l [necessary] serial number of leader robot."<<std::endl;
51 std::cout<<
" -f [necessary] serial number of follower robot."<<std::endl;
52 std::cout<<
" -D [optional] Enable Digital Input reading task." << std::endl;
53 std::cout<<
"Usage: sudo ./transparent_cartesian_teleop_lan [-l leader_robot_sn] [-f follower_robot_sn] [-D]"<<std::endl;
57const struct option kLongOptions[] = {
59 {
"leader SN", required_argument, 0,
'l'},
60 {
"follower SN", required_argument, 0,
'f'},
61 {
"enable digital input", no_argument, 0,
'D'},
71 while (g_running.load() && !teleop.
any_fault()) {
74 }
catch (
const std::exception& e) {
75 spdlog::error(
"Exception in ReadDigitalInputTask: {}", e.what());
77 std::this_thread::sleep_for(std::chrono::milliseconds(10));
79 spdlog::info(
"ReadDigitalInputTask exiting.");
87 auto PrintCommandMenu = []() {
90 x/y/z : Toggle translation lock in WORLD coord (X/Y/Z)
91 q/w/e : Toggle orientation lock in WORLD coord (Rx/Ry/Rz)
92 X/Y/Z : Toggle translation lock in TCP coord (X/Y/Z)
93 Q/W/E : Toggle orientation lock in TCP coord (Rx/Ry/Rz)
95 --- Teleop Engagement ---
99 --- Wrench Feedback Scaling ---
100 t : Set wrench feedback scaling to 0.5
101 T : Set wrench feedback scaling to 2.0
102 c : Reset wrench feedback scaling to 1.0 default value
104 --- Axis Lock Presets ---
105 u : Unlock all axes (TCP coord)
106 U : Lock all axes (TCP coord)
108 --- Null Space Posture ---
109 i/I : Set local nullspace to Preferred/Home posture
110 o/O : Set remote nullspace to Preferred/Home posture
112 --- Max Contact Wrench ---
113 p : Set default remote max contact wrench
115 --- Repulsive Force ---
116 a : Set repulsive force to {5, 0, 0}
117 A : Set repulsive force to {-5, 0, 0}
125 --- Is teleop stopped or not ---
126 s : Query if teleop stopped or not
129 Any other key to show this help menu
133 unsigned int index = 0;
137 while (g_running.load() && !teleop.
any_fault()) {
139 std::string userInput {};
142 std::getline(std::cin, userInput);
144 if (userInput.empty()) {
145 spdlog::warn(
"Empty command!");
151 switch (userInput[0]) {
215 teleop.
Engage(index, kJointGroup,
true);
218 teleop.
Engage(index, kJointGroup,
false);
276 teleop.
stopped(0) ? spdlog::info(
"Teleop pair 0 stopped")
277 : spdlog::info(
"Teleop pair 0 started");
280 spdlog::warn(
"Invalid command!");
284 }
catch (
const std::exception& e) {
285 spdlog::error(
"Exception in ConsoleTask: {}", e.what());
286 g_running.store(
false);
290 spdlog::info(
"Console thread exiting.");
294int main(
int argc,
char* argv[])
296 std::string follower_sn {};
297 std::string leader_sn {};
300 bool enable_digital_input =
false;
302 while ((opt = getopt_long_only(argc, argv,
"f:l:D", kLongOptions, &longIndex)) != -1) {
305 follower_sn = std::string(optarg);
308 leader_sn = std::string(optarg);
311 enable_digital_input =
true;
318 if (leader_sn.empty() || follower_sn.empty()) {
334 std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
337 std::optional<std::thread> pedal_thread;
339 if (enable_digital_input) {
340 spdlog::info(
"Starting ReadDigitalInputTask thread.");
341 pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
343 spdlog::info(
"ReadDigitalInputTask thread NOT started (-D flag not provided).");
347 console_thread.join();
350 g_running.store(
false);
353 if (pedal_thread && pedal_thread->joinable()) {
354 pedal_thread->join();
360 }
catch (
const std::exception& e) {
361 spdlog::error(e.what());
Teleoperation control interface to run Cartesian-space teleoperation for one or more pairs of robots ...
bool any_fault(void) const
[Non-blocking] Whether any of the connected robots is in fault state.
void GetAxisLockState(unsigned int idx, JointGroup group, AxisLock &data)
[Non-blocking] Get the leader robot axis locking status
void Stop()
[Blocking] Stop the teleoperation control loop and make all robots hold their pose.
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) ...
void Engage(unsigned int idx, JointGroup group, bool engaged)
[Non-blocking] Engage/disengage the leader and follower robot. TransparentCartesianTeleopLAN supports...
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 SetRepulsiveForce(unsigned int idx, JointGroup group, 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.
void SetAxisLockCmd(unsigned int idx, JointGroup group, const AxisLock &cmd)
[Non-blocking] Set the leader robot axis locking command.
void Start()
[Non-Blocking] Start the teleoperation control loop for all robot pairs.
bool stopped(unsigned int idx) const
[Non-blocking] Whether teleop process has stopped. After teleop is started, the teleop process may st...
void SetWrenchFeedbackScalingFactor(unsigned int idx, JointGroup group, double factor=1.0)
[Non-blocking] Set the wrench feedback scaling factor.
void SetFollowerMaxContactWrench(unsigned int idx, JointGroup group, const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the follower robot of specified robot pair....
void SetFollowerNullSpacePosture(unsigned int idx, JointGroup group, 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 SetLeaderNullSpacePosture(unsigned int idx, JointGroup group, 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.
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.