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 <spdlog/spdlog.h>
#include <getopt.h>
#include <atomic>
#include <chrono>
#include <iostream>
#include <thread>
#include <optional>
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
= {5.0, 5.0, 5.0, 40.0, 40.0, 40.0};
std::atomic<bool> g_running {true};
constexpr auto kJointGroup = flexiv::rdk::JointGroup::ARM_1;
}
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<<" -D [optional] Enable Digital Input reading task." << std::endl;
std::cout<<"Usage: sudo ./transparent_cartesian_teleop_lan [-l leader_robot_sn] [-f follower_robot_sn] [-D]"<<std::endl;
}
const struct option kLongOptions[] = {
{"leader SN", required_argument, 0, 'l'},
{"follower SN", required_argument, 0, 'f'},
{"enable digital input", no_argument, 0, 'D'},
{0, 0, 0, 0 }
};
{
while (g_running.load() && !teleop.
any_fault()) {
try {
} catch (const std::exception& e) {
spdlog::error("Exception in ReadDigitalInputTask: {}", e.what());
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
spdlog::info("ReadDigitalInputTask 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 force
--- Start/Stop ---
b : Stop teleop
B : Start teleop
--- Is teleop stopped or not ---
s : Query if teleop stopped or not
--- Help ---
Any other key to show this help menu
)" << std::endl;
};
unsigned int index = 0;
while (g_running.load() && !teleop.
any_fault()) {
std::string userInput {};
std::getline(std::cin, userInput);
if (userInput.empty()) {
spdlog::warn("Empty command!");
PrintCommandMenu();
continue;
}
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':
teleop.
Engage(index, kJointGroup,
true);
break;
case 'R':
teleop.
Engage(index, kJointGroup,
false);
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':
teleop.
stopped(0) ? spdlog::info(
"Teleop pair 0 stopped")
: spdlog::info("Teleop pair 0 started");
break;
default:
spdlog::warn("Invalid command!");
PrintCommandMenu();
break;
}
} catch (const std::exception& e) {
spdlog::error("Exception in ConsoleTask: {}", e.what());
g_running.store(false);
return;
}
}
spdlog::info("Console thread exiting.");
return;
}
int main(int argc, char* argv[])
{
std::string follower_sn {};
std::string leader_sn {};
int opt = 0;
int longIndex = 0;
bool enable_digital_input = false;
while ((opt = getopt_long_only(argc, argv, "f:l:D", kLongOptions, &longIndex)) != -1) {
switch (opt) {
case 'f':
follower_sn = std::string(optarg);
break;
case 'l':
leader_sn = std::string(optarg);
break;
case 'D':
enable_digital_input = true;
break;
default:
PrintHelp();
return 1;
}
}
if (leader_sn.empty() || follower_sn.empty()) {
PrintHelp();
return 1;
}
try {
tctl.Start();
std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
std::optional<std::thread> pedal_thread;
if (enable_digital_input) {
spdlog::info("Starting ReadDigitalInputTask thread.");
pedal_thread.emplace(ReadDigitalInputTask, std::ref(tctl));
} else {
spdlog::info("ReadDigitalInputTask thread NOT started (-D flag not provided).");
}
console_thread.join();
g_running.store(false);
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
tctl.Stop();
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
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.