From 601ebfbf18a1e8f11935d75a842b27aa56339f4b Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 27 Aug 2026 18:14:38 +0800 Subject: [PATCH] feat: add vendor arm tool frame support --- cmvr-es/common/math/tool_frame_math.h | 109 +++++ cmvr-es/common/types/arm/arm_types.h | 20 + cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 7 + cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 371 ++++++++++++++- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 31 ++ .../arm/aubo_arm/aubo_tool_frame_test.cpp | 87 ++++ cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 424 ++++++++++++++++-- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 35 ++ cmvr-es/devices/arm/robot_arm.h | 57 +++ .../grpc/action/src/action_queue_executor.cpp | 31 +- .../service/grpc/include/grpc_arm_service.h | 6 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 174 ++++++- protos/cmvr/api/arm_command.proto | 45 ++ protos/cmvr/api/arm_service.proto | 2 + .../cmvr/config/arm_config/arm_config.proto | 14 + 15 files changed, 1370 insertions(+), 43 deletions(-) create mode 100644 cmvr-es/common/math/tool_frame_math.h create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_tool_frame_test.cpp diff --git a/cmvr-es/common/math/tool_frame_math.h b/cmvr-es/common/math/tool_frame_math.h new file mode 100644 index 00000000..33ef3f18 --- /dev/null +++ b/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 +#include +#include + +#include + +#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(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(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 diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index a4682a77..00095812 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/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 #include #include #include @@ -159,6 +160,25 @@ struct ToolConfig { CartesianPose tcp_offset; }; +struct ToolFrame { + std::string name; + std::array 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}; diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index 64d4cca8..d97771b6 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/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() diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 3c5b52d0..5f4b94ed 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -11,12 +11,15 @@ #include #include #include +#include +#include #include #include #include #include #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& values) return pose; } +std::vector 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 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& 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 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& tool_frames, + ToolFrameCapabilities& capabilities, + std::uint64_t& revision) const +{ + std::lock_guard 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 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 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 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 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 tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + auto robot_config = robot_interface->getRobotConfig(); + 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 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 tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + auto robot_config = robot_interface->getRobotConfig(); + 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 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}; 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)); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 8665aecc..5938a88c 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -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 acceleration = std::nullopt) override; Result stopMotion() override; + Result getToolFrames(std::vector& tool_frames, + ToolFrameCapabilities& capabilities, + std::uint64_t& revision) const override; + Result addToolFrame(const ToolFrame& tool_frame, + std::optional 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 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& 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 connected_{false}; @@ -122,6 +150,9 @@ private: std::atomic servo_mode_{false}; std::atomic emergency_stopped_{false}; mutable std::mutex mutex_; + mutable std::mutex tool_frames_mutex_; + std::map tool_frames_; + std::uint64_t tool_frames_revision_{1}; std::unique_ptr sdk_; }; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_tool_frame_test.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_tool_frame_test.cpp new file mode 100644 index 00000000..a17995d0 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_tool_frame_test.cpp @@ -0,0 +1,87 @@ +#include +#include +#include +#include +#include + +#include + +#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 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 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; +} diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 74ef039f..3098f653 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -3,10 +3,12 @@ #include #include #include +#include #include #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 zeroHrFrame() return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } +std::uint64_t toolFramesRevision(const std::vector& tool_frames) +{ + std::uint64_t hash = 1469598103934665603ULL; + const auto append_byte = [&hash](const unsigned char byte) { + hash ^= static_cast(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(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(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 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& tool_frames, + ToolFrameCapabilities& capabilities, + std::uint64_t& revision) const +{ + const auto ready = ensureConnected_("getToolFrames"); + if (!ready.ok()) { + return ready; + } + std::lock_guard 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 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 lock(mutex_); + if (busy_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] cannot add a Tool Frame while motion is active"); + } + std::vector 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); - 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_(); - - 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, - 0, 0, 0, command_id); + int ret = 0; + std::string resolved_tcp_name = requested_tcp_name; + { + std::lock_guard lock(mutex_); + if (validate_name) { + const auto resolved = resolveTcpNameLocked_( + requested_tcp_name, resolved_tcp_name); + if (!resolved.ok()) { + busy_.store(false); + return resolved; + } + } + 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); - 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_); + } + 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 = - acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm; + CartesianVelocity command_velocity = velocity; + int ret = 0; + { + std::lock_guard 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 = - acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; + const double vx_mm = metersToMm(command_velocity.vx); + 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 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); + const double angular_acc_deg = + acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; + + 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 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& tool_frames, + std::uint64_t& revision) const +{ + std::vector 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 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; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 5db631fa..4a57ff8c 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/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 acceleration) override; Result stopMotion() override; + Result getToolFrames(std::vector& tool_frames, + ToolFrameCapabilities& capabilities, + std::uint64_t& revision) const override; + Result addToolFrame(const ToolFrame& tool_frame, + std::optional 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 readJointVelocityRad_() const; CartesianPose readTcpPose_() const; CartesianVelocity readTcpVelocity_() const; + Result readTcpPoseForToolLocked_(const std::string& tcp_name, + CartesianPose& pose) const; + Result readToolFramesLocked_(std::vector& 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 currentJointPositionDeg_() const; std::string nextCommandId_() const; Result waitMotionDone_(const std::string& context, int timeout_ms) const; diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 7b9eff4f..0b418de6 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/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 acceleration = std::nullopt) = 0; virtual Result stopMotion() = 0; + virtual Result getToolFrames(std::vector& 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& 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) diff --git a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp b/cmvr-es/service/grpc/action/src/action_queue_executor.cpp index d2175348..5ed224a4 100644 --- a/cmvr-es/service/grpc/action/src/action_queue_executor.cpp +++ b/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 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( diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 3133cc0c..0f909d0f 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/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; diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 60dc3a26..077d848c 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/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() - ? arm->fk(true) - : arm->fk(request->base_link(), request->ee_link()); + device::CartesianPose pose; + if (request->tcp_frame_name().empty()) { + 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_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + std::vector 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_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + std::optional 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 - diff --git a/protos/cmvr/api/arm_command.proto b/protos/cmvr/api/arm_command.proto index 53fbf804..7084c4ad 100644 --- a/protos/cmvr/api/arm_command.proto +++ b/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; } } diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index a07018f7..65ea015f 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/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); diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index b717e89d..a53ffc1c 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/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 {