feat: add multi-tool frame arm grpc support

cmvr-es/common/types/arm/arm_types.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_ARM_TYPES_H
 
 #include <cstdint>
+#include <array>
 #include <string>
 #include <vector>
 
@@ -64,6 +65,31 @@ struct CartesianVelocity {
     double wz{0.0};
 };
 
+enum class ToolFrameSource {
+    Config,
+    Runtime,
+    Controller
+};
+
+struct ToolFrame {
+    std::string name;
+    std::array<double, 16> flange_T_tcp{
+        1.0, 0.0, 0.0, 0.0,
+        0.0, 1.0, 0.0, 0.0,
+        0.0, 0.0, 1.0, 0.0,
+        0.0, 0.0, 0.0, 1.0};
+    ToolFrameSource source{ToolFrameSource::Config};
+    bool is_default{false};
+};
+
+struct ToolFrameCapabilities {
+    bool named_move_l_supported{false};
+    bool named_speed_l_supported{false};
+    bool named_get_pose_supported{false};
+    bool get_supported{false};
+    bool add_supported{false};
+};
+
 struct CartesianWrench {
     double fx{0.0};
     double fy{0.0};

cmvr-es/devices/arm/CMakeLists.txt

@@ -15,3 +15,11 @@ target_link_libraries(robot_arm
 )
 
 add_library(cmvr_es::device::arm ALIAS robot_arm)
+
+if(BUILD_TESTING)
+    enable_testing()
+    add_executable(tool_frame_utils_test
+            tests/tool_frame_utils_test.cpp
+    )
+    add_test(NAME tool_frame_utils_test COMMAND tool_frame_utils_test)
+endif()

cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp

@@ -1,12 +1,20 @@
 #include "devices/arm/aubo_arm/aubo_arm.h"
 
 #include <algorithm>
+#include <cerrno>
+#include <cctype>
 #include <chrono>
+#include <cstring>
 #include <exception>
+#include <filesystem>
+#include <fstream>
+#include <fcntl.h>
 #include <thread>
 #include <tuple>
+#include <unistd.h>
 
 #include "common/base/logging/logger.h"
+#include "devices/arm/tool_frame_utils.h"
 
 #include "aubo_sdk/rpc.h"
 
@@ -122,6 +130,33 @@ CartesianPose poseFromVector(const std::vector<double>& values)
     return pose;
 }
 
+std::string fileSafeId(std::string id)
+{
+    for (char& ch : id) {
+        const auto value = static_cast<unsigned char>(ch);
+        if (!std::isalnum(value) && ch != '-' && ch != '_') {
+            ch = '_';
+        }
+    }
+    return id.empty() ? "arm" : id;
+}
+
+bool configMatrix(const config::ToolFrameConfig& config,
+                  std::array<double, 16>& matrix)
+{
+    if (config.flange_t_tcp_size() != static_cast<int>(matrix.size())) {
+        return false;
+    }
+    std::copy(config.flange_t_tcp().begin(), config.flange_t_tcp().end(), matrix.begin());
+    return true;
+}
+
+std::vector<double> toolOffset(const ToolFrame& tool_frame)
+{
+    const auto pose = transformMatrixToPose(tool_frame.flange_T_tcp);
+    return {pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz};
+}
+
 } // namespace
 
 struct AuboArm::SdkState {
@@ -140,6 +175,12 @@ AuboArm::AuboArm(const config::RobotArmConfig& cfg)
     port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004;
     username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username();
     password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password();
+    tool_frame_store_path_ = vendor_cfg_.tool_frame_store_path();
+    if (tool_frame_store_path_.empty()) {
+        tool_frame_store_path_ =
+            (std::filesystem::path("runtime") / "tool_frames" /
+             (fileSafeId(id_) + ".pb")).string();
+    }
 
     const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
     model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model();
@@ -153,6 +194,7 @@ AuboArm::AuboArm(const config::RobotArmConfig& cfg)
         CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_;
         model_.joint_names = defaultJointNames(dof);
     }
+    initializeToolFrames_();
 }
 
 AuboArm::~AuboArm()
@@ -472,8 +514,27 @@ Result AuboArm::stopJ(double acceleration)
 }
 
 Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
+{
+    return moveL(target, options, frame, "");
+}
+
+Result AuboArm::moveL(const CartesianPose& target,
+                      const MotionOptions& options,
+                      const FrameType frame,
+                      const std::string& tcp_frame_name)
 {
     (void)frame;
+    ToolFrame selected_tool;
+    if (!tcp_frame_name.empty()) {
+        const auto resolve_result = resolveToolFrame_(tcp_frame_name, selected_tool);
+        if (!resolve_result.ok()) {
+            return resolve_result;
+        }
+        if (frame == FrameType::World || frame == FrameType::User) {
+            return Result::failure(ArmErrorCode::UnsupportedCommand,
+                                   "[AuboArm] named moveL supports only Base and Tool frames");
+        }
+    }
     const auto ready = ensureConnected_("moveL");
     if (!ready.ok()) {
         return ready;
@@ -484,6 +545,7 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
     BusyGuard busy_guard{busy_};
 
     try {
+        std::lock_guard<std::mutex> lock(mutex_);
         const auto robot_names = sdk_->rpc_client->getRobotNames();
         if (robot_names.empty()) {
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
@@ -493,8 +555,15 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
             return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
         }
         robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
-        std::vector<double> tcp_offset(6, 0.0);
+        const auto tcp_offset = tcp_frame_name.empty()
-        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+            ? std::vector<double>(6, 0.0)
+            : toolOffset(selected_tool);
+        const int tcp_ret = robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+        if (tcp_ret != 0) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] moveL failed to activate tool frame '" +
+                                       tcp_frame_name + "': ret=" + std::to_string(tcp_ret));
+        }
         std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
         robot_interface->getMotionControl()->moveLine(
             pose,
@@ -513,6 +582,26 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
 
 Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
 {
+    return speedL(velocity, acceleration, duration, frame, "");
+}
+
+Result AuboArm::speedL(const CartesianVelocity& velocity,
+                       const double acceleration,
+                       const double duration,
+                       const FrameType frame,
+                       const std::string& tcp_frame_name)
+{
+    ToolFrame selected_tool;
+    if (!tcp_frame_name.empty()) {
+        const auto resolve_result = resolveToolFrame_(tcp_frame_name, selected_tool);
+        if (!resolve_result.ok()) {
+            return resolve_result;
+        }
+        if (frame == FrameType::World || frame == FrameType::User) {
+            return Result::failure(ArmErrorCode::UnsupportedCommand,
+                                   "[AuboArm] named speedL supports only Base and Tool frames");
+        }
+    }
     const auto ready = ensureConnected_("speedL");
     if (!ready.ok()) {
         return ready;
@@ -523,6 +612,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
     BusyGuard busy_guard{busy_};
 
     try {
+        std::lock_guard<std::mutex> lock(mutex_);
         Result interface_result;
         auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result);
         if (!interface_result.ok()) {
@@ -530,22 +620,27 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
         }
 
         robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
-        std::vector<double> tcp_offset(6, 0.0);
+        const auto tcp_offset = tcp_frame_name.empty()
-        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+            ? std::vector<double>(6, 0.0)
+            : toolOffset(selected_tool);
+        const int tcp_ret = robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+        if (tcp_ret != 0) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] speedL failed to activate tool frame '" +
+                                       tcp_frame_name + "': ret=" + std::to_string(tcp_ret));
+        }
 
