1344 lines
48 KiB
C++
1344 lines
48 KiB
C++
#include "devices/arm/aubo_arm/aubo_arm.h"
|
|
|
|
#include "devices/arm/aubo_arm/aubo_motion_result.h"
|
|
|
|
#include <algorithm>
|
|
#include <cctype>
|
|
#include <chrono>
|
|
#include <cstring>
|
|
#include <exception>
|
|
#include <thread>
|
|
#include <tuple>
|
|
|
|
#include "common/base/logging/logger.h"
|
|
#include "json/json.h"
|
|
|
|
#include "aubo_sdk/rpc.h"
|
|
|
|
namespace cmvr::device {
|
|
namespace {
|
|
|
|
struct BusyGuard {
|
|
std::atomic<bool>& busy;
|
|
~BusyGuard() { busy.store(false); }
|
|
};
|
|
|
|
std::vector<std::string> defaultJointNames(const std::size_t dof)
|
|
{
|
|
std::vector<std::string> names;
|
|
names.reserve(dof);
|
|
for (std::size_t i = 0; i < dof; ++i) {
|
|
names.push_back("joint_" + std::to_string(i + 1));
|
|
}
|
|
return names;
|
|
}
|
|
|
|
std::string vendorBrandName(const config::VendorRobotArmBrand brand)
|
|
{
|
|
switch (brand) {
|
|
case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM:
|
|
return "AuboARM";
|
|
case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN:
|
|
default:
|
|
return "Unknown";
|
|
}
|
|
}
|
|
|
|
using arcs::common_interface::RobotModeType;
|
|
using arcs::aubo_sdk::RobotInterfacePtr;
|
|
|
|
constexpr int kAuboServoMode = 3;
|
|
|
|
enum class CabinetIoOperation {
|
|
GetDigitalInput,
|
|
GetDigitalOutput,
|
|
SetDigitalOutput,
|
|
};
|
|
|
|
std::string lowerString(std::string value)
|
|
{
|
|
std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) {
|
|
return static_cast<char>(std::tolower(c));
|
|
});
|
|
return value;
|
|
}
|
|
|
|
bool parseJsonCommand(const std::string& request_json,
|
|
Json::Value& root,
|
|
std::string& error)
|
|
{
|
|
Json::CharReaderBuilder builder;
|
|
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
|
|
return reader->parse(
|
|
request_json.data(),
|
|
request_json.data() + request_json.size(),
|
|
&root,
|
|
&error);
|
|
}
|
|
|
|
std::string compactJson(const Json::Value& value)
|
|
{
|
|
Json::StreamWriterBuilder builder;
|
|
builder[std::string("indentation")] = "";
|
|
return Json::writeString(builder, value);
|
|
}
|
|
|
|
Json::Value& jsonMember(Json::Value& root, const char* name)
|
|
{
|
|
return *root.demand(name, name + std::strlen(name));
|
|
}
|
|
|
|
const Json::Value* findJsonMember(const Json::Value& root, const char* name)
|
|
{
|
|
return root.find(name, name + std::strlen(name));
|
|
}
|
|
|
|
bool requiredJsonString(const Json::Value& root,
|
|
const char* name,
|
|
std::string& value)
|
|
{
|
|
const Json::Value* member = findJsonMember(root, name);
|
|
if (!member || !member->isString() || member->asString().empty()) {
|
|
return false;
|
|
}
|
|
value = member->asString();
|
|
return true;
|
|
}
|
|
|
|
bool requiredJsonInt(const Json::Value& root, const char* name, int& value)
|
|
{
|
|
const Json::Value* member = findJsonMember(root, name);
|
|
if (!member || !member->isInt()) {
|
|
return false;
|
|
}
|
|
value = member->asInt();
|
|
return true;
|
|
}
|
|
|
|
bool requiredJsonBool(const Json::Value& root, const char* name, bool& value)
|
|
{
|
|
const Json::Value* member = findJsonMember(root, name);
|
|
if (!member || !member->isBool()) {
|
|
return false;
|
|
}
|
|
value = member->asBool();
|
|
return true;
|
|
}
|
|
|
|
bool parseCabinetIoOperation(const std::string& name, CabinetIoOperation& operation)
|
|
{
|
|
const std::string normalized = lowerString(name);
|
|
if (normalized == "get_di") {
|
|
operation = CabinetIoOperation::GetDigitalInput;
|
|
} else if (normalized == "get_do") {
|
|
operation = CabinetIoOperation::GetDigitalOutput;
|
|
} else if (normalized == "set_do") {
|
|
operation = CabinetIoOperation::SetDigitalOutput;
|
|
} else {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
std::string standardOutputRunstateName(
|
|
const arcs::common_interface::StandardOutputRunState runstate)
|
|
{
|
|
return arcs::common_interface::toString(runstate);
|
|
}
|
|
|
|
RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr<arcs::aubo_sdk::RpcClient>& rpc_client,
|
|
const std::string& context,
|
|
Result& result)
|
|
{
|
|
const auto robot_names = rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
result = Result::failure(ArmErrorCode::RobotNotReady,
|
|
"[AuboArm] " + context + " failed: robot name list is empty");
|
|
return nullptr;
|
|
}
|
|
|
|
auto robot_interface = rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
result = Result::failure(ArmErrorCode::RobotNotReady,
|
|
"[AuboArm] " + context + " failed: robot interface is null");
|
|
return nullptr;
|
|
}
|
|
|
|
result = Result::success();
|
|
return robot_interface;
|
|
}
|
|
|
|
bool waitForRobotMode(const RobotInterfacePtr& robot_interface,
|
|
const RobotModeType& target_mode)
|
|
{
|
|
const auto start_time = std::chrono::steady_clock::now();
|
|
while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) {
|
|
const auto current_mode = robot_interface->getRobotState()->getRobotModeType();
|
|
if (current_mode == target_mode) {
|
|
return true;
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int waitArrival(const RobotInterfacePtr& robot_interface)
|
|
{
|
|
int retry_count = 0;
|
|
int exec_id = robot_interface->getMotionControl()->getExecId();
|
|
while (exec_id == -1 && retry_count++ < 5) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
exec_id = robot_interface->getMotionControl()->getExecId();
|
|
}
|
|
if (exec_id == -1) {
|
|
return -1;
|
|
}
|
|
while (robot_interface->getMotionControl()->getExecId() != -1) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode)
|
|
{
|
|
for (int i = 0; i < 20; ++i) {
|
|
if (robot_interface->getMotionControl()->getServoModeSelect() == mode) {
|
|
return true;
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
CartesianPose poseFromVector(const std::vector<double>& values)
|
|
{
|
|
CartesianPose pose;
|
|
if (values.size() >= 6) {
|
|
pose.x = values[0];
|
|
pose.y = values[1];
|
|
pose.z = values[2];
|
|
pose.rx = values[3];
|
|
pose.ry = values[4];
|
|
pose.rz = values[5];
|
|
}
|
|
return pose;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct AuboArm::SdkState {
|
|
std::shared_ptr<arcs::aubo_sdk::RpcClient> rpc_client;
|
|
};
|
|
|
|
AuboArm::AuboArm(const config::RobotArmConfig& cfg)
|
|
: cfg_(cfg)
|
|
{
|
|
id_ = cfg.id();
|
|
if (cfg.has_vendor()) {
|
|
vendor_cfg_ = cfg.vendor();
|
|
}
|
|
|
|
ip_ = vendor_cfg_.ip();
|
|
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();
|
|
|
|
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();
|
|
model_.manufacturer = vendorBrandName(vendor_cfg_.brand());
|
|
model_.dof = dof;
|
|
model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end());
|
|
if (model_.joint_names.empty()) {
|
|
model_.joint_names = defaultJointNames(dof);
|
|
}
|
|
if (model_.joint_names.size() != dof) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_;
|
|
model_.joint_names = defaultJointNames(dof);
|
|
}
|
|
}
|
|
|
|
AuboArm::~AuboArm()
|
|
{
|
|
(void)disconnect();
|
|
}
|
|
|
|
bool AuboArm::init()
|
|
{
|
|
if (ip_.empty()) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_;
|
|
return false;
|
|
}
|
|
const auto result = connect(ip_, port_);
|
|
if (!result.ok()) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool AuboArm::stop()
|
|
{
|
|
return stopMotion().ok();
|
|
}
|
|
|
|
bool AuboArm::executeJsonCommand(const std::string& request_json,
|
|
std::string& response_json)
|
|
{
|
|
Json::Value response(Json::objectValue);
|
|
jsonMember(response, "success") = false;
|
|
const auto fail = [&](const std::string& error_code,
|
|
const std::string& error_message) {
|
|
jsonMember(response, "success") = false;
|
|
jsonMember(response, "error_code") = error_code;
|
|
jsonMember(response, "error_message") = error_message;
|
|
response_json = compactJson(response);
|
|
return false;
|
|
};
|
|
|
|
Json::Value root;
|
|
std::string parse_error;
|
|
if (!parseJsonCommand(request_json, root, parse_error)) {
|
|
return fail("invalid_json", "invalid json: " + parse_error);
|
|
}
|
|
if (!root.isObject()) {
|
|
return fail("invalid_json", "invalid json: root must be an object");
|
|
}
|
|
|
|
std::string command;
|
|
if (!requiredJsonString(root, "command", command)) {
|
|
return fail("invalid_argument",
|
|
"field 'command' is required and must be a non-empty string");
|
|
}
|
|
command = lowerString(command);
|
|
if (command != "cabinet_io") {
|
|
return fail("unsupported_command", "unsupported json command: " + command);
|
|
}
|
|
jsonMember(response, "command") = command;
|
|
|
|
std::string operation_name;
|
|
if (!requiredJsonString(root, "operation", operation_name)) {
|
|
return fail("invalid_argument",
|
|
"field 'operation' is required and must be a non-empty string");
|
|
}
|
|
operation_name = lowerString(operation_name);
|
|
CabinetIoOperation operation{};
|
|
if (!parseCabinetIoOperation(operation_name, operation)) {
|
|
return fail("invalid_operation",
|
|
"unsupported cabinet_io operation: " + operation_name);
|
|
}
|
|
jsonMember(response, "operation") = operation_name;
|
|
|
|
int index = -1;
|
|
if (!requiredJsonInt(root, "index", index) || index < 0) {
|
|
return fail("invalid_argument",
|
|
"field 'index' is required and must be a non-negative JSON integer");
|
|
}
|
|
jsonMember(response, "index") = index;
|
|
|
|
bool output_value = false;
|
|
if (operation == CabinetIoOperation::SetDigitalOutput &&
|
|
!requiredJsonBool(root, "value", output_value)) {
|
|
return fail("invalid_argument",
|
|
"field 'value' is required for set_do and must be a JSON boolean");
|
|
}
|
|
|
|
std::lock_guard lock(mutex_);
|
|
const auto ready = ensureConnected_("cabinet_io");
|
|
if (!ready.ok()) {
|
|
return fail("not_connected", ready.message);
|
|
}
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface =
|
|
getPrimaryRobotInterface(sdk_->rpc_client, "cabinet_io", interface_result);
|
|
if (!interface_result.ok() || !robot_interface) {
|
|
return fail("robot_interface_unavailable", interface_result.message);
|
|
}
|
|
|
|
auto io = robot_interface->getIoControl();
|
|
if (!io) {
|
|
return fail("io_interface_unavailable",
|
|
"[AuboArm] cabinet_io failed: IO interface is null");
|
|
}
|
|
|
|
const bool is_input = operation == CabinetIoOperation::GetDigitalInput;
|
|
const int count = is_input
|
|
? io->getStandardDigitalInputNum()
|
|
: io->getStandardDigitalOutputNum();
|
|
jsonMember(response, "count") = count;
|
|
if (index >= count) {
|
|
return fail(
|
|
"index_out_of_range",
|
|
"[AuboArm] cabinet_io index out of range: index=" +
|
|
std::to_string(index) + ", count=" + std::to_string(count));
|
|
}
|
|
|
|
if (operation == CabinetIoOperation::GetDigitalInput) {
|
|
jsonMember(response, "value") = io->getStandardDigitalInput(index);
|
|
} else {
|
|
const auto runstate = io->getStandardDigitalOutputRunstate(index);
|
|
jsonMember(response, "runstate") = standardOutputRunstateName(runstate);
|
|
jsonMember(response, "runstate_code") = static_cast<int>(runstate);
|
|
|
|
if (operation == CabinetIoOperation::GetDigitalOutput) {
|
|
jsonMember(response, "value") =
|
|
io->getStandardDigitalOutput(index);
|
|
} else {
|
|
if (runstate != arcs::common_interface::StandardOutputRunState::None) {
|
|
return fail(
|
|
"output_managed_by_runstate",
|
|
"[AuboArm] cabinet_io set_do rejected: output is managed by "
|
|
"controller runstate; configure this channel as None before writing");
|
|
}
|
|
|
|
const int ret = io->setStandardDigitalOutput(index, output_value);
|
|
jsonMember(response, "sdk_return_code") = ret;
|
|
if (ret != 0) {
|
|
return fail(
|
|
"sdk_command_failed",
|
|
"[AuboArm] cabinet_io set_do failed: sdk ret=" +
|
|
std::to_string(ret));
|
|
}
|
|
jsonMember(response, "requested_value") = output_value;
|
|
}
|
|
}
|
|
|
|
jsonMember(response, "success") = true;
|
|
response_json = compactJson(response);
|
|
return true;
|
|
} catch (const arcs::common_interface::AuboException& e) {
|
|
jsonMember(response, "sdk_return_code") = e.code();
|
|
return fail("sdk_exception",
|
|
std::string("[AuboArm] cabinet_io failed: ") + e.what());
|
|
} catch (const std::exception& e) {
|
|
return fail("sdk_exception",
|
|
std::string("[AuboArm] cabinet_io failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
ArmState AuboArm::getRobotState() const
|
|
{
|
|
ArmState state;
|
|
state.connected = connected_.load();
|
|
state.powered_on = state.connected;
|
|
state.brake_released = state.connected;
|
|
state.moving = busy_.load();
|
|
state.robot_mode = getRobotMode();
|
|
state.safety_mode = getSafetyMode();
|
|
state.control_mode = getControlMode();
|
|
state.emergency_stopped = emergency_stopped_;
|
|
state.speed_scaling = speed_scaling_;
|
|
state.actual_joint_state = getJointState();
|
|
state.target_joint_state = state.actual_joint_state;
|
|
return state;
|
|
}
|
|
|
|
JointGroupState AuboArm::getJointState() const
|
|
{
|
|
JointGroupState state;
|
|
state.position.assign(model_.dof, 0.0);
|
|
state.velocity.assign(model_.dof, 0.0);
|
|
state.effort.assign(model_.dof, 0.0);
|
|
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return state;
|
|
}
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty";
|
|
return state;
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null";
|
|
return state;
|
|
}
|
|
const auto robot_state = robot_interface->getRobotState();
|
|
const auto positions = robot_state->getJointPositions();
|
|
const auto velocities = robot_state->getJointSpeeds();
|
|
const auto n = std::min<std::size_t>(model_.dof, positions.size());
|
|
for (std::size_t i = 0; i < n; ++i) {
|
|
state.position[i] = positions[i];
|
|
}
|
|
const auto vn = std::min<std::size_t>(model_.dof, velocities.size());
|
|
for (std::size_t i = 0; i < vn; ++i) {
|
|
state.velocity[i] = velocities[i];
|
|
}
|
|
} catch (const std::exception& e) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what();
|
|
}
|
|
return state;
|
|
}
|
|
|
|
CartesianPose AuboArm::getTcpPose(FrameType frame) const
|
|
{
|
|
(void)frame;
|
|
CartesianPose pose;
|
|
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return pose;
|
|
}
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot name list is empty";
|
|
return pose;
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot interface is null";
|
|
return pose;
|
|
}
|
|
const auto pose_values = robot_interface->getRobotState()->getTcpPose();
|
|
if (pose_values.size() >= 6) {
|
|
pose.x = pose_values[0];
|
|
pose.y = pose_values[1];
|
|
pose.z = pose_values[2];
|
|
pose.rx = pose_values[3];
|
|
pose.ry = pose_values[4];
|
|
pose.rz = pose_values[5];
|
|
}
|
|
} catch (const std::exception& e) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: " << e.what();
|
|
}
|
|
|
|
return pose;
|
|
}
|
|
|
|
RobotMode AuboArm::getRobotMode() const
|
|
{
|
|
if (!connected_.load()) {
|
|
return RobotMode::Disconnected;
|
|
}
|
|
if (emergency_stopped_) {
|
|
return RobotMode::Stopped;
|
|
}
|
|
return busy_.load() ? RobotMode::Running : RobotMode::Idle;
|
|
}
|
|
|
|
Result AuboArm::torqueOn()
|
|
{
|
|
const auto ready = ensureConnected_("torqueOn");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
}
|
|
|
|
double mass = 0.0;
|
|
std::vector<double> cog(3, 0.0);
|
|
std::vector<double> aom(3, 0.0);
|
|
std::vector<double> inertia(6, 0.0);
|
|
robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
|
|
|
|
const auto current_mode = robot_interface->getRobotState()->getRobotModeType();
|
|
if (current_mode != arcs::common_interface::RobotModeType::Running) {
|
|
robot_interface->getRobotManage()->poweron();
|
|
if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle");
|
|
}
|
|
robot_interface->getRobotManage()->startup();
|
|
if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running");
|
|
}
|
|
}
|
|
emergency_stopped_ = false;
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::torqueOff()
|
|
{
|
|
const auto ready = ensureConnected_("torqueOff");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
}
|
|
robot_interface->getRobotManage()->poweroff();
|
|
if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff");
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::calibrateZeroQ(const std::string& joint_name)
|
|
{
|
|
(void)joint_name;
|
|
return unsupported_("calibrateZeroQ");
|
|
}
|
|
|
|
Result AuboArm::emergencyStop()
|
|
{
|
|
emergency_stopped_ = true;
|
|
return stopMotion();
|
|
}
|
|
|
|
Result AuboArm::setSpeedScaling(const double scaling)
|
|
{
|
|
if (scaling < 0.0 || scaling > 1.0) {
|
|
return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]");
|
|
}
|
|
speed_scaling_ = scaling;
|
|
return Result::success();
|
|
}
|
|
|
|
Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
|
|
{
|
|
std::string error;
|
|
if (!validDof_(target.position.size(), error)) {
|
|
return Result::failure(ArmErrorCode::InvalidDof, error);
|
|
}
|
|
const auto ready = ensureConnected_("moveJ");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
if (busy_.exchange(true)) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
}
|
|
BusyGuard busy_guard{busy_};
|
|
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
}
|
|
auto motion_control = robot_interface->getMotionControl();
|
|
motion_control->setSpeedFraction(speed_scaling_);
|
|
const int ret = motion_control->moveJoint(
|
|
target.position,
|
|
options.acceleration > 0.0 ? options.acceleration : 0.5,
|
|
options.velocity > 0.0 ? options.velocity : 0.5,
|
|
options.blend_radius,
|
|
0);
|
|
const auto outcome = aubo_internal::resolveMotionCommand(
|
|
ret,
|
|
arcs::common_interface::AUBO_OK,
|
|
arcs::common_interface::AUBO_REQUEST_IGNORE,
|
|
[&robot_interface]() { return waitArrival(robot_interface); });
|
|
switch (outcome) {
|
|
case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
|
|
CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret="
|
|
<< ret << " ("
|
|
<< arcs::common_interface::returnValue2Str(ret) << ")";
|
|
return Result::success();
|
|
case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
|
|
return Result::success();
|
|
case aubo_internal::MotionCommandOutcome::SubmitFailed:
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[AuboArm] moveJ failed: sdk ret=" + std::to_string(ret) +
|
|
" (" + arcs::common_interface::returnValue2Str(ret) + ")");
|
|
case aubo_internal::MotionCommandOutcome::CompletionFailed:
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete");
|
|
}
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ failed: unknown outcome");
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration)
|
|
{
|
|
std::string error;
|
|
if (!validDof_(velocity.velocity.size(), error)) {
|
|
return Result::failure(ArmErrorCode::InvalidDof, error);
|
|
}
|
|
const auto ready = ensureConnected_("speedJ");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
if (busy_.exchange(true)) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
}
|
|
BusyGuard busy_guard{busy_};
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
|
|
robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
|
|
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5;
|
|
const double resolved_duration = duration > 0.0 ? duration : 100.0;
|
|
const int ret = robot_interface->getMotionControl()->speedJoint(
|
|
velocity.velocity,
|
|
resolved_acceleration,
|
|
resolved_duration);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] speedJ failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::stopJ(double acceleration)
|
|
{
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return Result::success();
|
|
}
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0;
|
|
const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration);
|
|
busy_.store(false);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] stopJ failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame)
|
|
{
|
|
(void)frame;
|
|
const auto ready = ensureConnected_("moveL");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
if (busy_.exchange(true)) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
}
|
|
BusyGuard busy_guard{busy_};
|
|
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty");
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (!robot_interface) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null");
|
|
}
|
|
auto motion_control = robot_interface->getMotionControl();
|
|
motion_control->setSpeedFraction(speed_scaling_);
|
|
std::vector<double> tcp_offset(6, 0.0);
|
|
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
|
|
std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
|
|
const int ret = motion_control->moveLine(
|
|
pose,
|
|
options.acceleration > 0.0 ? options.acceleration : 0.5,
|
|
options.velocity > 0.0 ? options.velocity : 0.25,
|
|
options.blend_radius,
|
|
0);
|
|
const auto outcome = aubo_internal::resolveMotionCommand(
|
|
ret,
|
|
arcs::common_interface::AUBO_OK,
|
|
arcs::common_interface::AUBO_REQUEST_IGNORE,
|
|
[&robot_interface]() { return waitArrival(robot_interface); });
|
|
switch (outcome) {
|
|
case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion:
|
|
CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret="
|
|
<< ret << " ("
|
|
<< arcs::common_interface::returnValue2Str(ret) << ")";
|
|
return Result::success();
|
|
case aubo_internal::MotionCommandOutcome::CompletedAfterMotion:
|
|
return Result::success();
|
|
case aubo_internal::MotionCommandOutcome::SubmitFailed:
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[AuboArm] moveL failed: sdk ret=" + std::to_string(ret) +
|
|
" (" + arcs::common_interface::returnValue2Str(ret) + ")");
|
|
case aubo_internal::MotionCommandOutcome::CompletionFailed:
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete");
|
|
}
|
|
return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL failed: unknown outcome");
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
|
|
{
|
|
const auto ready = ensureConnected_("speedL");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
if (busy_.exchange(true)) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_);
|
|
}
|
|
BusyGuard busy_guard{busy_};
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
|
|
robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_);
|
|
std::vector<double> tcp_offset(6, 0.0);
|
|
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
|
|
|
|
std::vector<double> line_speed{velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0};
|
|
std::vector<double> angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
|
|
if (frame == FrameType::Tool) {
|
|
auto tool_frame = robot_interface->getRobotState()->getTcpPose();
|
|
if (tool_frame.size() < 6) {
|
|
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);
|
|
} else if (frame == FrameType::User) {
|
|
return Result::failure(ArmErrorCode::UnsupportedCommand,
|
|
"[AuboArm] speedL User frame requires a configured user coordinate frame");
|
|
}
|
|
|
|
std::vector<double> speed{
|
|
line_speed[0],
|
|
line_speed[1],
|
|
line_speed[2],
|
|
angular_speed[0],
|
|
angular_speed[1],
|
|
angular_speed[2],
|
|
};
|
|
|
|
const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2;
|
|
const double resolved_duration = duration > 0.0 ? duration : 100.0;
|
|
const int ret = robot_interface->getMotionControl()->speedLine(
|
|
speed,
|
|
resolved_acceleration,
|
|
resolved_duration);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] speedL failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::stopL(std::optional<double> acceleration)
|
|
{
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return Result::success();
|
|
}
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
const double resolved_acceleration =
|
|
acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0;
|
|
const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration);
|
|
busy_.store(false);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] stopL failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::stopMotion()
|
|
{
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return Result::success();
|
|
}
|
|
try {
|
|
const auto robot_names = sdk_->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
return Result::success();
|
|
}
|
|
auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front());
|
|
if (robot_interface) {
|
|
robot_interface->getMotionControl()->stopMove(true, true);
|
|
}
|
|
busy_.store(false);
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::startServoMode(const ServoOptions& options)
|
|
{
|
|
const auto ready = ensureConnected_("startServoMode");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "startServoMode", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] startServoMode failed: ret=" + std::to_string(ret));
|
|
}
|
|
if (!waitServoModeSelect(robot_interface, kAuboServoMode)) {
|
|
return Result::failure(ArmErrorCode::Timeout,
|
|
"[AuboArm] startServoMode failed: timeout waiting for servo mode");
|
|
}
|
|
servo_options_ = options;
|
|
servo_mode_.store(true);
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] startServoMode failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::servoJ(const JointPositionCommand& target)
|
|
{
|
|
std::string error;
|
|
if (!validDof_(target.position.size(), error)) {
|
|
return Result::failure(ArmErrorCode::InvalidDof, error);
|
|
}
|
|
const auto ready = ensureConnected_("servoJ");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoJ", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) {
|
|
const auto start_result = startServoMode(servo_options_);
|
|
if (!start_result.ok()) {
|
|
return start_result;
|
|
}
|
|
}
|
|
const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
|
|
const int ret = robot_interface->getMotionControl()->servoJoint(
|
|
target.position,
|
|
0.0,
|
|
0.0,
|
|
period,
|
|
servo_options_.lookahead_time,
|
|
servo_options_.gain);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] servoJ failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::servoL(const CartesianPose& target, FrameType frame)
|
|
{
|
|
const auto ready = ensureConnected_("servoL");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoL", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) {
|
|
const auto start_result = startServoMode(servo_options_);
|
|
if (!start_result.ok()) {
|
|
return start_result;
|
|
}
|
|
}
|
|
|
|
std::vector<double> pose{target.x, target.y, target.z, target.rx, target.ry, target.rz};
|
|
if (frame == FrameType::Tool) {
|
|
const auto current_pose = robot_interface->getRobotState()->getTcpPose();
|
|
if (current_pose.size() < 6) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] servoL failed: tcp pose size is less than 6");
|
|
}
|
|
pose = sdk_->rpc_client->getMath()->poseTrans(current_pose, pose);
|
|
} else if (frame == FrameType::User) {
|
|
return Result::failure(ArmErrorCode::UnsupportedCommand,
|
|
"[AuboArm] servoL User frame requires a configured user coordinate frame");
|
|
}
|
|
|
|
const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
|
|
const int ret = robot_interface->getMotionControl()->servoCartesian(
|
|
pose,
|
|
0.0,
|
|
0.0,
|
|
period,
|
|
servo_options_.lookahead_time,
|
|
servo_options_.gain);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] servoL failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity)
|
|
{
|
|
const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
|
|
return speedJ(velocity, 1.5, period);
|
|
}
|
|
|
|
Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame)
|
|
{
|
|
const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
|
|
return speedL(velocity, 1.2, period, frame);
|
|
}
|
|
|
|
Result AuboArm::stopServoMode()
|
|
{
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
servo_mode_.store(false);
|
|
return Result::success();
|
|
}
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopServoMode", interface_result);
|
|
if (!interface_result.ok()) {
|
|
return interface_result;
|
|
}
|
|
const int ret = robot_interface->getMotionControl()->setServoModeSelect(0);
|
|
servo_mode_.store(false);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] stopServoMode failed: ret=" + std::to_string(ret));
|
|
}
|
|
if (!waitServoModeSelect(robot_interface, 0)) {
|
|
return Result::failure(ArmErrorCode::Timeout,
|
|
"[AuboArm] stopServoMode failed: timeout waiting for servo mode disabled");
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] stopServoMode failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::connect(const std::string& ip, const int port)
|
|
{
|
|
std::lock_guard lock(mutex_);
|
|
if (connected_.load()) {
|
|
return Result::success();
|
|
}
|
|
if (ip.empty()) {
|
|
return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty");
|
|
}
|
|
|
|
try {
|
|
const int resolved_port = port > 0 ? port : 30004;
|
|
auto sdk_state = std::make_unique<SdkState>();
|
|
sdk_state->rpc_client = std::shared_ptr<arcs::aubo_sdk::RpcClient>(
|
|
::createRpcClient(),
|
|
[](arcs::aubo_sdk::RpcClient* client) {
|
|
if (client) {
|
|
::destroyRpcClient(client);
|
|
}
|
|
});
|
|
if (!sdk_state->rpc_client) {
|
|
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
"[AuboArm] connect failed: create RPC client failed");
|
|
}
|
|
|
|
sdk_state->rpc_client->setRequestTimeout(1000);
|
|
int ret = sdk_state->rpc_client->connect(ip, resolved_port);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
"[AuboArm] connect failed: rpc connect ret=" +
|
|
std::to_string(ret) + ", ip=" + ip +
|
|
", port=" + std::to_string(resolved_port));
|
|
}
|
|
|
|
ret = sdk_state->rpc_client->login(username_, password_);
|
|
if (ret != 0) {
|
|
if (sdk_state->rpc_client->hasConnected()) {
|
|
sdk_state->rpc_client->disconnect();
|
|
}
|
|
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
"[AuboArm] connect failed: login ret=" + std::to_string(ret));
|
|
}
|
|
|
|
const auto robot_names = sdk_state->rpc_client->getRobotNames();
|
|
if (robot_names.empty()) {
|
|
if (sdk_state->rpc_client->hasLogined()) {
|
|
sdk_state->rpc_client->logout();
|
|
}
|
|
if (sdk_state->rpc_client->hasConnected()) {
|
|
sdk_state->rpc_client->disconnect();
|
|
}
|
|
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
"[AuboArm] connect failed: robot name list is empty");
|
|
}
|
|
|
|
ip_ = ip;
|
|
port_ = resolved_port;
|
|
sdk_ = std::move(sdk_state);
|
|
connected_.store(true);
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
sdk_.reset();
|
|
connected_.store(false);
|
|
return Result::failure(ArmErrorCode::ConnectionFailed,
|
|
std::string("[AuboArm] connect failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::disconnect()
|
|
{
|
|
std::lock_guard lock(mutex_);
|
|
try {
|
|
if (sdk_ && sdk_->rpc_client) {
|
|
if (sdk_->rpc_client->hasLogined()) {
|
|
sdk_->rpc_client->logout();
|
|
}
|
|
if (sdk_->rpc_client->hasConnected()) {
|
|
sdk_->rpc_client->disconnect();
|
|
}
|
|
}
|
|
} catch (const std::exception& e) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what();
|
|
}
|
|
sdk_.reset();
|
|
connected_.store(false);
|
|
busy_.store(false);
|
|
servo_mode_.store(false);
|
|
return Result::success();
|
|
}
|
|
|
|
Result AuboArm::shutdown()
|
|
{
|
|
(void)stopMotion();
|
|
return disconnect();
|
|
}
|
|
|
|
Result AuboArm::loadProgram(const std::string& program_name)
|
|
{
|
|
if (program_name.empty()) {
|
|
return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] loadProgram failed: program name is empty");
|
|
}
|
|
const auto ready = ensureConnected_("loadProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
try {
|
|
const int ret = sdk_->rpc_client->getRuntimeMachine()->loadProgram(program_name);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] loadProgram failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] loadProgram failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::playProgram()
|
|
{
|
|
const auto ready = ensureConnected_("playProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
try {
|
|
const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram();
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] playProgram failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] playProgram failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::pauseProgram()
|
|
{
|
|
const auto ready = ensureConnected_("pauseProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
try {
|
|
const int ret = sdk_->rpc_client->getRuntimeMachine()->pause();
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] pauseProgram failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] pauseProgram failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
Result AuboArm::stopProgram()
|
|
{
|
|
const auto ready = ensureConnected_("stopProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
try {
|
|
const int ret = sdk_->rpc_client->getRuntimeMachine()->abort();
|
|
busy_.store(false);
|
|
if (ret != 0) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
"[AuboArm] stopProgram failed: ret=" + std::to_string(ret));
|
|
}
|
|
return Result::success();
|
|
} catch (const std::exception& e) {
|
|
return Result::failure(ArmErrorCode::CommandFailed,
|
|
std::string("[AuboArm] stopProgram failed: ") + e.what());
|
|
}
|
|
}
|
|
|
|
std::vector<double> AuboArm::ik(const std::string& base_link,
|
|
const std::string& ee_link,
|
|
const CartesianPose& pose)
|
|
{
|
|
(void)base_link;
|
|
(void)ee_link;
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] ik failed: arm is not connected";
|
|
return {};
|
|
}
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "ik", interface_result);
|
|
if (!interface_result.ok()) {
|
|
CMVR_LOG(ERROR) << interface_result.message;
|
|
return {};
|
|
}
|
|
const auto qnear = getJointState().position;
|
|
const std::vector<double> target_pose{pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz};
|
|
const auto result = robot_interface->getRobotAlgorithm()->inverseKinematics(qnear, target_pose);
|
|
const int ret = std::get<1>(result);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] ik failed: ret=" << ret;
|
|
return {};
|
|
}
|
|
return std::get<0>(result);
|
|
} catch (const std::exception& e) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] ik failed: " << e.what();
|
|
}
|
|
return {};
|
|
}
|
|
|
|
CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link)
|
|
{
|
|
(void)base_link;
|
|
(void)ee_link;
|
|
return getTcpPose(FrameType::Base);
|
|
}
|
|
|
|
CartesianPose AuboArm::fk(bool is_tcp)
|
|
{
|
|
if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
|
|
return {};
|
|
}
|
|
try {
|
|
Result interface_result;
|
|
auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "fk", interface_result);
|
|
if (!interface_result.ok()) {
|
|
CMVR_LOG(ERROR) << interface_result.message;
|
|
return {};
|
|
}
|
|
const auto q = getJointState().position;
|
|
if (q.size() != model_.dof) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] fk failed: joint state dof mismatch";
|
|
return {};
|
|
}
|
|
const auto result = is_tcp
|
|
? robot_interface->getRobotAlgorithm()->forwardKinematics(q)
|
|
: robot_interface->getRobotAlgorithm()->forwardToolKinematics(q);
|
|
const int ret = std::get<1>(result);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] fk failed: ret=" << ret;
|
|
return {};
|
|
}
|
|
return poseFromVector(std::get<0>(result));
|
|
} catch (const std::exception& e) {
|
|
CMVR_LOG(ERROR) << "[AuboArm] fk failed: " << e.what();
|
|
}
|
|
return {};
|
|
}
|
|
|
|
Result AuboArm::unsupported_(const std::string& name) const
|
|
{
|
|
const std::string message = "[AuboArm] " + name + " is not implemented";
|
|
CMVR_LOG(ERROR) << message;
|
|
return Result::failure(ArmErrorCode::UnsupportedCommand, message);
|
|
}
|
|
|
|
bool AuboArm::validDof_(const std::size_t size, std::string& error) const
|
|
{
|
|
if (size != model_.dof) {
|
|
error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) +
|
|
", actual=" + std::to_string(size);
|
|
CMVR_LOG(ERROR) << error;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
Result AuboArm::ensureConnected_(const std::string& context) const
|
|
{
|
|
if (!connected_.load()) {
|
|
return Result::failure(ArmErrorCode::NotConnected,
|
|
"[AuboArm] " + context + " failed: arm is not connected");
|
|
}
|
|
if (!sdk_ || !sdk_->rpc_client) {
|
|
return Result::failure(ArmErrorCode::NotConnected,
|
|
"[AuboArm] " + context + " failed: SDK client is null");
|
|
}
|
|
return Result::success();
|
|
}
|
|
|
|
} // namespace cmvr::device
|