feat: add vendor arm tool frame support

cmvr-es/common/math/tool_frame_math.h

@@ -0,0 +1,109 @@
+#ifndef CMVR_ES_COMMON_MATH_TOOL_FRAME_MATH_H
+#define CMVR_ES_COMMON_MATH_TOOL_FRAME_MATH_H
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+
+#include <Eigen/Dense>
+
+#include "common/math/transform_math.h"
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::common::math {
+
+inline Eigen::Matrix4d toolFrameMatrix(const device::ToolFrame& tool_frame)
+{
+    Eigen::Matrix4d matrix;
+    for (Eigen::Index row = 0; row < 4; ++row) {
+        for (Eigen::Index column = 0; column < 4; ++column) {
+            matrix(row, column) = tool_frame.flange_to_tcp[
+                static_cast<std::size_t>(row * 4 + column)];
+        }
+    }
+    return matrix;
+}
+
+inline device::ToolFrame toolFrameFromPose(const std::string& name,
+                                           const device::CartesianPose& pose,
+                                           const bool is_default,
+                                           const std::string& source)
+{
+    device::ToolFrame tool_frame;
+    tool_frame.name = name;
+    tool_frame.is_default = is_default;
+    tool_frame.source = source;
+    const Eigen::Matrix4d matrix = poseToMatrix(pose);
+    for (Eigen::Index row = 0; row < 4; ++row) {
+        for (Eigen::Index column = 0; column < 4; ++column) {
+            tool_frame.flange_to_tcp[
+                static_cast<std::size_t>(row * 4 + column)] =
+                matrix(row, column);
+        }
+    }
+    return tool_frame;
+}
+
+inline device::CartesianPose toolFramePose(const device::ToolFrame& tool_frame)
+{
+    return matrixToPose(toolFrameMatrix(tool_frame));
+}
+
+inline bool validateToolFrame(const device::ToolFrame& tool_frame,
+                              std::string& error)
+{
+    if (tool_frame.name.empty() || tool_frame.name.size() > 128U) {
+        error = "Tool Frame name must contain 1 to 128 bytes";
+        return false;
+    }
+    if (!std::all_of(
+            tool_frame.name.begin(), tool_frame.name.end(),
+            [](const unsigned char value) {
+                return value >= 0x20U && value != 0x7fU;
+            })) {
+        error = "Tool Frame name must not contain control characters";
+        return false;
+    }
+    if (!std::all_of(
+            tool_frame.flange_to_tcp.begin(),
+            tool_frame.flange_to_tcp.end(),
+            [](const double value) { return std::isfinite(value); })) {
+        error = "Tool Frame transform must contain only finite values";
+        return false;
+    }
+
+    const Eigen::Matrix4d matrix = toolFrameMatrix(tool_frame);
+    constexpr double kMatrixTolerance = 1e-6;
+    if ((matrix.row(3) - Eigen::RowVector4d(0.0, 0.0, 0.0, 1.0)).norm() >
+        kMatrixTolerance) {
+        error = "Tool Frame transform last row must be [0, 0, 0, 1]";
+        return false;
+    }
+    const Eigen::Matrix3d rotation = matrix.block<3, 3>(0, 0);
+    if ((rotation.transpose() * rotation - Eigen::Matrix3d::Identity()).norm() >
+        kMatrixTolerance) {
+        error = "Tool Frame rotation must be orthonormal";
+        return false;
+    }
+    if (std::abs(rotation.determinant() - 1.0) > kMatrixTolerance) {
+        error = "Tool Frame rotation determinant must be +1";
+        return false;
+    }
+    return true;
+}
+
+inline bool sameToolTransform(const device::ToolFrame& lhs,
+                              const device::ToolFrame& rhs,
+                              const double tolerance = 1e-9)
+{
+    for (std::size_t i = 0; i < lhs.flange_to_tcp.size(); ++i) {
+        if (std::abs(lhs.flange_to_tcp[i] - rhs.flange_to_tcp[i]) > tolerance) {
+            return false;
+        }
+    }
+    return true;
+}
+
+} // namespace cmvr::common::math
+
+#endif // CMVR_ES_COMMON_MATH_TOOL_FRAME_MATH_H

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

@@ -1,6 +1,7 @@
 #ifndef CMVR_ES_ARM_TYPES_H
 #define CMVR_ES_ARM_TYPES_H
 
+#include <array>
 #include <cstdint>
 #include <functional>
 #include <string>
@@ -159,6 +160,25 @@ struct ToolConfig {
     CartesianPose tcp_offset;
 };
 
+struct ToolFrame {
+    std::string name;
+    std::array<double, 16> flange_to_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};
+    bool is_default{false};
+    std::string source;
+};
+
+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 PayloadConfig {
     double mass{0.0};
     double cog_x{0.0};

cmvr-es/devices/arm/aubo_arm/CMakeLists.txt

@@ -79,3 +79,10 @@ target_link_libraries(aubo_arm
 
 add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm)
 install(TARGETS aubo_arm LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+    add_executable(aubo_tool_frame_test aubo_tool_frame_test.cpp)
+    target_link_libraries(aubo_tool_frame_test PRIVATE aubo_arm)
+    add_test(NAME aubo_tool_frame_test COMMAND aubo_tool_frame_test)
+    set_tests_properties(aubo_tool_frame_test PROPERTIES TIMEOUT 10)
+endif()

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

@@ -11,12 +11,15 @@
 #include <condition_variable>
 #include <cstring>
 #include <exception>
+#include <filesystem>
+#include <fstream>
 #include <mutex>
 #include <thread>
 #include <tuple>
 #include <utility>
 
 #include "common/base/logging/logger.h"
+#include "common/math/tool_frame_math.h"
 #include "json/json.h"
 
 #include "aubo_sdk/rpc.h"
@@ -1358,6 +1361,12 @@ CartesianPose poseFromVector(const std::vector<double>& values)
     return pose;
 }
 
+std::vector<double> toolOffsetVector(const ToolFrame& tool_frame)
+{
+    const auto pose = common::math::toolFramePose(tool_frame);
+    return {pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz};
+}
+
 } // namespace
 
 struct AuboArm::SdkState {
@@ -1404,6 +1413,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()
@@ -1411,6 +1421,253 @@ AuboArm::~AuboArm()
     (void)disconnect();
 }
 