-        std::vector<double> line_speed{velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0};
+        std::array<double, 3> line_speed{velocity.vx, velocity.vy, velocity.vz};
-        std::vector<double> angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
+        std::array<double, 3> angular_speed{velocity.wx, velocity.wy, velocity.wz};
         if (frame == FrameType::Tool) {
-            auto tool_frame = robot_interface->getRobotState()->getTcpPose();
+            const auto tcp_pose_values = robot_interface->getRobotState()->getTcpPose();
-            if (tool_frame.size() < 6) {
+            if (tcp_pose_values.size() < 6) {
                 return Result::failure(ArmErrorCode::CommandFailed,
                                        "[AuboArm] speedL failed: tcp pose size is less than 6");
             }
-            tool_frame[0] = 0.0;
+            const auto tcp_pose = poseFromVector(tcp_pose_values);
-            tool_frame[1] = 0.0;
+            line_speed = rotateVector(tcp_pose, line_speed);
-            tool_frame[2] = 0.0;
+            angular_speed = rotateVector(tcp_pose, angular_speed);
-            line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed);
-            angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed);
         } else if (frame == FrameType::User) {
             return Result::failure(ArmErrorCode::UnsupportedCommand,
                                    "[AuboArm] speedL User frame requires a configured user coordinate frame");
@@ -1031,6 +1126,265 @@ CartesianPose AuboArm::fk(bool is_tcp)
     return {};
 }
 
+Result AuboArm::getPose(const std::string& base_link,
+                        const std::string& ee_link,
+                        const std::string& tcp_frame_name,
+                        CartesianPose& pose,
+                        std::string& resolved_tcp_frame_name)
+{
+    if (tcp_frame_name.empty()) {
+        return RobotArm::getPose(base_link, ee_link, tcp_frame_name, pose,
+                                 resolved_tcp_frame_name);
+    }
+    if (!ee_link.empty()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[AuboArm] ee_link must be empty when tcp_frame_name is set");
+    }
+    const std::string configured_base = vendor_cfg_.base_frame().empty()
+        ? "base"
+        : vendor_cfg_.base_frame();
+    if (!base_link.empty() && base_link != configured_base) {
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "[AuboArm] named getPose supports only the configured base frame");
+    }
+
+    ToolFrame selected_tool;
+    auto result = resolveToolFrame_(tcp_frame_name, selected_tool);
+    if (!result.ok()) {
+        return result;
+    }
+    result = ensureConnected_("getPose");
+    if (!result.ok()) {
+        return result;
+    }
+
+    try {
+        std::lock_guard<std::mutex> lock(mutex_);
+        Result interface_result;
+        auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "getPose", interface_result);
+        if (!interface_result.ok()) {
+            return interface_result;
+        }
+        const auto q = robot_interface->getRobotState()->getJointPositions();
+        if (q.size() != model_.dof) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] getPose failed: joint state dof mismatch");
+        }
+        const auto fk_result = robot_interface->getRobotAlgorithm()->forwardKinematics1(
+            q, toolOffset(selected_tool));
+        const int ret = std::get<1>(fk_result);
+        if (ret != 0) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] getPose failed: ret=" + std::to_string(ret));
+        }
+        pose = poseFromVector(std::get<0>(fk_result));
+        resolved_tcp_frame_name = tcp_frame_name;
+        return Result::success();
+    } catch (const std::exception& e) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               std::string("[AuboArm] getPose failed: ") + e.what());
+    }
+}
+
+Result AuboArm::getToolFrames(std::vector<ToolFrame>& tool_frames,
+                              ToolFrameCapabilities& capabilities) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    tool_frames.clear();
+    tool_frames.reserve(tool_frames_.size());
+    for (const auto& entry : tool_frames_) {
+        tool_frames.push_back(entry.second);
+    }
+    capabilities = {true, true, true, true, true};
+    return Result::success();
+}
+
+Result AuboArm::addToolFrame(const ToolFrame& tool_frame)
+{
+    ToolFrame candidate = tool_frame;
+    candidate.source = ToolFrameSource::Runtime;
+    candidate.is_default = false;
+    const auto validation = validateToolFrame(candidate);
+    if (!validation.ok()) {
+        return validation;
+    }
+    if (busy_.load()) {
+        return Result::failure(ArmErrorCode::RobotNotReady,
+                               "[AuboArm] cannot add a tool frame while the arm is busy");
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    const auto existing = tool_frames_.find(candidate.name);
+    if (existing != tool_frames_.end()) {
+        if (toolFrameMatricesEqual(existing->second.flange_T_tcp, candidate.flange_T_tcp)) {
+            return Result::success();
+        }
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[AuboArm] tool frame already exists with a different transform: " +
+                                   candidate.name);
+    }
+
+    tool_frames_.emplace(candidate.name, candidate);
+    const auto persist_result = persistRuntimeToolFrames_();
+    if (!persist_result.ok()) {
+        tool_frames_.erase(candidate.name);
+        return persist_result;
+    }
+    return Result::success();
+}
+
+void AuboArm::initializeToolFrames_()
+{
+    ToolFrame identity;
+    identity.name = vendor_cfg_.tool_frame().empty() ? "tool0" : vendor_cfg_.tool_frame();
+    identity.source = ToolFrameSource::Config;
+    tool_frames_[identity.name] = identity;
+
+    for (const auto& configured : vendor_cfg_.tool_frames()) {
+        ToolFrame tool_frame;
+        tool_frame.name = configured.name();
+        tool_frame.source = ToolFrameSource::Config;
+        if (!configMatrix(configured, tool_frame.flange_T_tcp)) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring tool frame with non-4x4 matrix: "
+                            << configured.name();
+            continue;
+        }
+        const auto validation = validateToolFrame(tool_frame);
+        if (!validation.ok()) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid tool frame '" << configured.name()
+                            << "': " << validation.message;
+            continue;
+        }
+        tool_frames_[tool_frame.name] = tool_frame;
+    }
+
+    loadRuntimeToolFrames_();
+    default_tool_frame_name_ = vendor_cfg_.default_tool_frame().empty()
+        ? identity.name
+        : vendor_cfg_.default_tool_frame();
+    if (tool_frames_.find(default_tool_frame_name_) == tool_frames_.end()) {
+        CMVR_LOG(ERROR) << "[AuboArm] configured default tool frame does not exist: "
+                        << default_tool_frame_name_;
+        default_tool_frame_name_ = identity.name;
+    }
+    for (auto& entry : tool_frames_) {
+        entry.second.is_default = entry.first == default_tool_frame_name_;
+    }
+}
+
+void AuboArm::loadRuntimeToolFrames_()
+{
+    std::ifstream input(tool_frame_store_path_, std::ios::binary);
+    if (!input.good()) {
+        return;
+    }
+    config::ToolFrameStore store;
+    if (!store.ParseFromIstream(&input)) {
+        CMVR_LOG(ERROR) << "[AuboArm] failed to parse tool frame store: "
+                        << tool_frame_store_path_;
+        return;
+    }
+    for (const auto& stored : store.tool_frames()) {
+        ToolFrame tool_frame;
+        tool_frame.name = stored.name();
+        tool_frame.source = ToolFrameSource::Runtime;
+        if (!configMatrix(stored, tool_frame.flange_T_tcp) ||
+            !validateToolFrame(tool_frame).ok()) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid runtime tool frame: "
+                            << stored.name();
+            continue;
+        }
+        const auto existing = tool_frames_.find(tool_frame.name);
+        if (existing != tool_frames_.end()) {
+            if (!toolFrameMatricesEqual(existing->second.flange_T_tcp,
+                                        tool_frame.flange_T_tcp)) {
+                CMVR_LOG(ERROR) << "[AuboArm] runtime tool frame conflicts with configured frame: "
+                                << stored.name();
+            }
+            continue;
+        }
+        tool_frames_[tool_frame.name] = tool_frame;
+    }
+}
+
+Result AuboArm::persistRuntimeToolFrames_() const
+{
+    config::ToolFrameStore store;
+    for (const auto& entry : tool_frames_) {
+        if (entry.second.source != ToolFrameSource::Runtime) {
+            continue;
+        }
+        auto* stored = store.add_tool_frames();
+        stored->set_name(entry.second.name);
+        for (const double value : entry.second.flange_T_tcp) {
+            stored->add_flange_t_tcp(value);
+        }
+    }
+
+    std::string serialized;
+    if (!store.SerializeToString(&serialized)) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "[AuboArm] failed to serialize tool frame store");
+    }
+    const std::filesystem::path path(tool_frame_store_path_);
+    std::error_code filesystem_error;
+    if (path.has_parent_path()) {
+        std::filesystem::create_directories(path.parent_path(), filesystem_error);
+        if (filesystem_error) {
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] failed to create tool frame store directory: " +
+                                       filesystem_error.message());
+        }
+    }
+
+    const std::string temporary_path = tool_frame_store_path_ + ".tmp";
+    const int fd = ::open(temporary_path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, 0600);
+    if (fd < 0) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "[AuboArm] failed to open tool frame store: " +
+                                   std::string(std::strerror(errno)));
+    }
+    std::size_t written = 0;
+    while (written < serialized.size()) {
+        const auto count = ::write(fd, serialized.data() + written, serialized.size() - written);
+        if (count <= 0) {
+            const std::string error = std::strerror(errno);
+            ::close(fd);
+            ::unlink(temporary_path.c_str());
+            return Result::failure(ArmErrorCode::CommandFailed,
+                                   "[AuboArm] failed to write tool frame store: " + error);
+        }
+        written += static_cast<std::size_t>(count);
+    }
+    const int fsync_result = ::fsync(fd);
+    const int close_result = ::close(fd);
+    if (fsync_result != 0 || close_result != 0) {
+        const std::string error = std::strerror(errno);
+        ::unlink(temporary_path.c_str());
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "[AuboArm] failed to flush tool frame store: " + error);
+    }
+    if (::rename(temporary_path.c_str(), tool_frame_store_path_.c_str()) != 0) {
+        const std::string error = std::strerror(errno);
+        ::unlink(temporary_path.c_str());
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "[AuboArm] failed to replace tool frame store: " + error);
+    }
+    return Result::success();
+}
+
+Result AuboArm::resolveToolFrame_(const std::string& name, ToolFrame& tool_frame) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    const auto found = tool_frames_.find(name);
+    if (found == tool_frames_.end()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[AuboArm] unknown tool frame: " + name);
+    }
+    tool_frame = found->second;
+    return Result::success();
+}
+
 Result AuboArm::unsupported_(const std::string& name) const
 {
     const std::string message = "[AuboArm] " + name + " is not implemented";

cmvr-es/devices/arm/aubo_arm/aubo_arm.h

@@ -3,6 +3,7 @@
 
 #include <atomic>
 #include <memory>
+#include <map>
 #include <mutex>
 #include <optional>
 #include <string>
@@ -46,7 +47,11 @@ public:
     Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
     Result stopJ(double acceleration) override;
     Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
+    Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame,
+                 const std::string& tcp_frame_name) override;
     Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
