From 049aaf9b77913e424556f62427c3cc95592b2a86 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 27 Aug 2026 16:25:07 +0800 Subject: [PATCH] feat: add multi-tool frame arm grpc support --- cmvr-es/common/types/arm/arm_types.h | 26 ++ cmvr-es/devices/arm/CMakeLists.txt | 8 + cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 380 +++++++++++++++++- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 20 + cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 260 +++++++++++- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 16 +- cmvr-es/devices/arm/robot_arm.h | 50 +++ .../arm/tests/tool_frame_utils_test.cpp | 66 +++ cmvr-es/devices/arm/tool_frame_utils.h | 119 ++++++ .../service/grpc/include/grpc_arm_service.h | 6 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 161 +++++++- protos/cmvr/api/arm_command.proto | 56 +++ protos/cmvr/api/arm_service.proto | 2 + .../cmvr/config/arm_config/arm_config.proto | 13 + 14 files changed, 1154 insertions(+), 29 deletions(-) create mode 100644 cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp create mode 100644 cmvr-es/devices/arm/tool_frame_utils.h diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..f0d961b2 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_ARM_TYPES_H #include +#include #include #include @@ -64,6 +65,31 @@ struct CartesianVelocity { double wz{0.0}; }; +enum class ToolFrameSource { + Config, + Runtime, + Controller +}; + +struct ToolFrame { + std::string name; + std::array 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}; diff --git a/cmvr-es/devices/arm/CMakeLists.txt b/cmvr-es/devices/arm/CMakeLists.txt index d2780c71..9ffcfcdb 100644 --- a/cmvr-es/devices/arm/CMakeLists.txt +++ b/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() diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 41dedc9f..46d76d3d 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,12 +1,20 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include +#include +#include #include +#include #include +#include +#include +#include #include #include +#include #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& values) return pose; } +std::string fileSafeId(std::string id) +{ + for (char& ch : id) { + const auto value = static_cast(ch); + if (!std::isalnum(value) && ch != '-' && ch != '_') { + ch = '_'; + } + } + return id.empty() ? "arm" : id; +} + +bool configMatrix(const config::ToolFrameConfig& config, + std::array& matrix) +{ + if (config.flange_t_tcp_size() != static_cast(matrix.size())) { + return false; + } + std::copy(config.flange_t_tcp().begin(), config.flange_t_tcp().end(), matrix.begin()); + return true; +} + +std::vector 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(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 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 tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + const auto tcp_offset = tcp_frame_name.empty() + ? std::vector(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 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 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 tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + const auto tcp_offset = tcp_frame_name.empty() + ? std::vector(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 line_speed{velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0}; - std::vector angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0}; + std::array line_speed{velocity.vx, velocity.vy, velocity.vz}; + std::array angular_speed{velocity.wx, velocity.wy, velocity.wz}; if (frame == FrameType::Tool) { - auto tool_frame = robot_interface->getRobotState()->getTcpPose(); - if (tool_frame.size() < 6) { + const auto tcp_pose_values = robot_interface->getRobotState()->getTcpPose(); + 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; - tool_frame[1] = 0.0; - tool_frame[2] = 0.0; - line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed); - angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed); + const auto tcp_pose = poseFromVector(tcp_pose_values); + line_speed = rotateVector(tcp_pose, line_speed); + angular_speed = rotateVector(tcp_pose, 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 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& tool_frames, + ToolFrameCapabilities& capabilities) const +{ + std::lock_guard 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 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(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 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"; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 20049b6f..a083d038 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -3,6 +3,7 @@ #include #include +#include #include #include #include @@ -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 acceleration = std::nullopt) override; Result stopMotion() override; @@ -77,6 +82,14 @@ public: std::shared_ptr 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& 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 tool_frames_; double speed_scaling_{1.0}; ServoOptions servo_options_; std::atomic connected_{false}; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 74ef039f..0ea84cc1 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/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 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); - const double vy_mm = metersToMm(velocity.vy); - const double vz_mm = metersToMm(velocity.vz); - const double wx_deg = radToDeg(velocity.wx); - const double wy_deg = radToDeg(velocity.wy); - const double wz_deg = radToDeg(velocity.wz); + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); + } + + std::lock_guard lock(mutex_); + std::array linear{velocity.vx, velocity.vy, velocity.vz}; + std::array 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 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 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& tool_frames, + ToolFrameCapabilities& capabilities) const +{ + const auto ready = ensureConnected_("getToolFrames"); + if (!ready.ok()) { + return ready; + } + + std::lock_guard lock(mutex_); + std::vector 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 lock(mutex_); + std::vector 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 HuayanRobot::currentJointPositionDeg_() const { const auto q_rad = readJointPositionRad_(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 92a3d1a3..3e682caa 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/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 acceleration) override; Result stopMotion() override; @@ -83,6 +87,14 @@ public: std::shared_ptr 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& 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 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 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 \ No newline at end of file +#endif //CMVR_ES_HUAYAN_ARM_H diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 6ab24556..275f4e8a 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/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 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& 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; }; diff --git a/cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp b/cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp new file mode 100644 index 00000000..0330c7cd --- /dev/null +++ b/cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +#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; +} diff --git a/cmvr-es/devices/arm/tool_frame_utils.h b/cmvr-es/devices/arm/tool_frame_utils.h new file mode 100644 index 00000000..70af2941 --- /dev/null +++ b/cmvr-es/devices/arm/tool_frame_utils.h @@ -0,0 +1,119 @@ +#pragma once + +#include +#include +#include +#include +#include + +#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& lhs, + const std::array& 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 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& 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 rotateVector(const CartesianPose& pose, + const std::array& 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 diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 07a2dc30..c1946b28 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/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; diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 1ae88019..781dc5e2 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -1,5 +1,8 @@ #include "service/grpc/include/grpc_arm_service.h" +#include +#include + #include #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 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& 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 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() - ? arm->fk(true) - : arm->fk(request->base_link(), request->ee_link()); + device::CartesianPose pose; + std::string resolved_tcp_frame_name; + 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_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + + std::vector 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_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 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 - diff --git a/protos/cmvr/api/arm_command.proto b/protos/cmvr/api/arm_command.proto index 3c0d7b99..b1b50245 100644 --- a/protos/cmvr/api/arm_command.proto +++ b/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; } } diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 92562f4a..e4ded63b 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/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); diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index 06c0c33b..976f3a34 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/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 {