+void AuboArm::initializeToolFrames_()
+{
+    default_tool_frame_name_ = vendor_cfg_.default_tool_frame().empty()
+        ? vendor_cfg_.tool_frame()
+        : vendor_cfg_.default_tool_frame();
+    if (default_tool_frame_name_.empty()) {
+        default_tool_frame_name_ = "tool0";
+    }
+    tool_frame_store_path_ = vendor_cfg_.tool_frame_store_path();
+
+    tool_frames_.clear();
+    tool_frames_.emplace(
+        default_tool_frame_name_,
+        common::math::toolFrameFromPose(
+            default_tool_frame_name_, {}, true, "legacy_config"));
+    for (const auto& configured : vendor_cfg_.tool_frames()) {
+        CartesianPose pose{
+            configured.x(), configured.y(), configured.z(),
+            configured.rx(), configured.ry(), configured.rz()};
+        auto tool_frame = common::math::toolFrameFromPose(
+            configured.name(), pose,
+            configured.name() == default_tool_frame_name_, "config");
+        std::string validation_error;
+        if (!common::math::validateToolFrame(tool_frame, validation_error)) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid configured Tool Frame: "
+                            << validation_error << ", id=" << id_;
+            continue;
+        }
+        tool_frames_[tool_frame.name] = std::move(tool_frame);
+    }
+
+    if (tool_frame_store_path_.empty()) {
+        return;
+    }
+    std::ifstream input(tool_frame_store_path_);
+    if (!input.good()) {
+        return;
+    }
+
+    Json::Value root;
+    Json::CharReaderBuilder reader;
+    std::string parse_error;
+    const bool parsed = Json::parseFromStream(
+        reader, input, &root, &parse_error);
+    const Json::Value* stored_tool_frames = parsed
+        ? findJsonMember(root, "tool_frames")
+        : nullptr;
+    if (!parsed || !stored_tool_frames || !stored_tool_frames->isArray()) {
+        CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid Tool Frame store: "
+                        << tool_frame_store_path_ << ", error=" << parse_error;
+        return;
+    }
+
+    std::map<std::string, ToolFrame> stored_frames;
+    for (const auto& item : *stored_tool_frames) {
+        ToolFrame tool_frame;
+        const Json::Value* stored_name = findJsonMember(item, "name");
+        const Json::Value* stored_matrix = findJsonMember(
+            item, "flange_to_tcp");
+        tool_frame.name = stored_name ? stored_name->asString() : std::string();
+        tool_frame.source = "aubo_store";
+        if (!stored_matrix || !stored_matrix->isArray() ||
+            stored_matrix->size() != 16U) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid Tool Frame store entry, name="
+                            << tool_frame.name;
+            return;
+        }
+        for (Json::ArrayIndex i = 0; i < stored_matrix->size(); ++i) {
+            tool_frame.flange_to_tcp[i] = (*stored_matrix)[i].asDouble();
+        }
+        std::string validation_error;
+        if (!common::math::validateToolFrame(tool_frame, validation_error)) {
+            CMVR_LOG(ERROR) << "[AuboArm] ignoring invalid Tool Frame store: "
+                            << validation_error;
+            return;
+        }
+        stored_frames[tool_frame.name] = std::move(tool_frame);
+    }
+    if (stored_frames.find(default_tool_frame_name_) == stored_frames.end()) {
+        CMVR_LOG(ERROR) << "[AuboArm] ignoring Tool Frame store without default frame: "
+                        << default_tool_frame_name_;
+        return;
+    }
+    for (auto& [name, tool_frame] : stored_frames) {
+        tool_frame.is_default = name == default_tool_frame_name_;
+    }
+    tool_frames_ = std::move(stored_frames);
+    const Json::Value* stored_revision = findJsonMember(root, "revision");
+    if (stored_revision && stored_revision->isUInt64() &&
+        stored_revision->asUInt64() > 0U) {
+        tool_frames_revision_ = stored_revision->asUInt64();
+    }
+}
+
+bool AuboArm::persistToolFramesLocked_(
+    const std::map<std::string, ToolFrame>& tool_frames,
+    const std::uint64_t revision,
+    std::string& error) const
+{
+    if (tool_frame_store_path_.empty()) {
+        error = "AUBO tool_frame_store_path is not configured";
+        return false;
+    }
+    try {
+        const std::filesystem::path store_path(tool_frame_store_path_);
+        if (store_path.has_parent_path()) {
+            std::filesystem::create_directories(store_path.parent_path());
+        }
+        const auto temporary_path = store_path.string() + ".tmp";
+        Json::Value root(Json::objectValue);
+        jsonMember(root, "revision") = Json::UInt64(revision);
+        jsonMember(root, "default_tool_frame") = default_tool_frame_name_;
+        Json::Value frames(Json::arrayValue);
+        for (const auto& [name, tool_frame] : tool_frames) {
+            Json::Value item(Json::objectValue);
+            jsonMember(item, "name") = name;
+            Json::Value matrix(Json::arrayValue);
+            for (const double value : tool_frame.flange_to_tcp) {
+                matrix.append(value);
+            }
+            jsonMember(item, "flange_to_tcp") = std::move(matrix);
+            frames.append(std::move(item));
+        }
+        jsonMember(root, "tool_frames") = std::move(frames);
+
+        std::ofstream output(temporary_path, std::ios::out | std::ios::trunc);
+        if (!output.is_open()) {
+            error = "could not open temporary Tool Frame store";
+            return false;
+        }
+        Json::StreamWriterBuilder writer;
+        writer[std::string("indentation")] = "  ";
+        output << Json::writeString(writer, root) << '\n';
+        output.flush();
+        if (!output.good()) {
+            error = "could not write temporary Tool Frame store";
+            output.close();
+            std::error_code remove_error;
+            std::filesystem::remove(temporary_path, remove_error);
+            return false;
+        }
+        output.close();
+        std::filesystem::rename(temporary_path, store_path);
+        return true;
+    } catch (const std::exception& exception) {
+        error = exception.what();
+        return false;
+    }
+}
+
+Result AuboArm::resolveToolFrame_(const std::string& requested_name,
+                                  ToolFrame& tool_frame) const
+{
+    std::lock_guard<std::mutex> lock(tool_frames_mutex_);
+    const std::string& name = requested_name.empty()
+        ? default_tool_frame_name_
+        : requested_name;
+    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::getToolFrames(std::vector<ToolFrame>& tool_frames,
+                              ToolFrameCapabilities& capabilities,
+                              std::uint64_t& revision) const
+{
+    std::lock_guard<std::mutex> lock(tool_frames_mutex_);
+    tool_frames.clear();
+    tool_frames.reserve(tool_frames_.size());
+    for (const auto& [name, tool_frame] : tool_frames_) {
+        (void)name;
+        tool_frames.push_back(tool_frame);
+    }
+    capabilities.named_move_l_supported = true;
+    capabilities.named_speed_l_supported = true;
+    capabilities.named_get_pose_supported = true;
+    capabilities.get_supported = true;
+    capabilities.add_supported = !tool_frame_store_path_.empty();
+    revision = tool_frames_revision_;
+    return Result::success();
+}
+
+Result AuboArm::addToolFrame(
+    const ToolFrame& requested_tool_frame,
+    const std::optional<std::uint64_t> expected_revision,
+    std::uint64_t& revision)
+{
+    std::string validation_error;
+    if (!common::math::validateToolFrame(requested_tool_frame, validation_error)) {
+        return Result::failure(
+            ArmErrorCode::InvalidArgument,
+            "[AuboArm] invalid Tool Frame: " + validation_error);
+    }
+
+    std::lock_guard<std::mutex> submission_lock(mutex_);
+    if (busy_.load() || (sdk_ && sdk_->motion_state->busy())) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "[AuboArm] cannot add a Tool Frame while motion is active");
+    }
+    std::lock_guard<std::mutex> registry_lock(tool_frames_mutex_);
+    revision = tool_frames_revision_;
+    if (expected_revision.has_value() && *expected_revision != revision) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[AuboArm] Tool Frame revision mismatch");
+    }
+    const auto existing = tool_frames_.find(requested_tool_frame.name);
+    if (existing != tool_frames_.end()) {
+        if (common::math::sameToolTransform(existing->second, requested_tool_frame)) {
+            return Result::success();
+        }
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[AuboArm] Tool Frame already exists with a different transform: " +
+                requested_tool_frame.name);
+    }
+    if (tool_frame_store_path_.empty()) {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "[AuboArm] adding Tool Frames requires tool_frame_store_path");
+    }
+
+    auto updated_frames = tool_frames_;
+    auto stored_tool_frame = requested_tool_frame;
+    stored_tool_frame.is_default = false;
+    stored_tool_frame.source = "aubo_store";
+    updated_frames.emplace(stored_tool_frame.name, std::move(stored_tool_frame));
+    const std::uint64_t updated_revision = tool_frames_revision_ + 1U;
+    std::string persistence_error;
+    if (!persistToolFramesLocked_(
+            updated_frames, updated_revision, persistence_error)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "[AuboArm] failed to persist Tool Frames: " + persistence_error);
+    }
+    tool_frames_ = std::move(updated_frames);
+    tool_frames_revision_ = updated_revision;
+    revision = updated_revision;
+    return Result::success();
+}
+
 bool AuboArm::init()
 {
     if (ip_.empty()) {
@@ -1674,6 +1931,57 @@ CartesianPose AuboArm::getTcpPose(FrameType frame) const
     return pose;
 }
 
+Result AuboArm::getTcpPose(const std::string& tcp_frame_name,
+                           const FrameType frame,
+                           CartesianPose& pose) const
+{
+    if (tcp_frame_name.empty()) {
+        pose = getTcpPose(frame);
+        return Result::success();
+    }
+    if (frame != FrameType::Base) {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "[AuboArm] named Tool Frame pose is only available in Base frame");
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    const auto ready = ensureConnected_("getTcpPose");
+    if (!ready.ok()) {
+        return ready;
+    }
+    ToolFrame tool_frame;
+    const auto resolved = resolveToolFrame_(tcp_frame_name, tool_frame);
+    if (!resolved.ok()) {
+        return resolved;
+    }
+    try {
+        Result interface_result;
+        auto robot_interface = getPrimaryRobotInterface(
+            sdk_->rpc_client, "getTcpPose", interface_result);
+        if (!interface_result.ok()) {
+            return interface_result;
+        }
+        const auto joints = robot_interface->getRobotState()->getJointPositions();
+        const auto fk_result = robot_interface->getRobotAlgorithm()
+            ->forwardKinematics1(joints, toolOffsetVector(tool_frame));
+        const auto& values = std::get<0>(fk_result);
+        const int error_code = std::get<1>(fk_result);
+        if (error_code != arcs::common_interface::AUBO_OK || values.size() < 6U) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] named getTcpPose forward kinematics failed: ret=" +
+                    std::to_string(error_code));
+        }
+        pose = poseFromVector(values);
+        return Result::success();
+    } catch (const std::exception& exception) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            std::string("[AuboArm] named getTcpPose failed: ") + exception.what());
+    }
+}
+
 RobotMode AuboArm::getRobotMode() const
 {
     if (!connected_.load()) {
@@ -2495,6 +2803,14 @@ 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;
     try {
@@ -2503,6 +2819,12 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
         if (!locked_ready.ok()) {
             return locked_ready;
         }
+        ToolFrame selected_tool_frame;
+        const auto tool_result = resolveToolFrame_(
+            tcp_frame_name, selected_tool_frame);
+        if (!tool_result.ok()) {
+            return tool_result;
+        }
         std::uint64_t safety_epoch = 0;
         const auto safety_ready = ensureMotionReady_(
             "moveL", safety_epoch);
@@ -2536,12 +2858,22 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
         }
         auto motion_control = robot_interface->getMotionControl();
         motion_control->setSpeedFraction(speed_scaling_);
-        std::vector<double> tcp_offset(6, 0.0);
+        auto robot_config = robot_interface->getRobotConfig();
-        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+        const auto previous_tcp_offset = robot_config->getTcpOffset();
+        const auto tcp_offset = toolOffsetVector(selected_tool_frame);
+        const int tcp_ret = robot_config->setTcpOffset(tcp_offset);
+        if (tcp_ret != arcs::common_interface::AUBO_OK) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] moveL failed to select Tool Frame " +
+                    selected_tool_frame.name + ": ret=" +
+                    std::to_string(tcp_ret));
+        }
         std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
         motion_owner.requireExplicitSettlement();
         if (cancellationRequested(options.cancellation_requested) ||
             !validateSafetyPermit(safety_monitor, safety_permit)) {
+            (void)robot_config->setTcpOffset(previous_tcp_offset);
             motion_owner.settle();
             return Result::failure(
                 ArmErrorCode::CommandRejected,
@@ -2556,6 +2888,7 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
         if (ret == arcs::common_interface::AUBO_OK) {
             submit_lock.unlock();
         } else {
+            (void)robot_config->setTcpOffset(previous_tcp_offset);
             motion_owner.settle();
         }
         const auto outcome = aubo_internal::resolveMotionCommand(
@@ -2634,6 +2967,15 @@ 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)
 {
     try {
         std::unique_lock submit_lock(mutex_);
@@ -2641,6 +2983,12 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
         if (!locked_ready.ok()) {
             return locked_ready;
         }
+        ToolFrame selected_tool_frame;
+        const auto tool_result = resolveToolFrame_(
+            tcp_frame_name, selected_tool_frame);
+        if (!tool_result.ok()) {
+            return tool_result;
+        }
         std::uint64_t safety_epoch = 0;
         const auto safety_ready = ensureMotionReady_(
             "speedL", safety_epoch);
@@ -2672,14 +3020,24 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
         }
 
         robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
-        std::vector<double> tcp_offset(6, 0.0);
+        auto robot_config = robot_interface->getRobotConfig();
-        robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
+        const auto previous_tcp_offset = robot_config->getTcpOffset();
+        const int tcp_ret = robot_config->setTcpOffset(
+            toolOffsetVector(selected_tool_frame));
+        if (tcp_ret != arcs::common_interface::AUBO_OK) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] speedL failed to select Tool Frame " +
+                    selected_tool_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::vector<double> angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
         if (frame == FrameType::Tool) {
             auto tool_frame = robot_interface->getRobotState()->getTcpPose();
             if (tool_frame.size() < 6) {
+                (void)robot_config->setTcpOffset(previous_tcp_offset);
                 return Result::failure(ArmErrorCode::CommandFailed,
                                        "[AuboArm] speedL failed: tcp pose size is less than 6");
             }
@@ -2691,6 +3049,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
             angular_speed = rpc_client->getMath()->poseTrans(
                 tool_frame, angular_speed);
         } else if (frame == FrameType::User) {
+            (void)robot_config->setTcpOffset(previous_tcp_offset);
             return Result::failure(ArmErrorCode::UnsupportedCommand,
                                    "[AuboArm] speedL User frame requires a configured user coordinate frame");
         }
@@ -2707,14 +3066,15 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
         const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2;
         const double resolved_duration = duration > 0.0 ? duration : 100.0;
         motion_owner.requireExplicitSettlement();
-        submit_lock.unlock();
         if (motion_state->cancelled(motion.token) ||
             !validateSafetyPermit(safety_monitor, safety_permit)) {
+            (void)robot_config->setTcpOffset(previous_tcp_offset);
             motion_owner.settle();
             return Result::failure(
                 ArmErrorCode::CommandRejected,
                 "[AuboArm] speedL cancelled before submission");
         }
+        submit_lock.unlock();
         const int ret = robot_interface->getMotionControl()->speedLine(
             speed,
             resolved_acceleration,
@@ -2727,6 +3087,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
                 "[AuboArm] speedL cancelled by stopMotion or hardware safety event");
         }
         if (ret != 0) {
+            (void)robot_config->setTcpOffset(previous_tcp_offset);
             motion_owner.settle();
             return Result::failure(ArmErrorCode::CommandFailed,
                                    "[AuboArm] speedL failed: ret=" + std::to_string(ret));

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

@@ -5,6 +5,7 @@
 #include <cstdint>
 #include <functional>
 #include <memory>
+#include <map>
 #include <mutex>
 #include <optional>
 #include <string>
@@ -31,6 +32,9 @@ public:
     ArmState getRobotState() const override;
     JointGroupState getJointState() const override;
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
+    Result getTcpPose(const std::string& tcp_frame_name,
+                      FrameType frame,
+                      CartesianPose& pose) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override;
@@ -53,9 +57,24 @@ 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;
+    Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                         ToolFrameCapabilities& capabilities,
+                         std::uint64_t& revision) const override;
+    Result addToolFrame(const ToolFrame& tool_frame,
+                        std::optional<std::uint64_t> expected_revision,
+                        std::uint64_t& revision) override;
 
     Result startServoMode(const ServoOptions& options) override;
     Result servoJ(const JointPositionCommand& target) override;
@@ -104,6 +123,13 @@ private:
     Result unlockProtectiveStop_(
         std::optional<std::uint64_t> expected_safety_epoch);
     Result stopMotion_(MotionStopKind kind, double acceleration);
+    Result resolveToolFrame_(const std::string& requested_name,
+                             ToolFrame& tool_frame) const;
+    bool persistToolFramesLocked_(
+        const std::map<std::string, ToolFrame>& tool_frames,
+        std::uint64_t revision,
+        std::string& error) const;
+    void initializeToolFrames_();
 
     struct SdkState;
 
@@ -115,6 +141,8 @@ private:
     int port_{30004};
     std::string username_;
     std::string password_;
+    std::string default_tool_frame_name_;
+    std::string tool_frame_store_path_;
     double speed_scaling_{1.0};
     ServoOptions servo_options_;
     std::atomic<bool> connected_{false};
@@ -122,6 +150,9 @@ private:
     std::atomic<bool> servo_mode_{false};
     std::atomic<bool> emergency_stopped_{false};
     mutable std::mutex mutex_;
+    mutable std::mutex tool_frames_mutex_;
+    std::map<std::string, ToolFrame> tool_frames_;
+    std::uint64_t tool_frames_revision_{1};
 
     std::unique_ptr<SdkState> sdk_;
 };

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

