intermediate2_realtime_joint_impedance_control.cpp

 
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/scheduler.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
 
#include <iostream>
#include <string>
#include <cmath>
#include <thread>
#include <atomic>
 
using namespace flexiv;
 
namespace {
constexpr double kLoopPeriod = 0.001;
 
constexpr double kSineAmp = 0.035;
constexpr double kSineFreq = 0.3;
 
std::atomic<bool> g_stop_sched = {false};
}
 
void PrintHelp()
{
    // clang-format off
    std::cout << "Required arguments: [robot_sn]" << std::endl;
    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-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;
    // clang-format on
}
 
void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
{
    // Local periodic loop counter
    static unsigned int loop_counter = 0;
 
    try {
        // Monitor fault on the connected robot
        if (robot.fault()) {
            throw std::runtime_error(
                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
        }
 
        std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
 
        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
            throw std::invalid_argument(
                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
        }
 
        for (const auto& [group, init_pos] : all_init_pos) {
            std::vector<double> target_pos(init_pos.size());
            std::vector<double> target_vel(init_pos.size());
            std::vector<double> target_acc(init_pos.size());
 
            if (motion_type == "hold") {
                target_pos = init_pos;
            } else {
                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);
                }
            }
 
            rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
        }
 
        // Reduce stiffness to half of nominal values after 5 seconds
        if (loop_counter == 5000) {
            for (const auto& [group, _] : single_arm_groups) {
                auto new_Kq = robot.info().K_q_nom.at(group);
                for (auto& v : new_Kq) {
                    v *= 0.5;
                }
                robot.SetJointImpedance(group, new_Kq);
                spdlog::info("[{}] Joint stiffness set to: [{}]", rdk::kJointGroupNames.at(group),
                    rdk::utility::Vec2Str(new_Kq));
            }
        }
 
        // Reset stiffness to nominal values after another 5 seconds
        if (loop_counter == 10000) {
            for (const auto& [group, _] : single_arm_groups) {
                const auto nominal_Kq = robot.info().K_q_nom.at(group);
                robot.SetJointImpedance(group, nominal_Kq);
                spdlog::info("[{}] Joint stiffness reset to nominal: [{}]",
                    rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(nominal_Kq));
            }
        }
 
        // Send commands
        robot.StreamJointPosition(rt_cmds);
 
        // Increment loop counter
        loop_counter++;
 
    } catch (const std::exception& e) {
        spdlog::error(e.what());
        g_stop_sched = true;
    }
}
 
int main(int argc, char* argv[])
{
    // Program Setup
    // =============================================================================================
    // Parse parameters
    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
        PrintHelp();
        return 1;
    }
    // Serial number of the robot to connect to
    std::string robot_sn = argv[1];
 
    // Print description
    spdlog::info(
        ">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to "
        "hold or sine-sweep all robot joints.\n");
 
    // Type of motion specified by user
    std::string motion_type = "";
    if (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 {
        // RDK Initialization
        // =========================================================================================
        // Instantiate robot interface
        rdk::Robot robot(robot_sn);
 
        // Clear fault on the connected robot if any
        if (robot.fault()) {
            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
            // Try to clear the fault
            if (!robot.ClearFault()) {
                spdlog::error("Fault cannot be cleared, exiting ...");
                return 1;
            }
            spdlog::info("Fault on the connected robot is cleared");
        }
 
        // Servo on the robot, make sure the E-stop is released
        spdlog::info("Servo on the robot ...");
        robot.ServoOn();
 
        // Wait for the robot to become operational
        while (!robot.operational()) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
        spdlog::info("Robot is now operational");
 
        // Move robot to home pose
        spdlog::info("Moving to home pose");
        robot.Home();
 
        // Real-time Joint Impedance Control
        // =========================================================================================
        // Direct joint control can be executed by single-arm joint groups and the external axis
        const auto& single_arm_groups = robot.info().single_arm_groups;
        if (single_arm_groups.empty()) {
            throw std::runtime_error("No single-arm joint group found on the connected robot");
        }
        // The external axis joint group (if it exists) also supports direct joint control
        auto exe_groups = single_arm_groups;
        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
            exe_groups.emplace(
                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
        }
 
        // Switch to real-time joint impedance control mode
        robot.SwitchMode(rdk::Mode::RT_JOINT_IMPEDANCE);
 
        // Set initial joint positions
        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
        const auto robot_states = robot.states();
        for (const auto& [group, _] : exe_groups) {
            all_init_pos[group] = robot_states.at(group).q;
            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
                rdk::utility::Vec2Str(all_init_pos.at(group)));
        }
 
        // Create real-time scheduler to run periodic tasks
        rdk::Scheduler scheduler;
        // Add periodic task with 1ms interval and highest applicable priority
        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
                              std::cref(single_arm_groups), std::cref(all_init_pos)),
            "HP periodic", 1, scheduler.max_priority());
        // Start all added tasks
        scheduler.Start();
 
        // Block and wait for signal to stop scheduler tasks
        while (!g_stop_sched) {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        // Received signal to stop scheduler tasks
        scheduler.Stop();
 
    } catch (const std::exception& e) {
        spdlog::error(e.what());
        return 1;
    }
 
    return 0;
}