This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An outer position loop is used to generate joint torque commands. This outer position loop + inner torque loop together is also known as an impedance controller.
#include <spdlog/spdlog.h>
#include <iostream>
#include <string>
#include <cmath>
#include <thread>
#include <atomic>
namespace {
constexpr double kLoopPeriod = 0.001;
const std::vector<double> kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0};
const std::vector<double> kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0};
constexpr double kSineAmp = 0.035;
constexpr double kSineFreq = 0.3;
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]" << std::endl;
std::cout << " --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
std::cout << std::endl;
}
void PeriodicTask(
flexiv::rdk::Robot& robot,
const std::string& motion_type,
const std::vector<double>& init_pos)
{
static unsigned int loop_counter = 0;
try {
throw std::runtime_error(
"PeriodicTask: Fault occurred on the connected robot, exiting ...");
}
std::vector<double> target_pos(robot.
info().
DoF);
if (motion_type == "hold") {
target_pos = init_pos;
} else if (motion_type == "sine-sweep") {
for (size_t i = 0; i < target_pos.size(); ++i) {
target_pos[i] = init_pos[i]
+ kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
}
} else {
throw std::invalid_argument(
"PeriodicTask: unknown motion type. Accepted motion types: hold, sine-sweep");
}
std::vector<double> target_torque(robot.
info().
DoF);
for (size_t i = 0; i < target_torque.size(); ++i) {
target_torque[i] = kImpedanceKp[i] * (target_pos[i] - robot.
states().
q[i])
}
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 joint torque control to hold "
"or sine-sweep all robot joints. An outer position loop is used to generate joint torque "
"commands. This outer position loop + inner torque loop together is also known as an "
"impedance controller.");
std::string motion_type = "";
if (
flexiv::rdk::utility::ProgramArgsExist(argc, argv,
"--hold")) {
spdlog::info("Robot holding current pose");
motion_type = "hold";
} else {
spdlog::info("Robot running joint sine-sweep");
motion_type = "sine-sweep";
}
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));
}
robot.
SwitchMode(
flexiv::rdk::Mode::RT_JOINT_TORQUE);
spdlog::info(
"Initial joint positions set to: {}",
flexiv::rdk::utility::Vec2Str(init_pos));
std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type), std::ref(init_pos)),
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.
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 StreamJointTorque(const std::vector< double > &torques, bool enable_gravity_comp=true, bool enable_soft_limits=true)
[Non-blocking] Continuously stream joint torque command to the robot.
void SwitchMode(Mode mode)
[Blocking] Switch to a new control mode and wait until mode transition is finished.
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::vector< double > dtheta