@@ -0,0 +1,87 @@
+#include <algorithm>
+#include <filesystem>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include <unistd.h>
+
+#include "common/math/tool_frame_math.h"
+#include "devices/arm/aubo_arm/aubo_arm.h"
+
+namespace {
+
+bool check(const bool condition, const std::string& message)
+{
+    if (!condition) {
+        std::cerr << message << '\n';
+    }
+    return condition;
+}
+
+} // namespace
+
+int main()
+{
+    const auto store_path = std::filesystem::temp_directory_path() /
+        ("cmvr-aubo-tool-frames-" + std::to_string(::getpid()) + ".json");
+    std::error_code remove_error;
+    std::filesystem::remove(store_path, remove_error);
+
+    cmvr::config::RobotArmConfig config;
+    config.set_id("aubo-tool-frame-test");
+    auto* vendor = config.mutable_vendor();
+    vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM);
+    vendor->set_tool_frame("tool0");
+    vendor->set_tool_frame_store_path(store_path.string());
+
+    std::uint64_t added_revision = 0;
+    {
+        cmvr::device::AuboArm arm(config);
+        std::vector<cmvr::device::ToolFrame> frames;
+        cmvr::device::ToolFrameCapabilities capabilities;
+        std::uint64_t revision = 0;
+        if (!check(
+                arm.getToolFrames(frames, capabilities, revision).ok(),
+                "initial getToolFrames failed") ||
+            !check(frames.size() == 1U, "legacy Tool Frame was not seeded") ||
+            !check(frames.front().name == "tool0", "legacy Tool Frame name changed") ||
+            !check(frames.front().is_default, "legacy Tool Frame is not default") ||
+            !check(capabilities.add_supported, "AUBO Add capability is disabled")) {
+            return 1;
+        }
+
+        const auto camera_tool = cmvr::common::math::toolFrameFromPose(
+            "camera_tool", {0.01, -0.02, 0.15, 0.1, -0.2, 0.3},
+            false, "test");
+        const auto add_result = arm.addToolFrame(
+            camera_tool, revision, added_revision);
+        if (!check(add_result.ok(), add_result.message) ||
+            !check(added_revision == revision + 1U, "revision did not increment") ||
+            !check(std::filesystem::exists(store_path), "Tool Frame store was not written")) {
+            return 1;
+        }
+    }
+
+    {
+        cmvr::device::AuboArm restored(config);
+        std::vector<cmvr::device::ToolFrame> frames;
+        cmvr::device::ToolFrameCapabilities capabilities;
+        std::uint64_t revision = 0;
+        const auto result = restored.getToolFrames(
+            frames, capabilities, revision);
+        const auto camera_tool = std::find_if(
+            frames.begin(), frames.end(),
+            [](const cmvr::device::ToolFrame& tool_frame) {
+                return tool_frame.name == "camera_tool";
+            });
+        if (!check(result.ok(), result.message) ||
+            !check(revision == added_revision, "stored revision was not restored") ||
+            !check(camera_tool != frames.end(), "stored Tool Frame was not restored")) {
+            return 1;
+        }
+    }
+
+    std::filesystem::remove(store_path, remove_error);
+    return 0;
+}

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