+    Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame,
+                  const std::string& tcp_frame_name) override;
     Result stopL(std::optional<double> acceleration = std::nullopt) override;
     Result stopMotion() override;
 
@@ -77,6 +82,14 @@ public:
     std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override { return nullptr; }
     CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
     CartesianPose fk(bool is_tcp = true) override;
+    Result getPose(const std::string& base_link,
+                   const std::string& ee_link,
+                   const std::string& tcp_frame_name,
+                   CartesianPose& pose,
+                   std::string& resolved_tcp_frame_name) override;
+    Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                         ToolFrameCapabilities& capabilities) const override;
+    Result addToolFrame(const ToolFrame& tool_frame) override;
     CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; }
     bool busy() const override { return busy_.load(); }
 
@@ -84,6 +97,10 @@ private:
     Result unsupported_(const std::string& name) const;
     bool validDof_(std::size_t size, std::string& error) const;
     Result ensureConnected_(const std::string& context) const;
+    void initializeToolFrames_();
+    void loadRuntimeToolFrames_();
+    Result persistRuntimeToolFrames_() const;
+    Result resolveToolFrame_(const std::string& name, ToolFrame& tool_frame) const;
 
 #if defined(CMVR_HAS_AUBO_SDK)
     struct SdkState;
@@ -97,6 +114,9 @@ private:
     int port_{30004};
     std::string username_;
     std::string password_;
+    std::string default_tool_frame_name_;
+    std::string tool_frame_store_path_;
+    std::map<std::string, ToolFrame> tool_frames_;
     double speed_scaling_{1.0};
     ServoOptions servo_options_;
     std::atomic<bool> connected_{false};

cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp

@@ -7,6 +7,7 @@
 
 #include "huayan_arm/v1.0/include/HR_Pro.h"
 #include "common/base/logging/logger.h"
+#include "devices/arm/tool_frame_utils.h"
 
 namespace cmvr::device {
 namespace {
@@ -347,7 +348,18 @@ Result HuayanRobot::stopJ(const double acceleration)
 
 Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame)
 {
-    (void)frame;
+    return moveL(target, options, frame, "");
+}
+
+Result HuayanRobot::moveL(const CartesianPose& target,
+                          const MotionOptions& options,
+                          const FrameType frame,
+                          const std::string& tcp_frame_name)
+{
+    if (!tcp_frame_name.empty() && (frame == FrameType::World || frame == FrameType::User)) {
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "[HuayanRobot] named moveL supports only Base and Tool frames");
+    }
     const auto ready = ensureConnected_("moveL");
     if (!ready.ok()) {
         return ready;
@@ -356,6 +368,17 @@ Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& opti
         return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_);
     }
 
