Flexiv TDK APIs 2.1
Loading...
Searching...
No Matches
transparent_cartesian_teleop_lan.cpp

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.

Note
This program is provided only as an example. Users must adapt it to their own application requirements, safety procedures, and software architecture before deployment.
Author
Flexiv
#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;
} // namespace
void PrintHelp()
{
// clang-format off
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;
// clang-format on
}
const struct option kLongOptions[] = {
// clang-format off
{"leader SN", required_argument, 0, 'l'},
{"follower SN", required_argument, 0, 'f'},
{"enable digital input", no_argument, 0, 'D'},
{0, 0, 0, 0 }
// clang-format on
};
void ReadDigitalInputTask(flexiv::tdk::TransparentCartesianTeleopLAN& teleop)
{
while (g_running.load() && !teleop.any_fault()) {
try {
teleop.Engage(0, kJointGroup, teleop.digital_inputs(0).first[0]);
} 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;
teleop.GetAxisLockState(index, kJointGroup, cmd);
while (g_running.load() && !teleop.any_fault()) {
std::string userInput {};
// Get user input
std::getline(std::cin, userInput);
if (userInput.empty()) {
spdlog::warn("Empty command!");
PrintCommandMenu();
continue;
}
try {
switch (userInput[0]) {
case 'x':
cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'y':
cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'z':
cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'q':
cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'w':
cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'e':
cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'X':
cmd.lock_trans_axis[0] = !cmd.lock_trans_axis[0];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'Y':
cmd.lock_trans_axis[1] = !cmd.lock_trans_axis[1];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'Z':
cmd.lock_trans_axis[2] = !cmd.lock_trans_axis[2];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'Q':
cmd.lock_ori_axis[0] = !cmd.lock_ori_axis[0];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'W':
cmd.lock_ori_axis[1] = !cmd.lock_ori_axis[1];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'E':
cmd.lock_ori_axis[2] = !cmd.lock_ori_axis[2];
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'r':
teleop.Engage(index, kJointGroup, true);
break;
case 'R':
teleop.Engage(index, kJointGroup, false);
break;
case 't':
teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 0.5);
break;
case 'T':
teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 2);
break;
case 'u':
cmd.lock_ori_axis = {false, false, false};
cmd.lock_trans_axis = {false, false, false};
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'U':
cmd.lock_ori_axis = {true, true, true};
cmd.lock_trans_axis = {true, true, true};
teleop.SetAxisLockCmd(index, kJointGroup, cmd);
break;
case 'i':
teleop.SetLeaderNullSpacePosture(index, kJointGroup, kPreferredJntPos);
break;
case 'I':
teleop.SetLeaderNullSpacePosture(index, kJointGroup, kHomeJntPos);
break;
case 'o':
teleop.SetFollowerNullSpacePosture(index, kJointGroup, kPreferredJntPos);
break;
case 'O':
teleop.SetFollowerNullSpacePosture(index, kJointGroup, kHomeJntPos);
break;
case 'p':
teleop.SetFollowerMaxContactWrench(index, kJointGroup, kDefaultMaxContactWrench);
break;
case 'a':
teleop.SetRepulsiveForce(index, kJointGroup, {5, 0, 0});
break;
case 'A':
teleop.SetRepulsiveForce(index, kJointGroup, {-5, 0, 0});
break;
case 'b':
teleop.Stop();
break;
case 'B':
teleop.Init();
teleop.Start();
break;
case 'c':
teleop.SetWrenchFeedbackScalingFactor(index, kJointGroup, 1);
break;
case 'C':
teleop.SetRepulsiveForce(index, kJointGroup, {0, 0, 0});
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 {
// Create teleop control interface
flexiv::tdk::TransparentCartesianTeleopLAN tctl({{leader_sn, follower_sn}});
// Run initialization sequence
tctl.Init();
// Start control loop
tctl.Start();
// Start console and pedal input threads
std::thread console_thread(std::bind(ConsoleTask, std::ref(tctl)));
// Start pedal_thread based on the new flag or the role
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).");
}
// Wait for threads to finish
console_thread.join();
// Stop all threads, notify other threads exit
g_running.store(false);
// Wait for digital reading task exit
if (pedal_thread && pedal_thread->joinable()) {
pedal_thread->join();
}
// Exit high transparency teleop
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.
@ COORD_WORLD
Definition data.hpp:197
Data for locking axis, including reference frame and axis to be locked for transparent cartesian tele...
Definition data.hpp:224
std::array< bool, 3 > lock_ori_axis
Orientation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:240
std::array< bool, 3 > lock_trans_axis
Translation axis lock, the corresponding axis order is . True for locking, false for floating.
Definition data.hpp:234
CoordType coord
Reference coordinate that the axis to be locked.
Definition data.hpp:228