@@ -3,10 +3,12 @@
 #include <algorithm>
 #include <array>
 #include <cmath>
+#include <cstring>
 #include <sstream>
 
 #include "huayan_arm/v1.0/include/HR_Pro.h"
 #include "common/base/logging/logger.h"
+#include "common/math/tool_frame_math.h"
 
 namespace cmvr::device {
 namespace {
@@ -72,6 +74,30 @@ std::vector<double> zeroHrFrame()
     return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 }
 
+std::uint64_t toolFramesRevision(const std::vector<ToolFrame>& tool_frames)
+{
+    std::uint64_t hash = 1469598103934665603ULL;
+    const auto append_byte = [&hash](const unsigned char byte) {
+        hash ^= static_cast<std::uint64_t>(byte);
+        hash *= 1099511628211ULL;
+    };
+    for (const auto& tool_frame : tool_frames) {
+        for (const unsigned char byte : tool_frame.name) {
+            append_byte(byte);
+        }
+        append_byte(0U);
+        for (const double value : tool_frame.flange_to_tcp) {
+            std::uint64_t bits = 0U;
+            static_assert(sizeof(bits) == sizeof(value));
+            std::memcpy(&bits, &value, sizeof(bits));
+            for (unsigned int shift = 0; shift < 64U; shift += 8U) {
+                append_byte(static_cast<unsigned char>(bits >> shift));
+            }
+        }
+    }
+    return hash == 0U ? 1U : hash;
+}
+
 } // namespace
 
 HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg)
@@ -84,7 +110,12 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg)
 
     ip_ = vendor_cfg_.ip();
     port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 10003;
-    tcp_name_ = vendor_cfg_.tool_frame().empty() ? "TCP" : vendor_cfg_.tool_frame();
+    tcp_name_ = vendor_cfg_.default_tool_frame().empty()
+        ? vendor_cfg_.tool_frame()
+        : vendor_cfg_.default_tool_frame();
+    if (tcp_name_.empty()) {
+        tcp_name_ = "TCP";
+    }
     ucs_name_ = vendor_cfg_.base_frame().empty() ? "Base" : vendor_cfg_.base_frame();
 
     const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
