This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot TCP. A simple collision detection is also included.
#include <spdlog/spdlog.h>
#include <iostream>
#include <cmath>
#include <thread>
#include <atomic>
namespace {
constexpr size_t kLoopFreq = 1000;
constexpr double kLoopPeriod = 0.001;
constexpr double kSwingAmp = 0.1;
constexpr double kSwingFreq = 0.3;
constexpr double kExtForceThreshold = 10.0;
constexpr double kExtTorqueThreshold = 5.0;
std::atomic<bool> g_stop_sched = {false};
}
void PrintHelp()
{
std::cout << "Required arguments: [robot SN]" << std::endl;
std::cout << " robot SN: Serial number of the robot to connect to. "
"Remove any space, for example: Rizon4s-123456" << std::endl;
std::cout << "Optional arguments: [--hold] [--collision]" << std::endl;
std::cout << " --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl;
std::cout << " --collision: enable collision detection, robot will stop upon collision" << std::endl;
std::cout << std::endl;
}
const std::array<double, flexiv::rdk::kPoseSize>& init_pose, bool enable_hold,
bool enable_collision)
{
static uint64_t loop_counter = 0;
try {
throw std::runtime_error(
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
}
auto target_pose = init_pose;
if (!enable_hold) {
target_pose[1] = init_pose[1]
+ kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
}
switch (loop_counter % (20 * kLoopFreq)) {
case (3 * kLoopFreq): {
std::vector<double> preferred_jnt_pos
= {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658};
spdlog::info("Preferred joint positions set to: "
+
flexiv::rdk::utility::Vec2Str(preferred_jnt_pos));
} break;
case (6 * kLoopFreq): {
for (auto& v : new_K) {
v *= 0.5;
}
spdlog::info(
"Cartesian stiffness set to: {}",
flexiv::rdk::utility::Arr2Str(new_K));
} break;
case (9 * kLoopFreq): {
std::vector<double> preferred_jnt_pos
= {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658};
spdlog::info("Preferred joint positions set to: "
+ flexiv::rdk::utility::Vec2Str(preferred_jnt_pos));
} break;
case (12 * kLoopFreq): {
spdlog::info("Cartesian stiffness is reset");
} break;
case (14 * kLoopFreq): {
spdlog::info("Preferred joint positions are reset");
} break;
case (16 * kLoopFreq): {
std::array<double, flexiv::rdk::kCartDoF> max_wrench
= {10.0, 10.0, 10.0, 2.0, 2.0, 2.0};
spdlog::info(
"Max contact wrench set to: {}", flexiv::rdk::utility::Arr2Str(max_wrench));
} break;
case (19 * kLoopFreq): {
spdlog::info("Max contact wrench is reset");
} break;
default:
break;
}
if (enable_collision) {
bool collision_detected = false;
if (ext_force.norm() > kExtForceThreshold) {
collision_detected = true;
}
if (fabs(v) > kExtTorqueThreshold) {
collision_detected = true;
}
}
if (collision_detected) {
spdlog::warn("Collision detected, stopping robot and exit program ...");
g_stop_sched = true;
}
}
loop_counter++;
} catch (const std::exception& e) {
spdlog::error(e.what());
g_stop_sched = true;
}
}
int main(int argc, char* argv[])
{
if (argc < 2 ||
flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {
"-h",
"--help"})) {
PrintHelp();
return 1;
}
std::string robot_sn = argv[1];
spdlog::info(
">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space pure motion "
"control to hold or sine-sweep the robot TCP. A simple collision detection is also "
"included.");
bool enable_hold = false;
if (
flexiv::rdk::utility::ProgramArgsExist(argc, argv,
"--hold")) {
spdlog::info("Robot holding current TCP pose");
enable_hold = true;
} else {
spdlog::info("Robot running TCP sine-sweep");
}
bool enable_collision = false;
if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--collision")) {
spdlog::info("Collision detection enabled");
enable_collision = true;
} else {
spdlog::info("Collision detection disabled");
}
try {
spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
spdlog::error("Fault cannot be cleared, exiting ...");
return 1;
}
spdlog::info("Fault on the connected robot is cleared");
}
spdlog::info("Enabling robot ...");
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::info("Robot is now operational");
spdlog::info("Moving to home pose");
robot.
SwitchMode(
flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::warn(
"Zeroing force/torque sensors, make sure nothing is in contact with the robot");
std::this_thread::sleep_for(std::chrono::seconds(1));
}
spdlog::info("Sensor zeroing complete");
robot.
SwitchMode(
flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: "
+ flexiv::rdk::utility::Arr2Str(init_pose));
scheduler.
AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pose), enable_hold,
enable_collision),
while (!g_stop_sched) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
Main interface with the robot, containing several function categories and background services.
void StreamCartesianMotionForce(const std::array< double, kPoseSize > &pose, const std::array< double, kCartDoF > &wrench={}, const std::array< double, kCartDoF > &acceleration={})
[Non-blocking] Continuously stream Cartesian motion and/or force command for the robot to track using...
void SetMaxContactWrench(const std::array< double, kCartDoF > &max_wrench)
[Non-blocking] Set maximum contact wrench for the motion control part of the Cartesian motion-force c...
const RobotStates states() const
[Non-blocking] Access the current robot states.
const RobotInfo info() const
[Non-blocking] Access general information of the robot.
bool operational(bool verbose=true) const
[Non-blocking] Whether the robot is normally operational, which requires the following conditions to ...
void ExecutePrimitive(const std::string &pt_cmd)
[Blocking] Execute a primitive by specifying its name and parameters, which can be found in the Flexi...
void Stop()
[Blocking] Stop the robot and transit robot mode to IDLE.
void ResetMaxContactWrench()
[Non-blocking] Reset max contact wrench regulation to nominal state, i.e. disabled.
void SwitchMode(Mode mode)
[Blocking] Switch to a new control mode and wait until mode transition is finished.
void SetCartesianImpedance(const std::array< double, kCartDoF > &K_x, const std::array< double, kCartDoF > &Z_x={0.7, 0.7, 0.7, 0.7, 0.7, 0.7})
[Non-blocking] Set impedance properties of the robot's Cartesian motion controller used in the Cartes...
void ResetNullSpacePosture()
[Non-blocking] Reset preferred joint positions to the ones automatically recorded when entering the a...
void Enable()
[Blocking] Enable the robot, if E-stop is released and there's no fault, the robot will release brake...
bool fault() const
[Non-blocking] Whether the robot is in fault state.
bool ClearFault(unsigned int timeout_sec=30)
[Blocking] Try to clear minor or critical fault of the robot without a power cycle.
bool busy() const
[Non-blocking] Whether the robot is currently executing a task. This includes any user commanded oper...
void SetNullSpacePosture(const std::vector< double > &preferred_positions)
[Non-blocking] Set preferred joint positions for the null-space posture control module used in the Ca...
void ResetCartesianImpedance()
[Non-blocking] Reset impedance properties of the robot's Cartesian motion controller to nominal value...
Real-time scheduler that can simultaneously run multiple periodic tasks. Parameters for each task are...
int max_priority() const
[Non-blocking] Get maximum available priority for user tasks.
void AddTask(std::function< void(void)> &&callback, const std::string &task_name, int interval, int priority, int cpu_affinity=-1)
[Non-blocking] Add a new periodic task to the scheduler's task pool. Each task in the pool is assigne...
void Stop()
[Blocking] Stop all added tasks. The periodic execution will stop and all task threads will be closed...
void Start()
[Blocking] Start all added tasks. A dedicated thread will be created for each added task and the peri...
std::array< double, kCartDoF > K_x_nom
std::array< double, kCartDoF > ext_wrench_in_world
std::vector< double > tau_ext
std::array< double, kPoseSize > tcp_pose