+    std::lock_guard<std::mutex> lock(mutex_);
+    const std::string selected_tcp = tcp_frame_name.empty() ? tcp_name_ : tcp_frame_name;
+    if (!tcp_frame_name.empty()) {
+        ToolFrame tool_frame;
+        const auto tool_result = readToolFrame_(selected_tcp, tool_frame);
+        if (!tool_result.ok()) {
+            busy_.store(false);
+            return tool_result;
+        }
+    }
+
     const auto pose = poseToHrCoord(target);
     const auto q_deg = toSix(currentJointPositionDeg_());
     const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm;
@@ -366,7 +389,7 @@ Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& opti
     const int ret = HRIF_MoveL(box_id_, robot_id_,
                               pose[0], pose[1], pose[2], pose[3], pose[4], pose[5],
                               q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5],
-                              tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend,
+                              selected_tcp, ucs_name_, velocity * speed_scaling_, acceleration, blend,
                               0, 0, 0, command_id);
     if (ret != 0) {
         busy_.store(false);
@@ -382,16 +405,61 @@ Result HuayanRobot::speedL(const CartesianVelocity& velocity,
                             const double duration,
                             const FrameType frame)
 {
+    return speedL(velocity, acceleration, duration, frame, "");
+}
+
+Result HuayanRobot::speedL(const CartesianVelocity& velocity,
+                            const double acceleration,
+                            const double duration,
+                            const FrameType frame,
+                            const std::string& tcp_frame_name)
+{
+    if (!tcp_frame_name.empty() && (frame == FrameType::World || frame == FrameType::User)) {
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "[HuayanRobot] named speedL supports only Base and Tool frames");
+    }
     const auto ready = ensureConnected_("speedL");
     if (!ready.ok()) {
         return ready;
     }
-    const double vx_mm = metersToMm(velocity.vx);
+    if (busy_.exchange(true)) {
-    const double vy_mm = metersToMm(velocity.vy);
+        return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_);
-    const double vz_mm = metersToMm(velocity.vz);
+    }
-    const double wx_deg = radToDeg(velocity.wx);
+
-    const double wy_deg = radToDeg(velocity.wy);
+    std::lock_guard<std::mutex> lock(mutex_);
-    const double wz_deg = radToDeg(velocity.wz);
+    std::array<double, 3> linear{velocity.vx, velocity.vy, velocity.vz};
+    std::array<double, 3> angular{velocity.wx, velocity.wy, velocity.wz};
+    if (!tcp_frame_name.empty()) {
+        ToolFrame tool_frame;
+        const auto tool_result = readToolFrame_(tcp_frame_name, tool_frame);
+        if (!tool_result.ok()) {
+            busy_.store(false);
+            return tool_result;
+        }
+        const auto activate_result = hrResult_(
+            HRIF_SetTCPByName(box_id_, robot_id_, tcp_frame_name), "SetTCPByName");
+        if (!activate_result.ok()) {
+            busy_.store(false);
+            return activate_result;
+        }
+        if (frame == FrameType::Tool) {
+            CartesianPose tcp_pose;
+            const auto pose_result = readPoseByTool_(tcp_frame_name, tcp_pose);
+            if (!pose_result.ok()) {
+                busy_.store(false);
+                return pose_result;
+            }
+            linear = rotateVector(tcp_pose, linear);
+            angular = rotateVector(tcp_pose, angular);
+        }
+    }
+
+    const double vx_mm = metersToMm(linear[0]);
+    const double vy_mm = metersToMm(linear[1]);
+    const double vz_mm = metersToMm(linear[2]);
+    const double wx_deg = radToDeg(angular[0]);
+    const double wy_deg = radToDeg(angular[1]);
+    const double wz_deg = radToDeg(angular[2]);
 
     const double linear_acc_mm =
         acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm;
@@ -401,7 +469,6 @@ Result HuayanRobot::speedL(const CartesianVelocity& velocity,
 
     const double runtime = duration > 0.0 ? duration : 0.5;
 
-    std::lock_guard<std::mutex> lock(mutex_);
     servo_mode_.store(false);
     const int ret = HRIF_SpeedL(box_id_, robot_id_, vx_mm, vy_mm, vz_mm,
         wx_deg, wy_deg, wz_deg, linear_acc_mm, angular_acc_deg, runtime);
@@ -624,6 +691,130 @@ CartesianPose HuayanRobot::fk(const bool is_tcp)
     return readTcpPose_();
 }
 