@@ -165,6 +196,144 @@ CartesianPose HuayanRobot::getTcpPose(const FrameType frame) const
     return readTcpPose_();
 }
 
+Result HuayanRobot::getTcpPose(const std::string& tcp_frame_name,
+                               const FrameType frame,
+                               CartesianPose& pose) const
+{
+    if (tcp_frame_name.empty()) {
+        pose = getTcpPose(frame);
+        return Result::success();
+    }
+    if (frame != FrameType::Base) {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "[HuayanRobot] named Tool Frame pose is only available in Base frame");
+    }
+    const auto ready = ensureConnected_("getTcpPose");
+    if (!ready.ok()) {
+        return ready;
+    }
+    std::lock_guard<std::mutex> lock(mutex_);
+    std::string resolved_name;
+    const auto resolved = resolveTcpNameLocked_(tcp_frame_name, resolved_name);
+    if (!resolved.ok()) {
+        return resolved;
+    }
+    return readTcpPoseForToolLocked_(resolved_name, pose);
+}
+
+Result HuayanRobot::getToolFrames(std::vector<ToolFrame>& tool_frames,
+                                  ToolFrameCapabilities& capabilities,
+                                  std::uint64_t& revision) const
+{
+    const auto ready = ensureConnected_("getToolFrames");
+    if (!ready.ok()) {
+        return ready;
+    }
+    std::lock_guard<std::mutex> lock(mutex_);
+    const auto result = readToolFramesLocked_(tool_frames, revision);
+    if (!result.ok()) {
+        return result;
+    }
+    capabilities.named_move_l_supported = true;
+    capabilities.named_speed_l_supported = true;
+    capabilities.named_get_pose_supported = true;
+    capabilities.get_supported = true;
+    capabilities.add_supported = true;
+    return Result::success();
+}
+
+Result HuayanRobot::addToolFrame(
+    const ToolFrame& requested_tool_frame,
+    const std::optional<std::uint64_t> expected_revision,
+    std::uint64_t& revision)
+{
+    std::string validation_error;
+    if (!common::math::validateToolFrame(requested_tool_frame, validation_error)) {
+        return Result::failure(
+            ArmErrorCode::InvalidArgument,
+            "[HuayanRobot] invalid Tool Frame: " + validation_error);
+    }
+    const auto ready = ensureConnected_("addToolFrame");
+    if (!ready.ok()) {
+        return ready;
+    }
+
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (busy_.load()) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "[HuayanRobot] cannot add a Tool Frame while motion is active");
+    }
+    std::vector<ToolFrame> current_frames;
+    const auto read_result = readToolFramesLocked_(current_frames, revision);
+    if (!read_result.ok()) {
+        return read_result;
+    }
+    if (expected_revision.has_value() && *expected_revision != revision) {
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[HuayanRobot] Tool Frame revision mismatch");
+    }
+    const auto existing = std::find_if(
+        current_frames.begin(), current_frames.end(),
+        [&](const ToolFrame& tool_frame) {
+            return tool_frame.name == requested_tool_frame.name;
+        });
+    if (existing != current_frames.end()) {
+        if (common::math::sameToolTransform(
+                *existing, requested_tool_frame, 1e-6)) {
+            return Result::success();
+        }
+        return Result::failure(
+            ArmErrorCode::CommandRejected,
+            "[HuayanRobot] Tool Frame already exists with a different transform: " +
+                requested_tool_frame.name);
+    }
+
+    const CartesianPose pose = common::math::toolFramePose(requested_tool_frame);
+    const int configure_result = HRIF_ConfigTCP(
+        box_id_, robot_id_, requested_tool_frame.name,
+        metersToMm(pose.x), metersToMm(pose.y), metersToMm(pose.z),
+        radToDeg(pose.rx), radToDeg(pose.ry), radToDeg(pose.rz));
+    if (configure_result != 0) {
+        return hrResult_(configure_result, "ConfigTCP");
+    }
+
+    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 int verify_result = HRIF_ReadTCPByName(
+        box_id_, robot_id_, requested_tool_frame.name,
+        x, y, z, rx, ry, rz);
+    if (verify_result != 0) {
+        return hrResult_(verify_result, "ReadTCPByName after ConfigTCP");
+    }
+    const ToolFrame read_back = common::math::toolFrameFromPose(
+        requested_tool_frame.name,
+        {mmToMeters(x), mmToMeters(y), mmToMeters(z),
+         degToRad(rx), degToRad(ry), degToRad(rz)},
+        requested_tool_frame.name == tcp_name_, "huayan_controller");
+    if (!common::math::sameToolTransform(
+            read_back, requested_tool_frame, 1e-6)) {
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            "[HuayanRobot] ConfigTCP read-back verification failed");
+    }
+
+    current_frames.push_back(read_back);
+    std::sort(
+        current_frames.begin(), current_frames.end(),
+        [](const ToolFrame& lhs, const ToolFrame& rhs) {
+            return lhs.name < rhs.name;
+        });
+    revision = toolFramesRevision(current_frames);
+    return Result::success();
+}
+
 RobotMode HuayanRobot::getRobotMode() const
 {
     if (!isConnected()) {
@@ -346,6 +515,26 @@ Result HuayanRobot::stopJ(const double acceleration)
 }
 
 Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame)
+{
+    return moveLWithTcp_(target, options, frame, tcp_name_, false);
+}
+
+Result HuayanRobot::moveL(const CartesianPose& target,
+                          const MotionOptions& options,
+                          const FrameType frame,
+                          const std::string& tcp_frame_name)
+{
+    if (tcp_frame_name.empty()) {
+        return moveL(target, options, frame);
+    }
+    return moveLWithTcp_(target, options, frame, tcp_frame_name, true);
+}
+
+Result HuayanRobot::moveLWithTcp_(const CartesianPose& target,
+                                  const MotionOptions& options,
+                                  const FrameType frame,
+                                  const std::string& requested_tcp_name,
+                                  const bool validate_name)
 {
     (void)frame;
     const auto ready = ensureConnected_("moveL");
@@ -356,18 +545,35 @@ Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& opti
         return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_);
     }
 
