This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the chosen reference frame will be activated for explicit force control, while the rest axes in the same reference frame will stay motion controlled.
#include <spdlog/spdlog.h>
#include <iostream>
#include <cmath>
#include <thread>
#include <atomic>
namespace {
constexpr double kLoopPeriod = 0.001;
constexpr double kSwingAmp = 0.1;
constexpr double kSwingFreq = 0.3;
constexpr double kPressingForce = 5.0;
constexpr double kSearchVelocity = 0.02;
constexpr double kSearchDistance = 1.0;
const std::array<double, flexiv::rdk::kCartDoF> kMaxWrenchForContactSearch
= {10.0, 10.0, 10.0, 3.0, 3.0, 3.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: [--TCP] [--polish]" << std::endl;
std::cout << " --TCP: use TCP frame as reference frame for force control, otherwise use world frame" << std::endl;
std::cout << " --polish: run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes"
<< std::endl
<< std::endl;
}
const std::array<double, flexiv::rdk::kPoseSize>& init_pose, const std::string frame_str,
bool enable_polish)
{
static uint64_t loop_counter = 0;
try {
throw std::runtime_error(
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
}
auto target_pose = init_pose;
double Fz = 0.0;
if (frame_str == "WORLD") {
Fz = -kPressingForce;
} else if (frame_str == "TCP") {
Fz = kPressingForce;
}
std::array<double, flexiv::rdk::kCartDoF> target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0};
if (enable_polish) {
target_pose[1] = init_pose[1]
+ kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
}
else {
}
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 unified "
"motion-force control. The Z axis of the chosen reference frame will be activated for "
"explicit force control, while the rest axes in the same reference frame will stay motion "
"controlled.");
std::string frame_str = "WORLD";
if (
flexiv::rdk::utility::ProgramArgsExist(argc, argv,
"--TCP")) {
spdlog::info("Reference frame used for force control: robot TCP frame");
frame_str = "TCP";
} else {
spdlog::info("Reference frame used for force control: robot world frame");
}
bool enable_polish = false;
if (flexiv::rdk::utility::ProgramArgsExist(argc, argv, "--polish")) {
spdlog::info("Robot will run a polish motion along XY plane in robot world frame");
enable_polish = true;
} else {
spdlog::info("Robot will hold its motion in all non-force-controlled axes");
}
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");
spdlog::info("Searching for contact ...");
spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: "
+
flexiv::rdk::utility::Arr2Str(init_pose));
robot.
SwitchMode(
flexiv::rdk::Mode::NRT_CARTESIAN_MOTION_FORCE);
auto target_pose = init_pose;
target_pose[2] -= kSearchDistance;
bool is_contacted = false;
while (!is_contacted) {
if (ext_force.norm() > kPressingForce) {
is_contacted = true;
spdlog::info("Contact detected at robot TCP");
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
std::array<bool, flexiv::rdk::kCartDoF> {false, false, true, false, false, false});
robot.
SwitchMode(
flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
scheduler.
AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pose),
std::ref(frame_str), enable_polish),
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.
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 SetForceControlAxis(const std::array< bool, kCartDoF > &enabled_axis, const std::array< double, kCartDoF/2 > &max_linear_vel={1.0, 1.0, 1.0})
[Blocking] Set force-controlled Cartesian axis(s) for the Cartesian motion-force control modes....
void SetForceControlFrame(const std::string &reference_frame)
[Blocking] Set force control reference frame for the Cartesian motion-force control modes....
void SendCartesianMotionForce(const std::array< double, kPoseSize > &pose, const std::array< double, kCartDoF > &wrench={}, double max_linear_vel=0.5, double max_angular_vel=1.0, double max_linear_acc=2.0, double max_angular_acc=5.0)
[Non-blocking] Discretely send Cartesian motion and/or force command for the robot to track using its...
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...
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 > ext_wrench_in_world
std::array< double, kPoseSize > tcp_pose