+Result HuayanRobot::getPose(const std::string& base_link,
+                            const std::string& ee_link,
+                            const std::string& tcp_frame_name,
+                            CartesianPose& pose,
+                            std::string& resolved_tcp_frame_name)
+{
+    if (tcp_frame_name.empty()) {
+        return RobotArm::getPose(base_link, ee_link, tcp_frame_name, pose,
+                                 resolved_tcp_frame_name);
+    }
+    if (!ee_link.empty()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[HuayanRobot] ee_link must be empty when tcp_frame_name is set");
+    }
+    if (!base_link.empty() && base_link != ucs_name_) {
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "[HuayanRobot] named getPose supports only the configured base frame");
+    }
+    const auto ready = ensureConnected_("getPose");
+    if (!ready.ok()) {
+        return ready;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    ToolFrame tool_frame;
+    const auto tool_result = readToolFrame_(tcp_frame_name, tool_frame);
+    if (!tool_result.ok()) {
+        return tool_result;
+    }
+    const auto pose_result = readPoseByTool_(tcp_frame_name, pose);
+    if (!pose_result.ok()) {
+        return pose_result;
+    }
+    resolved_tcp_frame_name = tcp_frame_name;
+    return Result::success();
+}
+
+Result HuayanRobot::getToolFrames(std::vector<ToolFrame>& tool_frames,
+                                  ToolFrameCapabilities& capabilities) const
+{
+    const auto ready = ensureConnected_("getToolFrames");
+    if (!ready.ok()) {
+        return ready;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::string> names;
+    const auto list_result = hrResult_(HRIF_ReadTCPList(box_id_, robot_id_, names),
+                                       "ReadTCPList");
+    if (!list_result.ok()) {
+        return list_result;
+    }
+    tool_frames.clear();
+    tool_frames.reserve(names.size());
+    for (const auto& name : names) {
+        ToolFrame tool_frame;
+        const auto read_result = readToolFrame_(name, tool_frame);
+        if (!read_result.ok()) {
+            return read_result;
+        }
+        tool_frames.push_back(std::move(tool_frame));
+    }
+    capabilities = {true, true, true, true, true};
+    return Result::success();
+}
+
+Result HuayanRobot::addToolFrame(const ToolFrame& tool_frame)
+{
+    ToolFrame candidate = tool_frame;
+    candidate.source = ToolFrameSource::Controller;
+    candidate.is_default = candidate.name == tcp_name_;
+    const auto validation = validateToolFrame(candidate);
+    if (!validation.ok()) {
+        return validation;
+    }
+    const auto ready = ensureConnected_("addToolFrame");
+    if (!ready.ok()) {
+        return ready;
+    }
+    if (busy_.load()) {
+        return Result::failure(ArmErrorCode::RobotNotReady,
+                               "[HuayanRobot] cannot add a tool frame while the arm is busy");
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::vector<std::string> names;
+    auto result = hrResult_(HRIF_ReadTCPList(box_id_, robot_id_, names), "ReadTCPList");
+    if (!result.ok()) {
+        return result;
+    }
+    if (std::find(names.begin(), names.end(), candidate.name) != names.end()) {
+        ToolFrame existing;
+        result = readToolFrame_(candidate.name, existing);
+        if (!result.ok()) {
+            return result;
+        }
+        if (toolFrameMatricesEqual(existing.flange_T_tcp, candidate.flange_T_tcp, 1e-6)) {
+            return Result::success();
+        }
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[HuayanRobot] tool frame already exists with a different transform: " +
+                                   candidate.name);
+    }
+
+    const auto pose = transformMatrixToPose(candidate.flange_T_tcp);
+    result = hrResult_(HRIF_ConfigTCP(box_id_, robot_id_, candidate.name,
+                                     metersToMm(pose.x), metersToMm(pose.y), metersToMm(pose.z),
+                                     radToDeg(pose.rx), radToDeg(pose.ry), radToDeg(pose.rz)),
+                       "ConfigTCP");
+    if (!result.ok()) {
+        return result;
+    }
+    ToolFrame stored;
+    result = readToolFrame_(candidate.name, stored);
+    if (!result.ok()) {
+        return result;
+    }
+    if (!toolFrameMatricesEqual(stored.flange_T_tcp, candidate.flange_T_tcp, 1e-6)) {
+        return Result::failure(ArmErrorCode::CommandFailed,
+                               "[HuayanRobot] controller returned a different tool frame transform after add");
+    }
+    return Result::success();
+}
+
 CartesianVelocity HuayanRobot::getSpeedLCommandTwistBase() const
 {
     return readTcpVelocity_();
@@ -815,6 +1006,57 @@ CartesianVelocity HuayanRobot::readTcpVelocity_() const
     return velocity;
 }
 
+Result HuayanRobot::readToolFrame_(const std::string& name, ToolFrame& tool_frame) const
+{
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+    double rx = 0.0;
+    double ry = 0.0;
+    double rz = 0.0;
+    const auto result = hrResult_(HRIF_ReadTCPByName(box_id_, robot_id_, name,
+                                                     x, y, z, rx, ry, rz),
+                                  "ReadTCPByName(" + name + ")");
+    if (!result.ok()) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "[HuayanRobot] unknown or unreadable tool frame: " + name +
+                                   "; " + result.message);
+    }
+    tool_frame.name = name;
+    tool_frame.flange_T_tcp = poseToTransformMatrix(
+        {mmToMeters(x), mmToMeters(y), mmToMeters(z),
+         degToRad(rx), degToRad(ry), degToRad(rz)});
+    tool_frame.source = ToolFrameSource::Controller;
+    tool_frame.is_default = name == tcp_name_;
+    return Result::success();
+}
+
+Result HuayanRobot::readPoseByTool_(const std::string& name, CartesianPose& pose) const
+{
+    double j1 = 0.0;
+    double j2 = 0.0;
+    double j3 = 0.0;
+    double j4 = 0.0;
+    double j5 = 0.0;
+    double j6 = 0.0;
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+    double rx = 0.0;
+    double ry = 0.0;
+    double rz = 0.0;
+    const auto result = hrResult_(HRIF_ReadActCoord(box_id_, robot_id_, ucs_name_, name,
+                                                    j1, j2, j3, j4, j5, j6,
+                                                    x, y, z, rx, ry, rz),
+                                  "ReadActCoord(" + name + ")");
+    if (!result.ok()) {
+        return result;
+    }
+    pose = {mmToMeters(x), mmToMeters(y), mmToMeters(z),
+            degToRad(rx), degToRad(ry), degToRad(rz)};
+    return Result::success();
+}
+
 std::vector<double> HuayanRobot::currentJointPositionDeg_() const
 {
     const auto q_rad = readJointPositionRad_();

cmvr-es/devices/arm/huayan_arm/huayan_arm.h

@@ -52,7 +52,11 @@ public:
     Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override;
     Result stopJ(double acceleration) override;
     Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
+    Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame,
+                 const std::string& tcp_frame_name) override;
     Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
+    Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame,
+                  const std::string& tcp_frame_name) override;
     Result stopL(const std::optional<double> acceleration) override;
     Result stopMotion() override;
 
@@ -83,6 +87,14 @@ public:
     std::shared_ptr<cmvr::IKSolver> kinematicsSolver() const override { return nullptr; }
     CartesianPose fk(const std::string& base_link, const std::string& ee_link) override;
     CartesianPose fk(bool is_tcp = true) override;
+    Result getPose(const std::string& base_link,
+                   const std::string& ee_link,
+                   const std::string& tcp_frame_name,
+                   CartesianPose& pose,
+                   std::string& resolved_tcp_frame_name) override;
+    Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                         ToolFrameCapabilities& capabilities) const override;
+    Result addToolFrame(const ToolFrame& tool_frame) override;
     CartesianVelocity getSpeedLCommandTwistBase() const override;
     bool busy() const override { return busy_.load(); }
 
@@ -113,6 +125,8 @@ private:
     std::vector<double> readJointVelocityRad_() const;
     CartesianPose readTcpPose_() const;
     CartesianVelocity readTcpVelocity_() const;
+    Result readToolFrame_(const std::string& name, ToolFrame& tool_frame) const;
+    Result readPoseByTool_(const std::string& name, CartesianPose& pose) const;
     std::vector<double> currentJointPositionDeg_() const;
     std::string nextCommandId_() const;
     Result waitMotionDone_(const std::string& context, int timeout_ms) const;
@@ -140,4 +154,4 @@ private:
 #endif // CMVR_ES_HUAYAN_ROBOT_H
 
 
-#endif //CMVR_ES_HUAYAN_ARM_H
+#endif //CMVR_ES_HUAYAN_ARM_H

cmvr-es/devices/arm/robot_arm.h

@@ -50,10 +50,33 @@ public:
     virtual Result moveL(const CartesianPose& target,
                          const MotionOptions& options,
                          FrameType frame = FrameType::Base) = 0;
+    virtual Result moveL(const CartesianPose& target,
+                         const MotionOptions& options,
+                         FrameType frame,
+                         const std::string& tcp_frame_name)
+    {
+        if (tcp_frame_name.empty()) {
+            return moveL(target, options, frame);
+        }
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "named tool frames are not supported by this arm");
+    }
     virtual Result speedL(const CartesianVelocity& velocity,
                           double acceleration,
                           double duration,
                           FrameType frame = FrameType::Base) = 0;
+    virtual Result speedL(const CartesianVelocity& velocity,
+                          double acceleration,
+                          double duration,
+                          FrameType frame,
+                          const std::string& tcp_frame_name)
+    {
+        if (tcp_frame_name.empty()) {
+            return speedL(velocity, acceleration, duration, frame);
+        }
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "named tool frames are not supported by this arm");
+    }
     virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
     virtual Result stopMotion() = 0;
 