-    const auto pose = poseToHrCoord(target);
+    int ret = 0;
-    const auto q_deg = toSix(currentJointPositionDeg_());
+    std::string resolved_tcp_name = requested_tcp_name;
-    const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm;
+    {
-    const double acceleration = options.acceleration > 0.0 ? metersToMm(options.acceleration) : kDefaultMoveLAccelerationMm;
+        std::lock_guard<std::mutex> lock(mutex_);
-    const double blend = metersToMm(options.blend_radius);
+        if (validate_name) {
-    const std::string command_id = nextCommandId_();
+            const auto resolved = resolveTcpNameLocked_(
-
+                requested_tcp_name, resolved_tcp_name);
-    const int ret = HRIF_MoveL(box_id_, robot_id_,
+            if (!resolved.ok()) {
-                              pose[0], pose[1], pose[2], pose[3], pose[4], pose[5],
+                busy_.store(false);
-                              q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5],
+                return resolved;
-                              tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend,
+            }
-                              0, 0, 0, command_id);
+        }
+        const auto pose = poseToHrCoord(target);
+        const auto q_deg = toSix(currentJointPositionDeg_());
+        const double velocity = options.velocity > 0.0
+            ? metersToMm(options.velocity)
+            : kDefaultMoveLVelocityMm;
+        const double acceleration = options.acceleration > 0.0
+            ? metersToMm(options.acceleration)
+            : kDefaultMoveLAccelerationMm;
+        const double blend = metersToMm(options.blend_radius);
+        const std::string command_id = nextCommandId_();
+        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],
+            resolved_tcp_name, ucs_name_, velocity * speed_scaling_,
+            acceleration, blend, 0, 0, 0, command_id);
+    }
     if (ret != 0) {
         busy_.store(false);
         return hrResult_(ret, "moveL");
@@ -381,30 +587,106 @@ Result HuayanRobot::speedL(const CartesianVelocity& velocity,
                             const double acceleration,
                             const double duration,
                             const FrameType frame)
+{
+    return speedLWithTcp_(velocity, acceleration, duration, frame, tcp_name_, false);
+}
+
+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()) {
+        return speedL(velocity, acceleration, duration, frame);
+    }
+    return speedLWithTcp_(
+        velocity, acceleration, duration, frame, tcp_frame_name, true);
+}
+
+Result HuayanRobot::speedLWithTcp_(const CartesianVelocity& velocity,
+                                   const double acceleration,
+                                   const double duration,
+                                   const FrameType frame,
+                                   const std::string& requested_tcp_name,
+                                   const bool validate_name)
 {
     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(
-    const double vz_mm = metersToMm(velocity.vz);
+            ArmErrorCode::RobotNotReady,
-    const double wx_deg = radToDeg(velocity.wx);
+            "[HuayanRobot] arm is busy: " + id_);
-    const double wy_deg = radToDeg(velocity.wy);
+    }
-    const double wz_deg = radToDeg(velocity.wz);
+    if (validate_name &&
+        frame != FrameType::Base && frame != FrameType::Tool) {
+        busy_.store(false);
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "[HuayanRobot] named speedL supports only Base and Tool frames");
+    }
 
-    const double linear_acc_mm =
+    CartesianVelocity command_velocity = velocity;
-        acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm;
+    int ret = 0;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        std::string resolved_tcp_name = requested_tcp_name;
+        if (validate_name) {
+            const auto resolved = resolveTcpNameLocked_(
+                requested_tcp_name, resolved_tcp_name);
+            if (!resolved.ok()) {
+                busy_.store(false);
+                return resolved;
+            }
+            const int set_tcp_result = HRIF_SetTCPByName(
+                box_id_, robot_id_, resolved_tcp_name);
+            if (set_tcp_result != 0) {
+                busy_.store(false);
+                return hrResult_(set_tcp_result, "SetTCPByName");
+            }
+            if (frame == FrameType::Tool) {
+                CartesianPose tcp_pose;
+                const auto pose_result = readTcpPoseForToolLocked_(
+                    resolved_tcp_name, tcp_pose);
+                if (!pose_result.ok()) {
+                    busy_.store(false);
+                    return pose_result;
+                }
+                const Eigen::Matrix3d rotation =
+                    common::math::poseToMatrix(tcp_pose).block<3, 3>(0, 0);
+                const Eigen::Vector3d linear = rotation * Eigen::Vector3d(
+                    velocity.vx, velocity.vy, velocity.vz);
+                const Eigen::Vector3d angular = rotation * Eigen::Vector3d(
+                    velocity.wx, velocity.wy, velocity.wz);
+                command_velocity = {
+                    linear.x(), linear.y(), linear.z(),
+                    angular.x(), angular.y(), angular.z()};
+            }
+        }
 
-    const double angular_acc_deg =
+        const double vx_mm = metersToMm(command_velocity.vx);
-        acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg;
+        const double vy_mm = metersToMm(command_velocity.vy);
+        const double vz_mm = metersToMm(command_velocity.vz);
+        const double wx_deg = radToDeg(command_velocity.wx);
+        const double wy_deg = radToDeg(command_velocity.wy);
+        const double wz_deg = radToDeg(command_velocity.wz);
 
-    const double runtime = duration > 0.0 ? duration : 0.5;
+        const double linear_acc_mm =
+            acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm;
 
-    std::lock_guard<std::mutex> lock(mutex_);
+        const double angular_acc_deg =
-    servo_mode_.store(false);
+            acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg;
-    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);
+        const double runtime = duration > 0.0 ? duration : 0.5;
+
+        servo_mode_.store(false);
+        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);
+    }
     if (ret != 0) {
         busy_.store(false);
         return hrResult_(ret, "SpeedL");
@@ -679,6 +961,92 @@ bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const
     return true;
 }
 
+Result HuayanRobot::resolveTcpNameLocked_(
+    const std::string& requested_name,
+    std::string& resolved_name) const
+{
+    resolved_name = requested_name.empty() ? tcp_name_ : requested_name;
+    std::vector<std::string> tcp_names;
+    const int result = HRIF_ReadTCPList(box_id_, robot_id_, tcp_names);
+    if (result != 0) {
+        return hrResult_(result, "ReadTCPList");
+    }
+    if (std::find(tcp_names.begin(), tcp_names.end(), resolved_name) ==
+        tcp_names.end()) {
+        return Result::failure(
+            ArmErrorCode::InvalidArgument,
+            "[HuayanRobot] unknown Tool Frame: " + resolved_name);
+    }
+    return Result::success();
+}
+
+Result HuayanRobot::readToolFramesLocked_(
+    std::vector<ToolFrame>& tool_frames,
+    std::uint64_t& revision) const
+{
+    std::vector<std::string> tcp_names;
+    const int list_result = HRIF_ReadTCPList(box_id_, robot_id_, tcp_names);
+    if (list_result != 0) {
+        return hrResult_(list_result, "ReadTCPList");
+    }
+    std::sort(tcp_names.begin(), tcp_names.end());
+    tcp_names.erase(
+        std::unique(tcp_names.begin(), tcp_names.end()), tcp_names.end());
+
+    std::vector<ToolFrame> result_frames;
+    result_frames.reserve(tcp_names.size());
+    for (const auto& name : tcp_names) {
+        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 int read_result = HRIF_ReadTCPByName(
+            box_id_, robot_id_, name, x, y, z, rx, ry, rz);
+        if (read_result != 0) {
+            return hrResult_(read_result, "ReadTCPByName(" + name + ")");
+        }
+        result_frames.push_back(common::math::toolFrameFromPose(
+            name,
+            {mmToMeters(x), mmToMeters(y), mmToMeters(z),
+             degToRad(rx), degToRad(ry), degToRad(rz)},
+            name == tcp_name_, "huayan_controller"));
+    }
+    revision = toolFramesRevision(result_frames);
+    tool_frames = std::move(result_frames);
+    return Result::success();
+}
+
+Result HuayanRobot::readTcpPoseForToolLocked_(
+    const std::string& tcp_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 int result = HRIF_ReadActCoord(
+        box_id_, robot_id_, ucs_name_, tcp_name,
+        j1, j2, j3, j4, j5, j6,
+        x, y, z, rx, ry, rz);
+    if (result != 0) {
+        return hrResult_(result, "ReadActCoord(" + tcp_name + ")");
+    }
+    pose = {
+        mmToMeters(x), mmToMeters(y), mmToMeters(z),
+        degToRad(rx), degToRad(ry), degToRad(rz)};
+    return Result::success();
+}
+
 HuayanRobot::HrState HuayanRobot::readHrState_() const
 {
     HrState state;

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

@@ -33,6 +33,9 @@ public:
     ArmState getRobotState() const override;
     JointGroupState getJointState() const override;
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
+    Result getTcpPose(const std::string& tcp_frame_name,
+                      FrameType frame,
+                      CartesianPose& pose) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
     ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
@@ -60,9 +63,24 @@ 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;
+    Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                         ToolFrameCapabilities& capabilities,
+                         std::uint64_t& revision) const override;
+    Result addToolFrame(const ToolFrame& tool_frame,
+                        std::optional<std::uint64_t> expected_revision,
+                        std::uint64_t& revision) override;
 
     Result startServoMode(const ServoOptions& options) override;
     Result servoJ(const JointPositionCommand& target) override;
