orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

Browse Source 
顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-12 17:08:43 +08:00
parent b903161113
commit f882c4309c
187 changed files with 22898 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
thirdparty/flexiv_rdk/test/CMakeLists.txt
Vendored
+77
View File
@@ -0,0 +1,77 @@
cmake_minimum_required(VERSION 3.22.1)
project(flexiv_rdk-tests)
# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
if(POLICY CMP0167)
cmake_policy(SET CMP0167 OLD)
endif()
# Show verbose build info
SET(CMAKE_VERBOSE_MAKEFILE ON)
message("OS: ${CMAKE_SYSTEM_NAME}")
message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
# Configure build type
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
endif()
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
find_package(flexiv_rdk REQUIRED)
# List of functionality tests
set(FUNCTIONALITY_TESTS
test_dynamics_engine
test_repeated_instantiation
)
if(CMAKE_HOST_UNIX)
list(APPEND FUNCTIONALITY_TESTS
test_endurance
test_loop_latency
test_scheduler
test_timeliness_monitor
)
endif()
# Build functionality tests
foreach(test ${FUNCTIONALITY_TESTS})
add_executable(${test} ${test}.cpp)
target_link_libraries(${test} flexiv::flexiv_rdk)
# C++20 required
set_target_properties(${test} PROPERTIES
CXX_STANDARD 20
CXX_STANDARD_REQUIRED ON)
endforeach()
# List of unit tests
set(UNIT_TESTS
test_robot_constructor
)
# Download GTest if not found
include(FetchContent)
find_package(GTest CONFIG QUIET)
if(NOT GTest_FOUND)
if(MSVC)
set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
endif()
FetchContent_Declare(
googletest
URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip
)
FetchContent_MakeAvailable(googletest)
endif()
# Build unit tests
foreach(test ${UNIT_TESTS})
add_executable(${test} ${test}.cpp)
target_link_libraries(${test} flexiv::flexiv_rdk GTest::gtest_main)
add_test(NAME ${test} COMMAND ${test})
# C++20 required
set_target_properties(${test} PROPERTIES
CXX_STANDARD 20
CXX_STANDARD_REQUIRED ON)
endforeach()
thirdparty/flexiv_rdk/test/test_dynamics_engine.cpp
Vendored
+256
View File
@@ -0,0 +1,256 @@
/**
* @test test_dynamics_engine.cpp
* A test to evaluate the dynamics engine (J, M, G), with and without end-effector tool.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/model.hpp>
#include <flexiv/rdk/tool.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>
namespace {
/** J, M, G ground truth from MATLAB */
struct GroundTruth
{
/** Jacobian matrix */
Eigen::Matrix<double, 6, 7> J;
/** Mass matrix */
Eigen::Matrix<double, 7, 7> M;
/** Gravity torques */
Eigen::Matrix<double, 7, 1> G;
};
}
/** Step the dynamics engine once */
void StepDynamics(flexiv::rdk::Robot& robot, flexiv::rdk::Model& model, const GroundTruth& ref)
{
// Mark timer start point
auto tic = std::chrono::high_resolution_clock::now();
// Update robot model in dynamics engine
const auto robot_states = robot.states();
model.Update(robot_states.at(flexiv::rdk::JointGroup::ALL).q,
robot_states.at(flexiv::rdk::JointGroup::ALL).dtheta);
// Get J, M, G from dynamic engine
Eigen::MatrixXd J = model.J("flange");
Eigen::MatrixXd M = model.M();
Eigen::VectorXd G = model.g();
// Get and print computation time
auto toc = std::chrono::high_resolution_clock::now();
auto computation_time
= std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
std::cout << "===================================================================" << std::endl;
std::cout << "Computation time = " << computation_time << " us" << std::endl;
// Evaluate J, M, G with true value
auto delta_J = J - ref.J;
auto delta_M = M - ref.M;
auto delta_G = G - ref.G;
std::cout << std::fixed << std::setprecision(5);
std::cout << "Difference of J between ground truth (MATLAB) and integrated dynamics engine = "
<< std::endl
<< delta_J << std::endl;
std::cout << "Norm of delta J: " << delta_J.norm() << '\n' << std::endl;
std::cout << "Difference of M between ground truth (MATLAB) and integrated dynamics engine = "
<< std::endl
<< delta_M << std::endl;
std::cout << "Norm of delta M: " << delta_M.norm() << '\n' << std::endl;
std::cout << "Difference of G between ground truth (MATLAB) and integrated dynamics engine = "
<< std::endl
<< delta_G.transpose() << std::endl;
std::cout << "Norm of delta G: " << delta_G.norm() << '\n' << std::endl;
}
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: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Parse Parameters
//==============================================================================================
if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to
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");
}
// 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
//==========================================================================================
robot.Home();
// Test Dynamics Engine without Tool
//==========================================================================================
spdlog::info(">>>>> Test 1: no end-effector tool <<<<<");
// Instantiate dynamics engine
flexiv::rdk::Model model(robot);
// Ground truth from MATLAB without robot tool
GroundTruth ref;
// clang-format off
ref.J <<
0.110000000000000, 0.078420538028673, 0.034471999940354, -0.368152340866938, -0.064278760968654, 0.136000000000000, -0.000000000000000,
0.687004857483100, -0.000000000000000, 0.576684003660457, 0.000000000000000, 0.033475407198662, -0.000000000000000, -0.000000000000000,
0.000000000000000, 0.687004857483100, -0.028925442435894, -0.417782862794537, -0.076604444311898, 0.110000000000000, -0.000000000000000,
0.000000000000000, -0.000000000000000, 0.642787609686539, 0.000000000000000, 0.766044443118978, -0.000000000000000, 0.000000000000000,
0, -1.000000000000000, -0.000000000000000, 1.000000000000000, 0.000000000000000, -1.000000000000000, 0.000000000000000,
1.000000000000000, -0.000000000000000, 0.766044443118978, 0.000000000000000, -0.642787609686539, -0.000000000000000, -1.000000000000000;
ref.M <<
2.480084579964625, -0.066276150278304, 1.520475428405090, -0.082258763257690, -0.082884472612488, 0.008542255000000, -0.000900000000000,
-0.066276150278304, 2.778187401738267, 0.067350373889147, -1.100953424157635, -0.129191018084721, 0.165182543869134, -0.000000000000000,
1.520475428405090, 0.067350373889147, 1.074177611590570, -0.000410055889147, -0.043572229972025, -0.001968185110853, -0.000689439998807,
-0.082258763257690, -1.100953424157635, -0.000410055889147, 0.964760218577003, 0.123748338084721, -0.125985653288502, 0.000000000000000,
-0.082884472612488, -0.129191018084721, -0.043572229972025, 0.123748338084721, 0.038006882479315, -0.020080821084721, 0.000578508848718,
0.008542255000000, 0.165182543869134, -0.001968185110853, -0.125985653288502, -0.020080821084721, 0.034608170000000, 0,
-0.000900000000000, -0.000000000000000, -0.000689439998807, 0.000000000000000, 0.000578508848718, 0, 0.000900000000000;
ref.G <<
0.000000000000000, 46.598931189891374, 1.086347692836129, -19.279904969860052, -2.045058704525489, 2.300886450000000, 0;
// clang-format on
// Step the dynamics engine
int i = 0;
while (++i <= 3) {
StepDynamics(robot, model, ref);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
// Test Dynamics Engine with Tool
//==========================================================================================
spdlog::info(">>>>> Test 2: with end-effector tool <<<<<");
// Ground truth from MATLAB with robot tool
// clang-format off
ref.M <<
2.916316686749461, -0.052869517013466, 1.903540434220357, -0.124348845003517, -0.041914639740668, 0.027649255000000, -0.001464000000000,
-0.052869517013466, 3.222619358431081, 0.053667041477633, -1.414236317529289, -0.184390078851855, 0.259867572215541, -0.000000000000000,
1.903540434220357, 0.053667041477633, 1.416023230140298, -0.002724223477633, 0.000093550780077, 0.001155982477633, -0.001121489064726,
-0.124348845003517, -1.414236317529289, -0.002724223477633, 1.287676548627496, 0.177147398851855, -0.244344118313748, 0.000000000000000,
-0.041914639740668, -0.184390078851855, 0.000093550780077, 0.177147398851855, 0.054219756143365, -0.038829881851855, 0.000941041060581,
0.027649255000000, 0.259867572215541, 0.001155982477633, -0.244344118313748, -0.038829881851855, 0.082171270000000, 0,
-0.001464000000000, -0.000000000000000, -0.001121489064726, 0.000000000000000, 0.000941041060581, 0, 0.001464000000000;
// Matlab value is the joint torque to resist gravity
ref.G <<
-0.000000000000001, 52.664497076609663, 0.830964961569619, -22.968509865473024, -2.721399343355234, 3.272076450000000, 0;
// clang-format on
// Instantiate tool interface
flexiv::rdk::Tool tool(robot);
// Tools can only be assigned to single-arm joint groups
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");
}
// Set name and parameters for the test tool
std::string tool_name = "DynamicsTestTool";
flexiv::rdk::ToolParams tool_params;
tool_params.mass = 0.9;
tool_params.CoM = {0.0, 0.0, 0.057};
tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
tool_params.tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
// Remove any existing tool with the same name
if (tool.exist(tool_name)) {
spdlog::warn("Tool with the same name [{}] already exists, removing it now", tool_name);
// Switch to other tool or no tool (Flange) before removing the current tool
for (const auto& [group, _] : single_arm_groups) {
tool.Switch(group, "Flange");
}
tool.Remove(tool_name);
}
// Add the test tool
spdlog::info("Adding test tool [{}] to the robot", tool_name);
tool.Add(tool_name, tool_params);
// Switch to the newly added test tool, i.e. set it as the active tool
spdlog::info("Switching to test tool [{}]", tool_name);
for (const auto& [group, _] : single_arm_groups) {
tool.Switch(group, tool_name);
}
// Get and print the current active tool for each joint group, should be the test tool
for (const auto& [group, _] : single_arm_groups) {
spdlog::info("[{}] Current active tool: {}", flexiv::rdk::kJointGroupNames.at(group),
tool.name(group));
}
// Reload robot + tool model using the latest data synced from the connected robot
model.Reload();
// Step the dynamics engine again
i = 0;
while (++i <= 3) {
StepDynamics(robot, model, ref);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
// Switch to other tool or no tool (Flange) before removing the current tool
for (const auto& [group, _] : single_arm_groups) {
tool.Switch(group, "Flange");
}
// Clean up by removing the test tool
spdlog::info("Removing tool [{}]", tool_name);
tool.Remove(tool_name);
spdlog::info("Program finished");
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
thirdparty/flexiv_rdk/test/test_endurance.cpp
Vendored
+278
View File
@@ -0,0 +1,278 @@
/**
* @test test_endurance.cpp
* Endurance test running Cartesian impedance control to slowly sine-sweep near home for a duration
* of user-specified hours. Raw data will be logged to CSV files continuously.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/scheduler.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <thread>
#include <atomic>
using namespace flexiv;
namespace {
/** RT loop period [sec] */
const double kLoopPeriod = 0.001;
/** TCP sine-sweep amplitude [m] */
const double kSwingAmp = 0.1;
/** TCP sine-sweep frequency [Hz] */
const double kSwingFreq = 0.025; // = 10mm/s linear velocity
/** Current Cartesian-space pose (position + rotation) of robot TCP */
std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> g_curr_tcp_pose;
/** Test duration converted from user-specified hours to loop counts */
uint64_t g_test_duration_loop_counts = 0;
/** Time duration for each log file [loop counts] */
const unsigned int kLogDurationLoopCounts = 10 * 60 * 1000; // = 10 min/file
/** Data to be logged in low-priority thread */
struct LogData
{
std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> tcp_pose;
std::map<rdk::JointGroup, std::array<double, rdk::kCartDoF>> tcp_force;
} g_log_data;
/** Atomic signal to stop the test */
std::atomic<bool> g_stop = {false};
}
void HighPriorityTask(rdk::Robot& robot,
const std::map<rdk::JointGroup, std::string>& single_arm_groups,
const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_poses)
{
// Local periodic loop counter
static uint64_t loop_counter = 0;
try {
// Monitor fault on the connected robot
if (robot.fault()) {
throw std::runtime_error(
"HighPriorityTask: Fault occurred on the connected robot, exiting ...");
}
// Swing along Z direction
std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
for (const auto& [group, _] : single_arm_groups) {
auto target_pose = g_curr_tcp_pose.at(group);
target_pose[2] = all_init_poses.at(group)[2]
+ kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
g_curr_tcp_pose[group] = target_pose;
rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
}
robot.StreamCartesianMotionForce(rt_cmds);
// Save data to global buffer, not using mutex to avoid interruption on RT loop from
// potential priority inversion
for (const auto& [group, states] : robot.states()) {
g_log_data.tcp_pose[group] = states.tcp_pose;
g_log_data.tcp_force[group] = states.tcp_wrench;
}
// Stop after test duration has elapsed
if (++loop_counter > g_test_duration_loop_counts) {
g_stop = true;
}
} catch (const std::exception& e) {
spdlog::error(e.what());
g_stop = true;
}
}
void LowPriorityTask()
{
// Low-priority task loop counter
uint64_t loop_counter = 0;
// Data logging CSV file
std::ofstream csv_file;
// CSV file name
std::string csv_file_name;
// CSV file counter (data during the test is divided to multiple files)
unsigned int file_counter = 0;
// Wait for a while for the robot states data to be available
std::this_thread::sleep_for(std::chrono::seconds(2));
// Use while loop to prevent this thread from return
while (true) {
// Log data at 1kHz
std::this_thread::sleep_for(std::chrono::milliseconds(1));
loop_counter++;
// Close existing log file and create a new one periodically
if (loop_counter % kLogDurationLoopCounts == 0) {
if (csv_file.is_open()) {
csv_file.close();
spdlog::info("Saved log file: {}", csv_file_name);
}
// Increment log file counter
file_counter++;
// Create new file name using the updated counter as suffix
csv_file_name = "endurance_test_data_" + std::to_string(file_counter) + ".csv";
// Open new log file
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);
}
}
// Log data to file in CSV format
if (csv_file.is_open()) {
// Loop counter x1, then grouped TCP pose x7 and TCP wrench x6
csv_file << loop_counter << ",";
for (const auto& [group, pose] : g_log_data.tcp_pose) {
csv_file << rdk::kJointGroupNames.at(group) << ",";
for (const auto& i : pose) {
csv_file << i << ",";
}
for (const auto& i : g_log_data.tcp_force.at(group)) {
csv_file << i << ",";
}
}
// End of line
csv_file << '\n';
}
// Check if the test duration has elapsed
if (g_stop) {
spdlog::info("Test duration has elapsed, saving any open log file ...");
// Close log file
if (csv_file.is_open()) {
csv_file.close();
spdlog::info("Saved log file: {}", csv_file_name);
}
// Exit thread
return;
}
}
}
void PrintHelp()
{
// clang-format off
std::cout << "Required arguments: [robot_sn] [test_hours]" << 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 << " test_hours: Duration of the test, can have decimals" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Parse Parameters
//==============================================================================================
if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to
std::string robot_sn = argv[1];
// test duration in hours
double test_hours = std::stof(argv[2]);
// convert duration in hours to loop counts
g_test_duration_loop_counts = (uint64_t)(test_hours * 3600.0 * 1000.0);
spdlog::info("Test duration: {} hours = {} cycles", test_hours, g_test_duration_loop_counts);
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
//==========================================================================================
// Direct Cartesian control can only be executed by single-arm joint groups
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");
}
robot.Home();
// Switch mode after robot is at home
robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
for (const auto& [group, _] : single_arm_groups) {
robot.SetForceControlAxis(
group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
}
// Set initial pose to current TCP pose
std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
const auto robot_states = robot.states();
for (const auto& [group, _] : single_arm_groups) {
all_init_pose[group] = robot_states.at(group).tcp_pose;
g_curr_tcp_pose[group] = robot_states.at(group).tcp_pose;
spdlog::info("[{}] Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: {}",
rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
}
// Periodic Tasks
//==========================================================================================
rdk::Scheduler scheduler;
// Add periodic task with 1ms interval and highest applicable priority
scheduler.AddTask(std::bind(HighPriorityTask, std::ref(robot), std::cref(single_arm_groups),
std::cref(all_init_pose)),
"HP periodic", 1, scheduler.max_priority());
// Start all added tasks
scheduler.Start();
// Use std::thread for logging task without strict chronology
std::thread low_priority_thread(LowPriorityTask);
// low_priority_thread is responsible to release blocking
low_priority_thread.join();
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
thirdparty/flexiv_rdk/test/test_loop_latency.cpp
Vendored
+175
View File
@@ -0,0 +1,175 @@
/**
* @test test_loop_latency.cpp
* A test to benchmark RDK's loop latency, including communication, computation, etc. The
* workstation PC's serial port is used as reference, and the robot server's digital out port is
* used as test target.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/scheduler.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
#include <iostream>
#include <thread>
#include <string.h>
#include <atomic>
// Standard input/output definitions
#include <stdio.h>
// UNIX standard function definitions
#include <unistd.h>
// File control definitions
#include <fcntl.h>
// Error number definitions
#include <errno.h>
// POSIX terminal control definitions
#include <termios.h>
namespace {
/** File descriptor of the serial port */
int g_fd = 0;
/** Atomic signal to stop scheduler tasks */
std::atomic<bool> g_stop_sched = {false};
}
// callback function for realtime periodic task
void PeriodicTask(flexiv::rdk::Robot& robot)
{
// 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 ...");
}
// send signal at 1Hz
switch (loop_counter % 1000) {
case 0: {
spdlog::info(
"Sending benchmark signal to both workstation PC's serial "
"port and robot server's digital out port[0]");
break;
}
case 1: {
// signal robot server's digital out port
robot.SetDigitalOutputs({{0, true}});
// signal workstation PC's serial port
auto n = write(g_fd, "0", 1);
if (n < 0) {
spdlog::error("Failed to write to serial port");
}
break;
}
case 900: {
// reset digital out after a few seconds
robot.SetDigitalOutputs({{0, false}});
break;
}
default:
break;
}
loop_counter++;
} catch (const std::exception& e) {
spdlog::error(e.what());
g_stop_sched = true;
}
}
void PrintHelp()
{
// clang-format off
std::cout << "Required arguments: [robot_sn] [serial_port_name]" << 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 << " serial_port_name: /dev/ttyS0 for COM1, /dev/ttyS1 for COM2, /dev/ttyUSB0 for USB-serial converter" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Parse Parameters
//=============================================================================
if (argc < 3 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to
std::string robot_sn = argv[1];
// serial port name
std::string serial_port = argv[2];
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");
}
// 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");
// Benchmark Signal
//=============================================================================
// get workstation PC's serial port ready,
g_fd = open(serial_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL | O_CLOEXEC);
if (g_fd == -1) {
spdlog::error("Unable to open serial port [{}]", serial_port);
}
// print messages
spdlog::warn("Benchmark signal will be sent every 1 second");
// Periodic Tasks
//=============================================================================
flexiv::rdk::Scheduler scheduler;
// Add periodic task with 1ms interval and highest applicable priority
scheduler.AddTask(
std::bind(PeriodicTask, std::ref(robot)), "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;
}
thirdparty/flexiv_rdk/test/test_repeated_instantiation.cpp
Vendored
+68
View File
@@ -0,0 +1,68 @@
/**
* @test test_repeated_instantiation.cpp
* A test to repeatedly construct and destruct rdk::Robot instance.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#include <flexiv/rdk/robot.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
#include <iostream>
#include <thread>
using namespace flexiv;
/** @brief Print program usage help */
void PrintHelp()
{
// clang-format off
std::cout << "Required arguments: [robot_sn] [test_cycles]" << 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 << " test_cycles: Number of cycles to repeat the instantiation" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Program Setup
// =============================================================================================
// Parse parameters
if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to
std::string robot_sn = argv[1];
// Number of cycles
unsigned int test_cycles = std::stoi(argv[2]);
try {
for (size_t i = 1; i <= test_cycles; i++) {
spdlog::info("Test cycle: {}/{}", i, test_cycles);
// Instantiate robot interface
rdk::Robot robot(robot_sn);
// 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");
}
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
thirdparty/flexiv_rdk/test/test_robot_constructor.cpp
Vendored
+56
View File
@@ -0,0 +1,56 @@
/**
* @test test_robot_constructor.cpp
* @brief Test offline construction of flexivrdk.Robot.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
*/
#include <flexiv/rdk/robot.hpp>
#include <gtest/gtest.h>
#include <exception>
#include <string>
#include <string_view>
namespace {
template <typename ExceptionT>
void ExpectRobotConstructorFailure(
const std::string& robot_sn, std::string_view expected_message_fragment)
{
try {
flexiv::rdk::Robot robot(robot_sn, false);
FAIL() << "Expected constructor to fail for serial [" << robot_sn << "], but it succeeded";
} catch (const ExceptionT& e) {
const std::string_view message(e.what());
EXPECT_NE(message.find(expected_message_fragment), std::string_view::npos)
<< "Constructor for serial [" << robot_sn
<< "] threw the expected exception type but with unexpected message: " << e.what();
} catch (const std::exception& e) {
FAIL() << "Constructor for serial [" << robot_sn
<< "] threw an unexpected exception type: " << e.what();
}
}
} // namespace
TEST(RobotConstructor, InvalidSerialNumberFormat)
{
ExpectRobotConstructorFailure<std::invalid_argument>("Enlight412345", "serial number is invalid");
}
TEST(RobotConstructor, UnsupportedProductModel)
{
for (const std::string robot_sn : {"Enlight-X-123456", "Enlight X-123456"}) {
SCOPED_TRACE(robot_sn);
ExpectRobotConstructorFailure<std::logic_error>(robot_sn, "is not supported");
}
}
TEST(RobotConstructor, SupportedModelWithoutServer)
{
for (const std::string robot_sn : {"Enlight-L-123456", "Enlight L-123456"}) {
SCOPED_TRACE(robot_sn);
ExpectRobotConstructorFailure<std::runtime_error>(robot_sn, "Failed to connect");
}
}
thirdparty/flexiv_rdk/test/test_robot_constructor.py
Vendored
+37
View File
@@ -0,0 +1,37 @@
"""Test offline construction of flexivrdk.Robot"""
import unittest
import flexivrdk
class RobotConstructorTest(unittest.TestCase):
def assert_robot_constructor_failure(
self, robot_sn, expected_exception, message_fragment
):
with self.assertRaises(expected_exception) as context:
flexivrdk.Robot(robot_sn, False)
self.assertIn(message_fragment, str(context.exception))
def test_invalid_format(self):
self.assert_robot_constructor_failure(
"Enlight412345", ValueError, "serial number is invalid"
)
def test_unsupported_model(self):
for robot_sn in ("Enlight-X-123456", "Enlight X-123456"):
with self.subTest(robot_sn=robot_sn):
self.assert_robot_constructor_failure(
robot_sn, RuntimeError, "is not supported"
)
def test_supported_model_without_server(self):
for robot_sn in ("Enlight-L-123456", "Enlight L-123456"):
with self.subTest(robot_sn=robot_sn):
self.assert_robot_constructor_failure(
robot_sn, RuntimeError, "Failed to connect"
)
if __name__ == "__main__":
unittest.main()
thirdparty/flexiv_rdk/test/test_scheduler.cpp
Vendored
+148
View File
@@ -0,0 +1,148 @@
/**
* @test test_scheduler.cpp
* A test to evaluate RDK's internal real-time scheduler.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#include <flexiv/rdk/scheduler.hpp>
#include <flexiv/rdk/utility.hpp>
#include <spdlog/spdlog.h>
#include <iostream>
#include <thread>
#include <chrono>
#include <mutex>
#include <atomic>
namespace {
/** Data shared between threads */
struct SharedData
{
int64_t measured_interval = 0;
} g_data;
/** Mutex on the shared data */
std::mutex g_mutex;
/** Atomic signal to stop scheduler tasks */
std::atomic<bool> g_stop_sched = {false};
}
/** User-defined high-priority periodic task @ 1kHz */
void HighPriorityTask()
{
static unsigned int loop_counter = 0;
// Scheduler loop interval start time point
static std::chrono::high_resolution_clock::time_point tic;
try {
// Mark loop interval end point
auto toc = std::chrono::high_resolution_clock::now();
// Calculate scheduler's interrupt interval and print
auto measured_interval
= std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
// Safely write shared data
{
std::lock_guard<std::mutex> lock(g_mutex);
g_data.measured_interval = measured_interval;
}
// Stop scheduler after 5 seconds
if (++loop_counter > 5000) {
loop_counter = 0;
g_stop_sched = true;
}
// Mark loop interval start point
tic = std::chrono::high_resolution_clock::now();
} catch (const std::exception& e) {
spdlog::error(e.what());
g_stop_sched = true;
}
}
/** User-defined low-priority periodic task @1Hz */
void LowPriorityTask()
{
static uint64_t accumulated_time = 0;
static uint64_t num_measures = 0;
static float avg_interval = 0.0;
int measured_interval = 0;
// Safely read shared data
{
std::lock_guard<std::mutex> lock(g_mutex);
measured_interval = g_data.measured_interval;
}
// calculate average time interval
accumulated_time += measured_interval;
num_measures++;
avg_interval = (float)accumulated_time / (float)num_measures;
// print time interval of high-priority periodic task
spdlog::info(
"High-priority task interval (curr | avg) = {} | {} us", measured_interval, avg_interval);
}
void PrintHelp()
{
// clang-format off
std::cout << "Required arguments: None" << std::endl;
std::cout << "Optional arguments: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Parse Parameters
//==============================================================================================
if (flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
try {
// Periodic Tasks
//==========================================================================================
flexiv::rdk::Scheduler scheduler;
// Add periodic task with 1ms interval and highest applicable priority
scheduler.AddTask(std::bind(HighPriorityTask), "HP periodic", 1, scheduler.max_priority());
// Add periodic task with 1s interval and lowest applicable priority
scheduler.AddTask(std::bind(LowPriorityTask), "LP periodic", 1000, 0);
// 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();
// Restart scheduler after 2 seconds
spdlog::warn("Scheduler will restart in 2 seconds");
std::this_thread::sleep_for(std::chrono::seconds(2));
g_stop_sched = false;
scheduler.Start();
// 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, flexiv::rdk::Scheduler's destructor can also do
// the thread exit and resources cleanup
} catch (const std::exception& e) {
spdlog::error(e.what());
return 1;
}
return 0;
}
thirdparty/flexiv_rdk/test/test_timeliness_monitor.cpp
Vendored
+160
View File
@@ -0,0 +1,160 @@
/**
* @test test_timeliness_monitor.cpp
* A test to evaluate RDK's internal timeliness monitor on real-time modes. Bad communication or
* insufficient real-time performance of the workstation PC will cause the monitor's timeliness
* check to fail. A warning will be issued first, then if the check has failed too many times, the
* RDK connection with the server will be closed. During this test, the robot will hold its position
* using joint torque streaming mode.
* @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#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>
namespace {
/** Atomic signal to stop scheduler tasks */
std::atomic<bool> g_stop_sched = {false};
}
// callback function for realtime periodic task
void PeriodicTask(flexiv::rdk::Robot& robot,
const std::map<flexiv::rdk::JointGroup, std::string>& single_arm_groups,
const std::map<flexiv::rdk::JointGroup, std::vector<double>>& all_init_pos)
{
// 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 ...");
}
// Hold position
std::map<flexiv::rdk::JointGroup, flexiv::rdk::RtJointPositionCmd> rt_cmds;
for (const auto& [group, _] : single_arm_groups) {
const auto& init_pos = all_init_pos.at(group);
std::vector<double> target_vel(init_pos.size());
std::vector<double> target_acc(init_pos.size());
rt_cmds[group] = flexiv::rdk::RtJointPositionCmd(init_pos, target_vel, target_acc);
}
robot.StreamJointPosition(rt_cmds);
if (loop_counter == 5000) {
spdlog::warn(">>>>> Adding simulated loop delay <<<<<");
}
// simulate prolonged loop time after 5 seconds
else if (loop_counter > 5000) {
std::this_thread::sleep_for(std::chrono::microseconds(995));
}
loop_counter++;
} catch (const std::exception& e) {
spdlog::error(e.what());
g_stop_sched = true;
}
}
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: None" << std::endl;
std::cout << std::endl;
// clang-format on
}
int main(int argc, char* argv[])
{
// Parse Parameters
//==============================================================================================
if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
PrintHelp();
return 1;
}
// Serial number of the robot to connect to
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");
}
// 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");
// set mode after robot is operational
robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION);
// Direct joint control can only be executed by single-arm joint groups
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");
}
// Set initial joint positions
std::map<flexiv::rdk::JointGroup, std::vector<double>> all_init_pos;
const auto robot_states = robot.states();
for (const auto& [group, _] : single_arm_groups) {
all_init_pos[group] = robot_states.at(group).q;
spdlog::info("[{}] Initial joint positions: {}",
flexiv::rdk::kJointGroupNames.at(group),
flexiv::rdk::utility::Vec2Str(all_init_pos.at(group)));
}
spdlog::warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<");
// 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::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;
}