@@ -94,6 +117,33 @@ public:
     virtual CartesianPose fk(const std::string& base_link,
                              const std::string& ee_link) = 0;
     virtual CartesianPose fk(bool is_tcp = true) = 0;
+    virtual Result getPose(const std::string& base_link,
+                           const std::string& ee_link,
+                           const std::string& tcp_frame_name,
+                           CartesianPose& pose,
+                           std::string& resolved_tcp_frame_name)
+    {
+        if (!tcp_frame_name.empty()) {
+            return Result::failure(ArmErrorCode::UnsupportedCommand,
+                                   "named tool frames are not supported by this arm");
+        }
+        pose = base_link.empty() || ee_link.empty() ? fk(true) : fk(base_link, ee_link);
+        resolved_tcp_frame_name.clear();
+        return Result::success();
+    }
+    virtual Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                                 ToolFrameCapabilities& capabilities) const
+    {
+        tool_frames.clear();
+        capabilities = {};
+        return Result::success();
+    }
+    virtual Result addToolFrame(const ToolFrame& tool_frame)
+    {
+        (void)tool_frame;
+        return Result::failure(ArmErrorCode::UnsupportedCommand,
+                               "adding tool frames is not supported by this arm");
+    }
     virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0;
     virtual bool busy() const = 0;
 };

cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp

@@ -0,0 +1,66 @@
+#include <cmath>
+#include <iostream>
+#include <string>
+
+#include "devices/arm/tool_frame_utils.h"
+
+namespace {
+
+bool expect(const bool condition, const std::string& message)
+{
+    if (!condition) {
+        std::cerr << message << '\n';
+    }
+    return condition;
+}
+
+bool near(const double lhs, const double rhs, const double tolerance = 1e-9)
+{
+    return std::abs(lhs - rhs) <= tolerance;
+}
+
+} // namespace
+
+int main()
+{
+    using namespace cmvr::device;
+    bool ok = true;
+
+    ToolFrame identity;
+    identity.name = "tool0";
+    ok &= expect(validateToolFrame(identity).ok(), "identity tool frame must be valid");
+
+    ToolFrame invalid_last_row = identity;
+    invalid_last_row.flange_T_tcp[15] = 0.0;
+    ok &= expect(!validateToolFrame(invalid_last_row).ok(),
+                 "non-homogeneous matrix last row must be rejected");
+
+    ToolFrame reflection = identity;
+    reflection.flange_T_tcp[0] = -1.0;
+    ok &= expect(!validateToolFrame(reflection).ok(),
+                 "reflection matrix must be rejected");
+
+    ToolFrame invalid_name = identity;
+    invalid_name.name = std::string("bad\nname");
+    ok &= expect(!validateToolFrame(invalid_name).ok(),
+                 "control characters in a tool frame name must be rejected");
+
+    const CartesianPose source_pose{0.12, -0.03, 0.45, 0.2, -0.3, 0.4};
+    const auto matrix = poseToTransformMatrix(source_pose);
+    const auto round_trip = transformMatrixToPose(matrix);
+    ok &= expect(near(round_trip.x, source_pose.x) &&
+                 near(round_trip.y, source_pose.y) &&
+                 near(round_trip.z, source_pose.z) &&
+                 near(round_trip.rx, source_pose.rx) &&
+                 near(round_trip.ry, source_pose.ry) &&
+                 near(round_trip.rz, source_pose.rz),
+                 "pose and transform matrix conversion must round-trip");
+
+    const CartesianPose quarter_turn{0.0, 0.0, 0.0, 0.0, 0.0,
+                                     3.14159265358979323846 / 2.0};
+    const auto rotated = rotateVector(quarter_turn, {1.0, 0.0, 0.0});
+    ok &= expect(near(rotated[0], 0.0) && near(rotated[1], 1.0) && near(rotated[2], 0.0),
+                 "tool-frame vector must rotate into the base frame");
+
+    return ok ? 0 : 1;
+}

cmvr-es/devices/arm/tool_frame_utils.h

@@ -0,0 +1,119 @@
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <string>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device {
+
+inline Result validateToolFrame(const ToolFrame& tool_frame)
+{
+    constexpr std::size_t kMaxNameLength = 128;
+    constexpr double kMatrixTolerance = 1e-6;
+    constexpr double kRotationTolerance = 1e-5;
+
+    if (tool_frame.name.empty() || tool_frame.name.size() > kMaxNameLength) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "tool frame name must contain 1 to 128 characters");
+    }
+    for (const unsigned char ch : tool_frame.name) {
+        if (ch < 0x20 || ch == 0x7f) {
+            return Result::failure(ArmErrorCode::InvalidArgument,
+                                   "tool frame name must not contain control characters");
+        }
+    }
+    for (const double value : tool_frame.flange_T_tcp) {
+        if (!std::isfinite(value)) {
+            return Result::failure(ArmErrorCode::InvalidArgument,
+                                   "tool frame matrix must contain only finite values");
+        }
+    }
+    const auto& m = tool_frame.flange_T_tcp;
+    if (std::abs(m[12]) > kMatrixTolerance || std::abs(m[13]) > kMatrixTolerance ||
+        std::abs(m[14]) > kMatrixTolerance || std::abs(m[15] - 1.0) > kMatrixTolerance) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "tool frame matrix last row must be [0, 0, 0, 1]");
+    }
+    for (std::size_t row = 0; row < 3; ++row) {
+        for (std::size_t col = 0; col < 3; ++col) {
+            double dot = 0.0;
+            for (std::size_t k = 0; k < 3; ++k) {
+                dot += m[k * 4 + row] * m[k * 4 + col];
+            }
+            const double expected = row == col ? 1.0 : 0.0;
+            if (std::abs(dot - expected) > kRotationTolerance) {
+                return Result::failure(ArmErrorCode::InvalidArgument,
+                                       "tool frame matrix rotation must be orthonormal");
+            }
+        }
+    }
+    const double determinant =
+        m[0] * (m[5] * m[10] - m[6] * m[9]) -
+        m[1] * (m[4] * m[10] - m[6] * m[8]) +
+        m[2] * (m[4] * m[9] - m[5] * m[8]);
+    if (std::abs(determinant - 1.0) > kRotationTolerance) {
+        return Result::failure(ArmErrorCode::InvalidArgument,
+                               "tool frame matrix rotation determinant must be 1");
+    }
+    return Result::success();
+}
+
+inline bool toolFrameMatricesEqual(const std::array<double, 16>& lhs,
+                                   const std::array<double, 16>& rhs,
+                                   const double tolerance = 1e-8)
+{
+    for (std::size_t i = 0; i < lhs.size(); ++i) {
+        if (std::abs(lhs[i] - rhs[i]) > tolerance) {
+            return false;
+        }
+    }
+    return true;
+}
+
+inline std::array<double, 16> poseToTransformMatrix(const CartesianPose& pose)
+{
+    const double cx = std::cos(pose.rx);
+    const double sx = std::sin(pose.rx);
+    const double cy = std::cos(pose.ry);
+    const double sy = std::sin(pose.ry);
+    const double cz = std::cos(pose.rz);
+    const double sz = std::sin(pose.rz);
+    return {
+        cz * cy, cz * sy * sx - sz * cx, cz * sy * cx + sz * sx, pose.x,
+        sz * cy, sz * sy * sx + cz * cx, sz * sy * cx - cz * sx, pose.y,
+        -sy, cy * sx, cy * cx, pose.z,
+        0.0, 0.0, 0.0, 1.0};
+}
+
+inline CartesianPose transformMatrixToPose(const std::array<double, 16>& matrix)
+{
+    CartesianPose pose;
+    pose.x = matrix[3];
+    pose.y = matrix[7];
+    pose.z = matrix[11];
+    pose.ry = std::asin(std::clamp(-matrix[8], -1.0, 1.0));
+    if (std::abs(std::cos(pose.ry)) > 1e-9) {
+        pose.rx = std::atan2(matrix[9], matrix[10]);
+        pose.rz = std::atan2(matrix[4], matrix[0]);
+    } else {
+        pose.rx = 0.0;
+        pose.rz = std::atan2(-matrix[1], matrix[5]);
+    }
+    return pose;
+}
+
+inline std::array<double, 3> rotateVector(const CartesianPose& pose,
+                                         const std::array<double, 3>& vector)
+{
+    const auto matrix = poseToTransformMatrix(pose);
+    return {
+        matrix[0] * vector[0] + matrix[1] * vector[1] + matrix[2] * vector[2],
+        matrix[4] * vector[0] + matrix[5] * vector[1] + matrix[6] * vector[2],
+        matrix[8] * vector[0] + matrix[9] * vector[1] + matrix[10] * vector[2]};
+}
+
+} // namespace cmvr::device