@@ -121,6 +139,23 @@ private:
     std::vector<double> readJointVelocityRad_() const;
     CartesianPose readTcpPose_() const;
     CartesianVelocity readTcpVelocity_() const;
+    Result readTcpPoseForToolLocked_(const std::string& tcp_name,
+                                     CartesianPose& pose) const;
+    Result readToolFramesLocked_(std::vector<ToolFrame>& tool_frames,
+                                 std::uint64_t& revision) const;
+    Result resolveTcpNameLocked_(const std::string& requested_name,
+                                 std::string& resolved_name) const;
+    Result moveLWithTcp_(const CartesianPose& target,
+                         const MotionOptions& options,
+                         FrameType frame,
+                         const std::string& requested_tcp_name,
+                         bool validate_name);
+    Result speedLWithTcp_(const CartesianVelocity& velocity,
+                          double acceleration,
+                          double duration,
+                          FrameType frame,
+                          const std::string& requested_tcp_name,
+                          bool validate_name);
     std::vector<double> currentJointPositionDeg_() const;
     std::string nextCommandId_() const;
     Result waitMotionDone_(const std::string& context, int timeout_ms) const;

cmvr-es/devices/arm/robot_arm.h

@@ -25,6 +25,18 @@ public:
     virtual ArmState getRobotState() const = 0;
     virtual JointGroupState getJointState() const = 0;
     virtual CartesianPose getTcpPose(FrameType frame = FrameType::Base) const = 0;
+    virtual Result getTcpPose(const std::string& tcp_frame_name,
+                              FrameType frame,
+                              CartesianPose& pose) const
+    {
+        if (tcp_frame_name.empty()) {
+            pose = getTcpPose(frame);
+            return Result::success();
+        }
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "named Tool Frames are unsupported by this RobotArm");
+    }
     virtual RobotMode getRobotMode() const = 0;
     virtual SafetyMode getSafetyMode() const = 0;
     virtual ControlMode getControlMode() const = 0;
@@ -96,13 +108,58 @@ 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 unsupported by this RobotArm");
+    }
     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 unsupported by this RobotArm");
+    }
     virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
     virtual Result stopMotion() = 0;
 
+    virtual Result getToolFrames(std::vector<ToolFrame>& tool_frames,
+                                 ToolFrameCapabilities& capabilities,
+                                 std::uint64_t& revision) const
+    {
+        tool_frames.clear();
+        capabilities = {};
+        revision = 0;
+        return Result::success();
+    }
+    virtual Result addToolFrame(
+        const ToolFrame&,
+        std::optional<std::uint64_t>,
+        std::uint64_t& revision)
+    {
+        revision = 0;
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "adding Tool Frames is unsupported by this RobotArm");
+    }
+
     virtual Result moveP(const CartesianPose& target,
                          const MotionOptions& options,
                          FrameType frame = FrameType::Base)

cmvr-es/service/grpc/action/src/action_queue_executor.cpp

@@ -622,6 +622,34 @@ ValidationResult validateRequest(
                         "ActionQueue MoveL currently supports the Base frame only";
                     return result;
                 }
+                if (!command.tcp_frame_name().empty()) {
+                    std::vector<device::ToolFrame> tool_frames;
+                    device::ToolFrameCapabilities capabilities;
+                    std::uint64_t revision = 0;
+                    const auto tool_result = prepared.arm->getToolFrames(
+                        tool_frames, capabilities, revision);
+                    if (!tool_result.ok()) {
+                        result.error = prefix +
+                            "could not query RobotArm Tool Frames: " +
+                            tool_result.message;
+                        return result;
+                    }
+                    if (!capabilities.named_move_l_supported) {
+                        result.error = prefix +
+                            "RobotArm backend does not support named Tool Frames for MoveL";
+                        return result;
+                    }
+                    const auto selected = std::find_if(
+                        tool_frames.begin(), tool_frames.end(),
+                        [&](const device::ToolFrame& tool_frame) {
+                            return tool_frame.name == command.tcp_frame_name();
+                        });
+                    if (selected == tool_frames.end()) {
+                        result.error = prefix + "unknown Tool Frame: " +
+                            command.tcp_frame_name();
+                        return result;
+                    }
+                }
                 const auto dof = prepared.arm->getDof();
                 if (!command.options().joint_velocity_limits().empty() &&
                     command.options().joint_velocity_limits_size() !=
@@ -2091,7 +2119,8 @@ struct ActionQueueExecutor::Impl {
                 auto options = toArmMotionOptions(command.options());
                 options.cancellation_requested = cancellation_requested;
                 motion_result = prepared.arm->moveL(
-                    target, options, toFrameType(command.frame()));
+                    target, options, toFrameType(command.frame()),
+                    command.tcp_frame_name());
             }
         } catch (const std::exception& error) {
             motion_result = device::Result::failure(

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

@@ -44,6 +44,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

@@ -24,7 +24,25 @@ grpc::Status resultToStatus(const device::Result& result)
     if (result.ok()) {
         return grpc::Status::OK;
     }
-    return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
+    switch (result.code) {
+        case device::ArmErrorCode::InvalidArgument:
+        case device::ArmErrorCode::InvalidDof:
+            return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, result.message);
+        case device::ArmErrorCode::UnsupportedCommand:
+            return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, result.message);
+        case device::ArmErrorCode::NotConnected:
+        case device::ArmErrorCode::ConnectionFailed:
+            return grpc::Status(grpc::StatusCode::UNAVAILABLE, result.message);
+        case device::ArmErrorCode::RobotNotReady:
+        case device::ArmErrorCode::RobotNotPowered:
+            return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, result.message);
+        case device::ArmErrorCode::CommandRejected:
+            return grpc::Status(grpc::StatusCode::ABORTED, result.message);
+        case device::ArmErrorCode::Timeout:
+            return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, result.message);
+        default:
+            return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
+    }
 }
 
 void logRpcSuccess(const char* rpc_name, const std::string& device_id)
@@ -92,6 +110,56 @@ api::CartesianPose toApiCartesianPose(const device::CartesianPose& src)
     return dst;
 }
 
