test_cart_motion_tracking.cpp

 
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/scheduler.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
 
#include <iostream>
#include <fstream>
#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;
 
std::atomic<bool> g_stop_sched = {false};
 
// Data buffers written to log file after motion is finished
std::vector<unsigned int> loop_counter_buf = {};
std::vector<double> target_x_buf = {};
std::vector<double> actual_x_buf = {};
}
 
void PrintHelp()
{
    // clang-format off
    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 << std::endl;
    // clang-format on
}
 
void PeriodicTask(
    flexiv::rdk::Robot& robot, const std::array<double, flexiv::rdk::kPoseSize>& init_pose)
{
    // Local periodic loop counter
    static uint64_t 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 ...");
        }
 
        // Initialize target pose to initial pose
        auto target_pose = init_pose;
 
        // Sine-sweep TCP along X axis
        double w = 2 * M_PI * kSwingFreq;
        double t = loop_counter * kLoopPeriod;
        target_pose[0] = init_pose[0] + kSwingAmp * sin(w * t);
 
        // Calculate 2nd derivative of A*sin(wt) as feed-forward acceleration
        std::array<double, flexiv::rdk::kCartDoF> target_acc = {};
        target_acc[0] = -kSwingAmp * w * w * sin(w * t);
 
        // Zero target wrench
        std::array<double, flexiv::rdk::kCartDoF> target_wrench = {};
 
        // Send command with target acc
        robot.StreamCartesianMotionForce(target_pose, target_wrench, target_acc);
 
        // Save data @ 100Hz
        loop_counter_buf.emplace_back(loop_counter);
        target_x_buf.emplace_back(target_pose[0]);
        actual_x_buf.emplace_back(robot.states().tcp_pose[0]);
 
        // Stop after 10s
        if (++loop_counter > 10000) {
            g_stop_sched = true;
        }
 
    } 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 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
        PrintHelp();
        return 1;
    }
    // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456
    std::string robot_sn = argv[1];
 
    try {
        // RDK Initialization
        // =========================================================================================
        // Instantiate robot interface
        flexiv::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");
        }
 
        // Enable the robot, make sure the E-stop is released before enabling
        spdlog::info("Enabling robot ...");
        robot.Enable();
 
        // 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.SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
        robot.ExecutePrimitive("Home()");
 
        // Wait for the primitive to finish
        while (robot.busy()) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
 
        // Start Pure Motion Control
        // =========================================================================================
 
        // Switch to real-time mode for continuous motion control
        robot.SwitchMode(flexiv::rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
 
        // Set initial pose to current TCP pose
        auto init_pose = robot.states().tcp_pose;
        spdlog::info("Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: "
                     + flexiv::rdk::utility::Arr2Str(init_pose));
 
        // Create real-time scheduler to run periodic tasks
        flexiv::rdk::Scheduler scheduler;
        // Add periodic task with 1ms interval and highest applicable priority
        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(init_pose)),
            "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();
 
        // Write data to file
        // =========================================================================================
        // Create csv file
        std::ofstream csv_file;
        std::string csv_file_name = "test_data.csv";
        csv_file.open(csv_file_name);
        if (csv_file.is_open()) {
            spdlog::info("Created new log file: {}", csv_file_name);
        } else {
            spdlog::error("Failed to create log file: {}", csv_file_name);
            return 1;
        }
 
        // Check vector size matches each other
        if (loop_counter_buf.size() != target_x_buf.size()
            || target_x_buf.size() != actual_x_buf.size()) {
            spdlog::error(
                "Size mismatch: loop_counter_buf.size() = {}, target_x_buf.size() = {}, "
                "actual_x_buf.size() = {}",
                loop_counter_buf.size(), target_x_buf.size(), actual_x_buf.size());
            return 1;
        }
 
        // Log data: [target_x, actual_x]
        for (size_t i = 0; i < loop_counter_buf.size(); i++) {
            csv_file << loop_counter_buf[i] << "," << target_x_buf[i] << "," << actual_x_buf[i]
                     << '\n';
        }
 
        // Close file
        csv_file.close();
        spdlog::info("Saved {} data points to log file {}", target_x_buf.size(), csv_file_name);
 
    } catch (const std::exception& e) {
        spdlog::error(e.what());
        return 1;
    }
 
    return 0;
}