cmvr-es/service/grpc/include/grpc_arm_service.h

@@ -41,6 +41,12 @@ public:
     grpc::Status getPose(grpc::ServerContext* context,
                          const api::GetPose_Request* request,
                          api::GetPose_Response* response) override;
+    grpc::Status getToolFrames(grpc::ServerContext* context,
+                               const api::GetToolFrames_Request* request,
+                               api::GetToolFrames_Response* response) override;
+    grpc::Status addToolFrame(grpc::ServerContext* context,
+                              const api::AddToolFrame_Request* request,
+                              api::AddToolFrame_Response* response) override;
     grpc::Status calibrateZeroQ(grpc::ServerContext* context,
                                 const api::CalibrateZeroQ_Request* request,
                                 api::CalibrateZeroQ_Response* response) override;

cmvr-es/service/grpc/src/grpc_arm_service.cpp

@@ -1,5 +1,8 @@
 #include "service/grpc/include/grpc_arm_service.h"
 
+#include <algorithm>
+#include <array>
+
 #include <google/protobuf/util/time_util.h>
 
 #include "common/base/logging/logger.h"
@@ -97,6 +100,67 @@ device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src)
     return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()};
 }
 
+std::array<double, 16> toTransformMatrix(const api::TransformMatrix4x4& src)
+{
+    return {
+        src.m00(), src.m01(), src.m02(), src.m03(),
+        src.m10(), src.m11(), src.m12(), src.m13(),
+        src.m20(), src.m21(), src.m22(), src.m23(),
+        src.m30(), src.m31(), src.m32(), src.m33()};
+}
+
+void fillTransformMatrix(const std::array<double, 16>& src,
+                         api::TransformMatrix4x4* dst)
+{
+    dst->set_m00(src[0]);
+    dst->set_m01(src[1]);
+    dst->set_m02(src[2]);
+    dst->set_m03(src[3]);
+    dst->set_m10(src[4]);
+    dst->set_m11(src[5]);
+    dst->set_m12(src[6]);
+    dst->set_m13(src[7]);
+    dst->set_m20(src[8]);
+    dst->set_m21(src[9]);
+    dst->set_m22(src[10]);
+    dst->set_m23(src[11]);
+    dst->set_m30(src[12]);
+    dst->set_m31(src[13]);
+    dst->set_m32(src[14]);
+    dst->set_m33(src[15]);
+}
+
+api::ToolFrameSource toApiToolFrameSource(const device::ToolFrameSource source)
+{
+    switch (source) {
+        case device::ToolFrameSource::Runtime:
+            return api::TOOL_FRAME_SOURCE_RUNTIME;
+        case device::ToolFrameSource::Controller:
+            return api::TOOL_FRAME_SOURCE_CONTROLLER;
+        case device::ToolFrameSource::Config:
+        default:
+            return api::TOOL_FRAME_SOURCE_CONFIG;
+    }
+}
+
+void fillToolFrame(const device::ToolFrame& src, api::ToolFrame* dst)
+{
+    dst->set_name(src.name);
+    fillTransformMatrix(src.flange_T_tcp, dst->mutable_flange_t_tcp());
+    dst->set_source(toApiToolFrameSource(src.source));
+    dst->set_is_default(src.is_default);
+}
+
+void fillToolFrameCapabilities(const device::ToolFrameCapabilities& src,
+                               api::ToolFrameCapabilities* dst)
+{
+    dst->set_named_move_l_supported(src.named_move_l_supported);
+    dst->set_named_speed_l_supported(src.named_speed_l_supported);
+    dst->set_named_get_pose_supported(src.named_get_pose_supported);
+    dst->set_get_supported(src.get_supported);
+    dst->set_add_supported(src.add_supported);
+}
+
 template <typename Response>
 grpc::Status setResponseResult(Response* response, const device::Result& result)
 {
@@ -205,7 +269,8 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         }
         const auto result = arm->moveL(toCartesianPose(request->target()),
                                        toMotionOptions(request->options()),
-                                       toFrameType(request->frame()));
+                                       toFrameType(request->frame()),
+                                       request->tcp_frame_name());
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
                             << ", frame=" << request->frame();
@@ -256,7 +321,8 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
                                         request->duration(),
-                                        toFrameType(request->frame()));
+                                        toFrameType(request->frame()),
+                                        request->tcp_frame_name());
         if (result.ok()) {
             CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id
                             << ", acceleration=" << request->acceleration()
@@ -354,10 +420,18 @@ grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        const auto pose = request->base_link().empty() || request->ee_link().empty()
+        device::CartesianPose pose;
-            ? arm->fk(true)
+        std::string resolved_tcp_frame_name;
-            : arm->fk(request->base_link(), request->ee_link());
+        const auto result = arm->getPose(request->base_link(),
+                                         request->ee_link(),
+                                         request->tcp_frame_name(),
+                                         pose,
+                                         resolved_tcp_frame_name);
+        if (!result.ok()) {
+            return setResponseResult(response, result);
+        }
         *response->mutable_pose() = toApiCartesianPose(pose);
+        response->set_resolved_tcp_frame_name(resolved_tcp_frame_name);
         fillFeedback(response->mutable_header(), true);
         CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
                         << ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z
@@ -369,6 +443,82 @@ grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
     }
 }
 