+device::ToolFrame toToolFrame(const api::ToolFrame& src)
+{
+    device::ToolFrame dst;
+    dst.name = src.name();
+    dst.is_default = src.is_default();
+    dst.source = src.source();
+    const auto& matrix = src.flange_to_tcp();
+    dst.flange_to_tcp = {
+        matrix.m00(), matrix.m01(), matrix.m02(), matrix.m03(),
+        matrix.m10(), matrix.m11(), matrix.m12(), matrix.m13(),
+        matrix.m20(), matrix.m21(), matrix.m22(), matrix.m23(),
+        matrix.m30(), matrix.m31(), matrix.m32(), matrix.m33()};
+    return dst;
+}
+
+void fillToolFrame(const device::ToolFrame& src, api::ToolFrame* dst)
+{
+    dst->set_name(src.name);
+    dst->set_is_default(src.is_default);
+    dst->set_source(src.source);
+    auto* matrix = dst->mutable_flange_to_tcp();
+    matrix->set_m00(src.flange_to_tcp[0]);
+    matrix->set_m01(src.flange_to_tcp[1]);
+    matrix->set_m02(src.flange_to_tcp[2]);
+    matrix->set_m03(src.flange_to_tcp[3]);
+    matrix->set_m10(src.flange_to_tcp[4]);
+    matrix->set_m11(src.flange_to_tcp[5]);
+    matrix->set_m12(src.flange_to_tcp[6]);
+    matrix->set_m13(src.flange_to_tcp[7]);
+    matrix->set_m20(src.flange_to_tcp[8]);
+    matrix->set_m21(src.flange_to_tcp[9]);
+    matrix->set_m22(src.flange_to_tcp[10]);
+    matrix->set_m23(src.flange_to_tcp[11]);
+    matrix->set_m30(src.flange_to_tcp[12]);
+    matrix->set_m31(src.flange_to_tcp[13]);
+    matrix->set_m32(src.flange_to_tcp[14]);
+    matrix->set_m33(src.flange_to_tcp[15]);
+}
+
+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);
+}
+
 api::CartesianVelocity toApiCartesianVelocity(const device::CartesianVelocity& src)
 {
     api::CartesianVelocity dst;
@@ -330,10 +398,12 @@ 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();
+                            << ", frame=" << request->frame()
+                            << ", tcp_frame_name=" << request->tcp_frame_name();
         }
         return setResponseResult(response, result);
     } catch (const std::exception& e) {
@@ -381,12 +451,14 @@ 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()
                             << ", duration=" << request->duration()
-                            << ", frame=" << request->frame();
+                            << ", frame=" << request->frame()
+                            << ", tcp_frame_name=" << request->tcp_frame_name();
         }
         return setResponseResult(response, result);
     } catch (const std::exception& e) {
@@ -498,9 +570,31 @@ 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)
+        if (request->tcp_frame_name().empty()) {
-            : arm->fk(request->base_link(), request->ee_link());
+            pose = request->base_link().empty() || request->ee_link().empty()
+                ? arm->fk(true)
+                : arm->fk(request->base_link(), request->ee_link());
+        } else {
+            if (!request->ee_link().empty()) {
+                const auto result = device::Result::failure(
+                    device::ArmErrorCode::InvalidArgument,
+                    "ee_link cannot be combined with tcp_frame_name");
+                return setResponseResult(response, result);
+            }
+            if (!request->base_link().empty()) {
+                const auto result = device::Result::failure(
+                    device::ArmErrorCode::UnsupportedCommand,
+                    "named Tool Frame pose currently requires an empty base_link");
+                return setResponseResult(response, result);
+            }
+            const auto result = arm->getTcpPose(
+                request->tcp_frame_name(), device::FrameType::Base, pose);
+            if (!result.ok()) {
+                return setResponseResult(response, result);
+            }
+            response->set_resolved_tcp_frame_name(request->tcp_frame_name());
+        }
         *response->mutable_pose() = toApiCartesianPose(pose);
         fillFeedback(response->mutable_header(), true);
         CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
@@ -513,6 +607,69 @@ 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;
+        std::uint64_t revision = 0;
+        const auto result = arm->getToolFrames(
+            tool_frames, capabilities, revision);
+        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());
+        response->set_revision(revision);
+        fillFeedback(response->mutable_header(), true);
+        logRpcSuccess("getToolFrames", device_id);
+        return grpc::Status::OK;
+    } catch (const std::exception& exception) {
+        fillFeedback(response->mutable_header(), false, exception.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, exception.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);
+        }
+        std::optional<std::uint64_t> expected_revision;
+        if (request->has_expected_revision()) {
+            expected_revision = request->expected_revision();
+        }
+        std::uint64_t revision = 0;
+        const auto result = arm->addToolFrame(
+            toToolFrame(request->tool_frame()), expected_revision, revision);
+        response->set_revision(revision);
+        if (result.ok()) {
+            logRpcSuccess("addToolFrame", device_id);
+        }
+        return setResponseResult(response, result);
+    } catch (const std::exception& exception) {
+        fillFeedback(response->mutable_header(), false, exception.what());
+        return grpc::Status(grpc::StatusCode::INTERNAL, exception.what());
+    }
+}
+
 grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
                                                 const api::CalibrateZeroQ_Request* request,
                                                 api::CalibrateZeroQ_Response* response)
@@ -570,4 +727,3 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context,
     }
 }
 } // namespace cmvr::service
-

protos/cmvr/api/arm_command.proto

@@ -112,6 +112,7 @@ message MoveL {
     CartesianPose target = 2;
     MotionOptions options = 3;
     ArmFrameType frame = 4;
+    string tcp_frame_name = 5;
   }
 
   message Response {
@@ -139,6 +140,7 @@ message SpeedL {
     double acceleration = 3;
     double duration = 4;
     ArmFrameType frame = 5;
+    string tcp_frame_name = 6;
   }
 
   message Response {
@@ -211,11 +213,54 @@ message GetPose {
     CommandHeader.Request header = 1;
     string base_link = 2;
     string ee_link = 3;
+    string tcp_frame_name = 4;
   }
 
   message Response {
     CommandHeader.Feedback header = 1;
     CartesianPose pose = 2;
+    string resolved_tcp_frame_name = 3;
+  }
+}
+
+message ToolFrame {
+  string name = 1;
+  TransformMatrix4x4 flange_to_tcp = 2;
+  bool is_default = 3;
+  string source = 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 GetToolFrames {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Response {
+    CommandHeader.Feedback header = 1;
+    repeated ToolFrame tool_frames = 2;
+    ToolFrameCapabilities capabilities = 3;
+    uint64 revision = 4;
+  }
+}
+
+message AddToolFrame {
+  message Request {
+    CommandHeader.Request header = 1;
+    ToolFrame tool_frame = 2;
+    optional uint64 expected_revision = 3;
+  }
+
+  message Response {
+    CommandHeader.Feedback header = 1;
+    uint64 revision = 2;
   }
 }
 

protos/cmvr/api/arm_service.proto

@@ -18,6 +18,8 @@ service ArmService {
   rpc getJointState(JointRequest) returns (JointResponse);
   rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response);
   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

@@ -37,6 +37,16 @@ enum VendorRobotArmBrand {
   VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM = 2;
 }
 
+message VendorToolFrameConfig {
+  string name = 1;
+  double x = 2;
+  double y = 3;
+  double z = 4;
+  double rx = 5;
+  double ry = 6;
+  double rz = 7;
+}
+
 message VendorRobotArmBackendConfig {
   VendorRobotArmBrand brand = 1;
   string ip = 2;
@@ -51,6 +61,10 @@ message VendorRobotArmBackendConfig {
   // AUBO only. Automatic energization after a physical E-stop is deliberately
   // opt-in because releasing brakes changes the hardware energy state.
   optional bool auto_power_on_after_hardware_estop_release = 11;
+  repeated VendorToolFrameConfig tool_frames = 12;
+  string default_tool_frame = 13;
+  // AUBO only. Runtime additions are rejected when this path is empty.
+  string tool_frame_store_path = 14;
 }
 
 enum DamiaoMotorModel {