SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add multi-tool frame arm grpc support

Browse Source
This commit is contained in:
xtkuang 2026-08-27 16:25:07 +08:00
parent f443a7ce53
commit 049aaf9b77
14 changed files with 1154 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cmvr-es/common/types/arm/arm_types.h
View File

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

8
cmvr-es/devices/arm/CMakeLists.txt
View File

@ -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()

380
cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp
View File

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

20
cmvr-es/devices/arm/aubo_arm/aubo_arm.h
View File

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

260
cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp
View File

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

14
cmvr-es/devices/arm/huayan_arm/huayan_arm.h
View File

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

50
cmvr-es/devices/arm/robot_arm.h
View File

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

66
cmvr-es/devices/arm/tests/tool_frame_utils_test.cpp Normal file
View File

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

119
cmvr-es/devices/arm/tool_frame_utils.h Normal file
View File

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

6
cmvr-es/service/grpc/include/grpc_arm_service.h
View File

@ -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;

161
cmvr-es/service/grpc/src/grpc_arm_service.cpp
View File

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

56
protos/cmvr/api/arm_command.proto
View File

@ -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;
}
}

2
protos/cmvr/api/arm_service.proto
View File

@ -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);

13
protos/cmvr/config/arm_config/arm_config.proto
View File

@ -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 {