+grpc::Status gRPCArmServiceImpl::getToolFrames(grpc::ServerContext*,
+                                               const api::GetToolFrames_Request* request,
+                                               api::GetToolFrames_Response* response)
+{
+    try {
+        const std::string device_id = request->header().device_id();
+        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
+        if (!arm) {
+            return setDeviceNotFound(response, device_id);
+        }
+
+        std::vector<device::ToolFrame> tool_frames;
+        device::ToolFrameCapabilities capabilities;
+        const auto result = arm->getToolFrames(tool_frames, capabilities);
+        if (!result.ok()) {
+            return setResponseResult(response, result);
+        }
+        for (const auto& tool_frame : tool_frames) {
+            fillToolFrame(tool_frame, response->add_tool_frames());
+        }
+        fillToolFrameCapabilities(capabilities, response->mutable_capabilities());
+        fillFeedback(response->mutable_header(), true);
+        logRpcSuccess("getToolFrames", device_id);
+        return grpc::Status::OK;
+    } catch (const std::exception& e) {
+        fillFeedback(response->mutable_header(), false, e.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+}
+
+grpc::Status gRPCArmServiceImpl::addToolFrame(grpc::ServerContext*,
+                                              const api::AddToolFrame_Request* request,
+                                              api::AddToolFrame_Response* response)
+{
+    try {
+        const std::string device_id = request->header().device_id();
+        auto arm = dmgr_.getDevice<device::RobotArm>(device_id);
+        if (!arm) {
+            return setDeviceNotFound(response, device_id);
+        }
+
+        device::ToolFrame requested_tool_frame;
+        requested_tool_frame.name = request->name();
+        requested_tool_frame.flange_T_tcp = toTransformMatrix(request->flange_t_tcp());
+        const auto result = arm->addToolFrame(requested_tool_frame);
+        if (!result.ok()) {
+            return setResponseResult(response, result);
+        }
+
+        std::vector<device::ToolFrame> tool_frames;
+        device::ToolFrameCapabilities capabilities;
+        const auto get_result = arm->getToolFrames(tool_frames, capabilities);
+        if (!get_result.ok()) {
+            return setResponseResult(response, get_result);
+        }
+        const auto added = std::find_if(
+            tool_frames.begin(), tool_frames.end(),
+            [&request](const device::ToolFrame& tool_frame) {
+                return tool_frame.name == request->name();
+            });
+        if (added == tool_frames.end()) {
+            const auto missing_result = device::Result::failure(
+                device::ArmErrorCode::CommandFailed,
+                "tool frame was not returned by the arm after it was added: " + request->name());
+            return setResponseResult(response, missing_result);
+        }
+        fillToolFrame(*added, response->mutable_tool_frame());
+        fillFeedback(response->mutable_header(), true);
+        logRpcSuccess("addToolFrame", device_id);
+        return grpc::Status::OK;
+    } catch (const std::exception& e) {
+        fillFeedback(response->mutable_header(), false, e.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
+    }
+}
+
 grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
                                                 const api::CalibrateZeroQ_Request* request,
                                                 api::CalibrateZeroQ_Response* response)
@@ -426,4 +576,3 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
     }
 }
 } // namespace cmvr::service
-

protos/cmvr/api/arm_command.proto

@@ -53,6 +53,29 @@ message TransformMatrix4x4 {
   double m30 = 13; double m31 = 14; double m32 = 15; double m33 = 16;
 }
 
+enum ToolFrameSource {
+  TOOL_FRAME_SOURCE_UNSPECIFIED = 0;
+  TOOL_FRAME_SOURCE_CONFIG = 1;
+  TOOL_FRAME_SOURCE_RUNTIME = 2;
+  TOOL_FRAME_SOURCE_CONTROLLER = 3;
+}
+
+message ToolFrame {
+  string name = 1;
+  // Homogeneous transform from flange to TCP. Translation is expressed in meters.
+  TransformMatrix4x4 flange_t_tcp = 2;
+  ToolFrameSource source = 3;
+  bool is_default = 4;
+}
+
+message ToolFrameCapabilities {
+  bool named_move_l_supported = 1;
+  bool named_speed_l_supported = 2;
+  bool named_get_pose_supported = 3;
+  bool get_supported = 4;
+  bool add_supported = 5;
+}
+
 message MoveJ {
   message Request {
     CommandHeader.Request header = 1;
@@ -71,6 +94,8 @@ message MoveL {
     CartesianPose target = 2;
     MotionOptions options = 3;
     ArmFrameType frame = 4;
+    // Empty preserves the arm's legacy/default TCP behavior.
+    string tcp_frame_name = 5;
   }
 
   message Response {
@@ -98,6 +123,8 @@ message SpeedL {
     double acceleration = 3;
     double duration = 4;
     ArmFrameType frame = 5;
+    // Empty preserves the arm's legacy/default TCP behavior.
+    string tcp_frame_name = 6;
   }
 
   message Response {
@@ -138,11 +165,40 @@ message GetPose {
     CommandHeader.Request header = 1;
     string base_link = 2;
     string ee_link = 3;
+    // When set, ee_link must be empty and the selected TCP is queried explicitly.
+    string tcp_frame_name = 4;
   }
 
   message Response {
     CommandHeader.Feedback header = 1;
     CartesianPose pose = 2;
+    string resolved_tcp_frame_name = 3;
+  }
+}
+
+message GetToolFrames {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Response {
+    CommandHeader.Feedback header = 1;
+    repeated ToolFrame tool_frames = 2;
+    ToolFrameCapabilities capabilities = 3;
+  }
+}
+
+message AddToolFrame {
+  message Request {
+    CommandHeader.Request header = 1;
+    string name = 2;
+    // Homogeneous transform from flange to TCP. Translation is expressed in meters.
+    TransformMatrix4x4 flange_t_tcp = 3;
+  }
+
+  message Response {
+    CommandHeader.Feedback header = 1;
+    ToolFrame tool_frame = 2;
   }
 }
 

protos/cmvr/api/arm_service.proto

@@ -17,6 +17,8 @@ service ArmService {
   rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback);
   rpc getJointState(JointRequest) returns (JointResponse);
   rpc getPose(GetPose.Request) returns (GetPose.Response);
+  rpc getToolFrames(GetToolFrames.Request) returns (GetToolFrames.Response);
+  rpc addToolFrame(AddToolFrame.Request) returns (AddToolFrame.Response);
   rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
   rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);
   rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response);

protos/cmvr/config/arm_config/arm_config.proto

@@ -32,6 +32,16 @@ enum VendorRobotArmBrand {
   VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM = 2;
 }
 
+message ToolFrameConfig {
+  string name = 1;
+  // Row-major homogeneous flange_T_tcp matrix. Translation is expressed in meters.
+  repeated double flange_t_tcp = 2 [packed = true];
+}
+
+message ToolFrameStore {
+  repeated ToolFrameConfig tool_frames = 1;
+}
+
 message VendorRobotArmBackendConfig {
   VendorRobotArmBrand brand = 1;
   string ip = 2;
@@ -43,6 +53,9 @@ message VendorRobotArmBackendConfig {
   string tool_frame = 8;
   string username = 9;
   string password = 10;
+  repeated ToolFrameConfig tool_frames = 11;
+  string default_tool_frame = 12;
+  string tool_frame_store_path = 13;
 }
 
 message SpeedLPlannerConfig {