886 lines
25 KiB
C++
886 lines
25 KiB
C++
#include "huayan_arm.h"
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
|
|
#include "huayan_arm/v1.0/include/HR_Pro.h"
|
|
#include "common/base/logging/logger.h"
|
|
|
|
namespace cmvr::device {
|
|
namespace {
|
|
|
|
constexpr double kPi = 3.14159265358979323846;
|
|
constexpr double kDefaultMoveJVelocityDeg = 30.0;
|
|
constexpr double kDefaultMoveJAccelerationDeg = 60.0;
|
|
constexpr double kDefaultMoveLVelocityMm = 100.0;
|
|
constexpr double kDefaultMoveLAccelerationMm = 200.0;
|
|
|
|
double radToDeg(const double value)
|
|
{
|
|
return value * 180.0 / kPi;
|
|
}
|
|
|
|
double degToRad(const double value)
|
|
{
|
|
return value * kPi / 180.0;
|
|
}
|
|
|
|
double metersToMm(const double value)
|
|
{
|
|
return value * 1000.0;
|
|
}
|
|
|
|
double mmToMeters(const double value)
|
|
{
|
|
return value / 1000.0;
|
|
}
|
|
|
|
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::array<double, 6> toSix(const std::vector<double>& values, const double fill = 0.0)
|
|
{
|
|
std::array<double, 6> out{fill, fill, fill, fill, fill, fill};
|
|
const auto n = std::min<std::size_t>(out.size(), values.size());
|
|
for (std::size_t i = 0; i < n; ++i) {
|
|
out[i] = values[i];
|
|
}
|
|
return out;
|
|
}
|
|
|
|
std::vector<double> poseToHrCoord(const CartesianPose& pose)
|
|
{
|
|
return {metersToMm(pose.x),
|
|
metersToMm(pose.y),
|
|
metersToMm(pose.z),
|
|
radToDeg(pose.rx),
|
|
radToDeg(pose.ry),
|
|
radToDeg(pose.rz)};
|
|
}
|
|
|
|
std::vector<double> zeroHrFrame()
|
|
{
|
|
return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
|
}
|
|
|
|
} // namespace
|
|
|
|
HuayanRobot::HuayanRobot(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() : 10003;
|
|
tcp_name_ = vendor_cfg_.tool_frame().empty() ? "TCP" : vendor_cfg_.tool_frame();
|
|
ucs_name_ = vendor_cfg_.base_frame().empty() ? "Base" : vendor_cfg_.base_frame();
|
|
|
|
const auto dof = vendor_cfg_.dof() > 0 ? static_cast<std::size_t>(vendor_cfg_.dof()) : 6U;
|
|
model_.name = vendor_cfg_.model().empty() ? "HuayanRobot" : vendor_cfg_.model();
|
|
model_.manufacturer = "Huayan";
|
|
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) << "[HuayanRobot] joint_names size mismatch, id=" << id_;
|
|
model_.joint_names = defaultJointNames(dof);
|
|
}
|
|
}
|
|
|
|
HuayanRobot::~HuayanRobot()
|
|
{
|
|
(void)disconnect();
|
|
}
|
|
|
|
bool HuayanRobot::init()
|
|
{
|
|
if (ip_.empty()) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] ip is empty, id=" << id_;
|
|
return false;
|
|
}
|
|
const auto result = connect(ip_, port_);
|
|
if (!result.ok()) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool HuayanRobot::stop()
|
|
{
|
|
return stopMotion().ok();
|
|
}
|
|
|
|
ArmState HuayanRobot::getRobotState() const
|
|
{
|
|
const auto hr_state = readHrState_();
|
|
|
|
ArmState state;
|
|
state.connected = isConnected();
|
|
state.powered_on = hr_state.valid ? hr_state.electrified != 0 : state.connected;
|
|
state.brake_released = hr_state.valid ? hr_state.brake != 0 : state.connected;
|
|
state.moving = hr_state.valid ? hr_state.moving != 0 : busy_.load();
|
|
state.program_running = state.moving;
|
|
state.protective_stopped = hr_state.valid ? hr_state.safeguard != 0 : false;
|
|
state.emergency_stopped = hr_state.valid ? hr_state.emergency_stop != 0 : false;
|
|
state.fault = hr_state.valid ? hr_state.error != 0 : false;
|
|
state.robot_mode = getRobotMode();
|
|
state.safety_mode = getSafetyMode();
|
|
state.control_mode = getControlMode();
|
|
state.speed_scaling = speed_scaling_;
|
|
state.actual_joint_state = getJointState();
|
|
state.target_joint_state = state.actual_joint_state;
|
|
state.actual_tcp_pose = readTcpPose_();
|
|
state.actual_tcp_velocity = readTcpVelocity_();
|
|
return state;
|
|
}
|
|
|
|
JointGroupState HuayanRobot::getJointState() const
|
|
{
|
|
JointGroupState state;
|
|
state.position = readJointPositionRad_();
|
|
state.velocity = readJointVelocityRad_();
|
|
state.effort.assign(model_.dof, 0.0);
|
|
return state;
|
|
}
|
|
|
|
CartesianPose HuayanRobot::getTcpPose(const FrameType frame) const
|
|
{
|
|
(void)frame;
|
|
return readTcpPose_();
|
|
}
|
|
|
|
RobotMode HuayanRobot::getRobotMode() const
|
|
{
|
|
if (!isConnected()) {
|
|
return RobotMode::Disconnected;
|
|
}
|
|
|
|
const auto state = readHrState_();
|
|
if (!state.valid) {
|
|
return busy_.load() ? RobotMode::Running : RobotMode::Idle;
|
|
}
|
|
if (state.error != 0) {
|
|
return RobotMode::Fault;
|
|
}
|
|
if (state.emergency_stop != 0) {
|
|
return RobotMode::Stopped;
|
|
}
|
|
if (state.paused != 0) {
|
|
return RobotMode::Paused;
|
|
}
|
|
if (state.electrified == 0) {
|
|
return RobotMode::PowerOff;
|
|
}
|
|
return state.moving != 0 ? RobotMode::Running : RobotMode::Idle;
|
|
}
|
|
|
|
SafetyMode HuayanRobot::getSafetyMode() const
|
|
{
|
|
const auto state = readHrState_();
|
|
if (!state.valid) {
|
|
return SafetyMode::Unknown;
|
|
}
|
|
if (state.error != 0) {
|
|
return SafetyMode::Fault;
|
|
}
|
|
if (state.emergency_stop != 0) {
|
|
return SafetyMode::EmergencyStop;
|
|
}
|
|
if (state.safeguard != 0) {
|
|
return SafetyMode::SafeguardStop;
|
|
}
|
|
return SafetyMode::Normal;
|
|
}
|
|
|
|
Result HuayanRobot::torqueOn()
|
|
{
|
|
const auto ready = ensureConnected_("torqueOn");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return hrResult_(HRIF_GrpEnable(box_id_, robot_id_), "GrpEnable");
|
|
}
|
|
|
|
Result HuayanRobot::torqueOff()
|
|
{
|
|
|
|
const auto ready = ensureConnected_("torqueOff");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return hrResult_(HRIF_GrpDisable(box_id_, robot_id_), "GrpDisable");
|
|
}
|
|
|
|
Result HuayanRobot::calibrateZeroQ(const std::string& joint_name)
|
|
{
|
|
(void)joint_name;
|
|
return unsupported_("calibrateZeroQ");
|
|
}
|
|
|
|
Result HuayanRobot::emergencyStop()
|
|
{
|
|
return stopMotion();
|
|
}
|
|
|
|
Result HuayanRobot::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;
|
|
if (isConnected()) {
|
|
return hrResult_(HRIF_SetOverride(box_id_, robot_id_, scaling), "SetOverride");
|
|
}
|
|
return Result::success();
|
|
}
|
|
|
|
bool HuayanRobot::isProtectiveStopped() const
|
|
{
|
|
const auto state = readHrState_();
|
|
return state.valid && state.safeguard != 0;
|
|
}
|
|
|
|
bool HuayanRobot::isEmergencyStopped() const
|
|
{
|
|
const auto state = readHrState_();
|
|
return state.valid && state.emergency_stop != 0;
|
|
}
|
|
|
|
bool HuayanRobot::isFault() const
|
|
{
|
|
const auto state = readHrState_();
|
|
return state.valid && state.error != 0;
|
|
}
|
|
|
|
Result HuayanRobot::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, "[HuayanRobot] arm is busy: " + id_);
|
|
}
|
|
|
|
std::array<double, 6> q_deg{};
|
|
for (std::size_t i = 0; i < std::min<std::size_t>(target.position.size(), q_deg.size()); ++i) {
|
|
q_deg[i] = radToDeg(target.position[i]);
|
|
}
|
|
|
|
const double velocity = options.velocity > 0.0 ? radToDeg(options.velocity) : kDefaultMoveJVelocityDeg;
|
|
const double acceleration = options.acceleration > 0.0 ? radToDeg(options.acceleration) : kDefaultMoveJAccelerationDeg;
|
|
const double blend = metersToMm(options.blend_radius);
|
|
const std::string command_id = nextCommandId_();
|
|
|
|
const int ret = HRIF_MoveJ(box_id_, robot_id_,
|
|
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
|
|
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,
|
|
1, 0, 0, 0, command_id);
|
|
if (ret != 0) {
|
|
busy_.store(false);
|
|
return hrResult_(ret, "moveJ");
|
|
}
|
|
const auto wait_result = waitMotionDone_("moveJ", 60000);
|
|
busy_.store(false);
|
|
return wait_result;
|
|
}
|
|
|
|
Result HuayanRobot::speedJ(const JointVelocityCommand& velocity, const double acceleration, const 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;
|
|
}
|
|
|
|
std::array<double, 6> qd_deg{};
|
|
for (std::size_t i = 0; i < std::min<std::size_t>(velocity.velocity.size(), qd_deg.size()); ++i) {
|
|
qd_deg[i] = radToDeg(velocity.velocity[i]);
|
|
}
|
|
const double acc_deg = acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg;
|
|
const double runtime = duration > 0.0 ? duration : 0.1;
|
|
|
|
const int ret = HRIF_SpeedJ(box_id_, robot_id_,
|
|
qd_deg[0], qd_deg[1], qd_deg[2], qd_deg[3], qd_deg[4], qd_deg[5],
|
|
acc_deg, runtime);
|
|
if (ret != 0) {
|
|
busy_.store(false);
|
|
return hrResult_(ret, "SpeedJ");
|
|
}
|
|
const auto wait_result = waitMotionDone_("SpeedJ", 60000);
|
|
busy_.store(false);
|
|
return wait_result;
|
|
}
|
|
|
|
Result HuayanRobot::stopJ(const double acceleration)
|
|
{
|
|
(void)acceleration;
|
|
return stopMotion();
|
|
}
|
|
|
|
Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame)
|
|
{
|
|
(void)frame;
|
|
const auto ready = ensureConnected_("moveL");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
if (busy_.exchange(true)) {
|
|
return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_);
|
|
}
|
|
|
|
const auto pose = poseToHrCoord(target);
|
|
const auto q_deg = toSix(currentJointPositionDeg_());
|
|
const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm;
|
|
const double acceleration = options.acceleration > 0.0 ? metersToMm(options.acceleration) : kDefaultMoveLAccelerationMm;
|
|
const double blend = metersToMm(options.blend_radius);
|
|
const std::string command_id = nextCommandId_();
|
|
|
|
const int ret = HRIF_MoveL(box_id_, robot_id_,
|
|
pose[0], pose[1], pose[2], pose[3], pose[4], pose[5],
|
|
q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5],
|
|
tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend,
|
|
0, 0, 0, command_id);
|
|
if (ret != 0) {
|
|
busy_.store(false);
|
|
return hrResult_(ret, "moveL");
|
|
}
|
|
const auto wait_result = waitMotionDone_("moveL", 60000);
|
|
busy_.store(false);
|
|
return wait_result;
|
|
}
|
|
|
|
Result HuayanRobot::speedL(const CartesianVelocity& velocity,
|
|
const double acceleration,
|
|
const double duration,
|
|
const FrameType frame)
|
|
{
|
|
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);
|
|
|
|
const double linear_acc_mm =
|
|
acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm;
|
|
|
|
const double angular_acc_deg =
|
|
acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg;
|
|
|
|
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);
|
|
if (ret != 0) {
|
|
busy_.store(false);
|
|
return hrResult_(ret, "SpeedL");
|
|
}
|
|
const auto wait_result = waitMotionDone_("SpeedL", 60000);
|
|
busy_.store(false);
|
|
return wait_result;
|
|
}
|
|
|
|
Result HuayanRobot::stopL(const double acceleration)
|
|
{
|
|
(void)acceleration;
|
|
return stopMotion();
|
|
}
|
|
|
|
Result HuayanRobot::stopMotion()
|
|
{
|
|
if (!isConnected()) {
|
|
busy_.store(false);
|
|
servo_mode_.store(false);
|
|
return Result::success();
|
|
}
|
|
|
|
const auto result = hrResult_(HRIF_GrpStop(box_id_, robot_id_), "GrpStop");
|
|
busy_.store(false);
|
|
servo_mode_.store(false);
|
|
return result;
|
|
}
|
|
|
|
Result HuayanRobot::startServoMode(const ServoOptions& options)
|
|
{
|
|
const auto ready = ensureConnected_("startServoMode");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
const double period = options.period > 0.0 ? options.period : 0.008;
|
|
const double lookahead = options.lookahead_time > 0.0 ? options.lookahead_time : 0.1;
|
|
const auto result = hrResult_(HRIF_StartServo(box_id_, robot_id_, period, lookahead), "StartServo");
|
|
if (result.ok()) {
|
|
servo_mode_.store(true);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Result HuayanRobot::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;
|
|
}
|
|
|
|
std::array<double, 6> q_deg{};
|
|
for (std::size_t i = 0; i < std::min<std::size_t>(target.position.size(), q_deg.size()); ++i) {
|
|
q_deg[i] = radToDeg(target.position[i]);
|
|
}
|
|
return hrResult_(HRIF_PushServoJ(box_id_, robot_id_,
|
|
q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5]),
|
|
"servoJ");
|
|
}
|
|
|
|
Result HuayanRobot::servoL(const CartesianPose& target, const FrameType frame)
|
|
{
|
|
(void)frame;
|
|
const auto ready = ensureConnected_("servoL");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
|
|
auto coord = poseToHrCoord(target);
|
|
auto ucs = zeroHrFrame();
|
|
auto tcp = zeroHrFrame();
|
|
return hrResult_(HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp), "servoL");
|
|
}
|
|
|
|
Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity)
|
|
{
|
|
(void)velocity;
|
|
return unsupported_("servoSpeedJ");
|
|
}
|
|
|
|
Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameType frame)
|
|
{
|
|
(void)velocity;
|
|
(void)frame;
|
|
return unsupported_("servoSpeedL");
|
|
}
|
|
|
|
Result HuayanRobot::stopServoMode()
|
|
{
|
|
servo_mode_.store(false);
|
|
return stopMotion();
|
|
}
|
|
|
|
Result HuayanRobot::connect(const std::string& ip, const int port)
|
|
{
|
|
if (connected_.load()) {
|
|
return Result::success();
|
|
}
|
|
if (ip.empty()) {
|
|
return Result::failure(ArmErrorCode::InvalidArgument, "[HuayanRobot] ip is empty");
|
|
}
|
|
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
const int use_port = port > 0 ? port : 10003;
|
|
const auto result = hrResult_(HRIF_Connect(box_id_, ip.c_str(), static_cast<unsigned short>(use_port)),
|
|
"Connect");
|
|
if (!result.ok()) {
|
|
connected_.store(false);
|
|
return result;
|
|
}
|
|
ip_ = ip;
|
|
port_ = use_port;
|
|
connected_.store(true);
|
|
return Result::success();
|
|
}
|
|
|
|
Result HuayanRobot::disconnect()
|
|
{
|
|
if (connected_.load() || HRIF_IsConnected(box_id_)) {
|
|
const auto result = hrResult_(HRIF_DisConnect(box_id_), "DisConnect");
|
|
connected_.store(false);
|
|
busy_.store(false);
|
|
servo_mode_.store(false);
|
|
return result;
|
|
}
|
|
|
|
connected_.store(false);
|
|
busy_.store(false);
|
|
servo_mode_.store(false);
|
|
return Result::success();
|
|
}
|
|
|
|
bool HuayanRobot::isConnected() const
|
|
{
|
|
return connected_.load() && HRIF_IsConnected(box_id_);
|
|
}
|
|
|
|
Result HuayanRobot::shutdown()
|
|
{
|
|
if (!isConnected()) {
|
|
return Result::success();
|
|
}
|
|
auto result = hrResult_(HRIF_ShutdownRobot(box_id_), "ShutdownRobot");
|
|
if (!result.ok()) {
|
|
return result;
|
|
}
|
|
return disconnect();
|
|
}
|
|
|
|
Result HuayanRobot::clearFault()
|
|
{
|
|
const auto ready = ensureConnected_("clearFault");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
return hrResult_(HRIF_GrpReset(box_id_, robot_id_), "GrpReset");
|
|
}
|
|
|
|
Result HuayanRobot::loadProgram(const std::string& program_name)
|
|
{
|
|
(void)program_name;
|
|
return Result::success();
|
|
}
|
|
|
|
Result HuayanRobot::playProgram()
|
|
{
|
|
const auto ready = ensureConnected_("playProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
return hrResult_(HRIF_StartScript(box_id_), "StartScript");
|
|
}
|
|
|
|
Result HuayanRobot::pauseProgram()
|
|
{
|
|
const auto ready = ensureConnected_("pauseProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
return hrResult_(HRIF_PauseScript(box_id_), "PauseScript");
|
|
}
|
|
|
|
Result HuayanRobot::stopProgram()
|
|
{
|
|
const auto ready = ensureConnected_("stopProgram");
|
|
if (!ready.ok()) {
|
|
return ready;
|
|
}
|
|
return hrResult_(HRIF_StopScript(box_id_), "StopScript");
|
|
}
|
|
|
|
std::vector<double> HuayanRobot::ik(const std::string& base_link,
|
|
const std::string& ee_link,
|
|
const CartesianPose& pose)
|
|
{
|
|
(void)base_link;
|
|
(void)ee_link;
|
|
(void)pose;
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] ik is not implemented";
|
|
return {};
|
|
}
|
|
|
|
CartesianPose HuayanRobot::fk(const std::string& base_link, const std::string& ee_link)
|
|
{
|
|
(void)base_link;
|
|
(void)ee_link;
|
|
return readTcpPose_();
|
|
}
|
|
|
|
CartesianPose HuayanRobot::fk(const bool is_tcp)
|
|
{
|
|
(void)is_tcp;
|
|
return readTcpPose_();
|
|
}
|
|
|
|
CartesianVelocity HuayanRobot::getSpeedLCommandTwistBase() const
|
|
{
|
|
return readTcpVelocity_();
|
|
}
|
|
|
|
Result HuayanRobot::ensureConnected_(const std::string& context) const
|
|
{
|
|
if (!isConnected()) {
|
|
return Result::failure(ArmErrorCode::NotConnected,
|
|
"[HuayanRobot] " + context + " failed: arm is not connected");
|
|
}
|
|
return Result::success();
|
|
}
|
|
|
|
Result HuayanRobot::unsupported_(const std::string& name) const
|
|
{
|
|
const std::string message = "[HuayanRobot] " + name + " is not implemented";
|
|
CMVR_LOG(ERROR) << message;
|
|
return Result::failure(ArmErrorCode::UnsupportedCommand, message);
|
|
}
|
|
|
|
Result HuayanRobot::hrResult_(const int code, const std::string& context) const
|
|
{
|
|
if (code == 0) {
|
|
return Result::success();
|
|
}
|
|
|
|
std::string sdk_message;
|
|
(void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message);
|
|
std::ostringstream oss;
|
|
oss << "[HuayanRobot] " << context << " failed, code=" << code;
|
|
if (!sdk_message.empty()) {
|
|
oss << ", message=" << sdk_message;
|
|
}
|
|
const auto message = oss.str();
|
|
CMVR_LOG(ERROR) << message;
|
|
return Result::failure(ArmErrorCode::CommandFailed, message);
|
|
}
|
|
|
|
bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const
|
|
{
|
|
if (size != model_.dof) {
|
|
error = "[HuayanRobot] command dof mismatch, expected=" + std::to_string(model_.dof) +
|
|
", actual=" + std::to_string(size);
|
|
CMVR_LOG(ERROR) << error;
|
|
return false;
|
|
}
|
|
if (model_.dof > 6) {
|
|
error = "[HuayanRobot] command dof exceeds SDK limit: " + std::to_string(model_.dof);
|
|
CMVR_LOG(ERROR) << error;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
HuayanRobot::HrState HuayanRobot::readHrState_() const
|
|
{
|
|
HrState state;
|
|
if (!isConnected()) {
|
|
return state;
|
|
}
|
|
|
|
const int ret = HRIF_ReadRobotState(box_id_, robot_id_,
|
|
state.moving,
|
|
state.enabled,
|
|
state.error,
|
|
state.error_code,
|
|
state.error_axis,
|
|
state.brake,
|
|
state.paused,
|
|
state.emergency_stop,
|
|
state.safeguard,
|
|
state.electrified,
|
|
state.connected_to_box,
|
|
state.blending_done,
|
|
state.in_pos);
|
|
state.valid = ret == 0;
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] read robot state failed, code=" << ret;
|
|
}
|
|
return state;
|
|
}
|
|
|
|
std::vector<double> HuayanRobot::readJointPositionRad_() const
|
|
{
|
|
std::vector<double> q(model_.dof, 0.0);
|
|
if (!isConnected()) {
|
|
return q;
|
|
}
|
|
|
|
double j1 = 0.0;
|
|
double j2 = 0.0;
|
|
double j3 = 0.0;
|
|
double j4 = 0.0;
|
|
double j5 = 0.0;
|
|
double j6 = 0.0;
|
|
const int ret = HRIF_ReadActJointPos(box_id_, robot_id_, j1, j2, j3, j4, j5, j6);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] read joint position failed, code=" << ret;
|
|
return q;
|
|
}
|
|
|
|
const std::array<double, 6> values{j1, j2, j3, j4, j5, j6};
|
|
for (std::size_t i = 0; i < std::min<std::size_t>(q.size(), values.size()); ++i) {
|
|
q[i] = degToRad(values[i]);
|
|
}
|
|
return q;
|
|
}
|
|
|
|
std::vector<double> HuayanRobot::readJointVelocityRad_() const
|
|
{
|
|
std::vector<double> qd(model_.dof, 0.0);
|
|
if (!isConnected()) {
|
|
return qd;
|
|
}
|
|
|
|
double j1 = 0.0;
|
|
double j2 = 0.0;
|
|
double j3 = 0.0;
|
|
double j4 = 0.0;
|
|
double j5 = 0.0;
|
|
double j6 = 0.0;
|
|
const int ret = HRIF_ReadActJointVel(box_id_, robot_id_, j1, j2, j3, j4, j5, j6);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] read joint velocity failed, code=" << ret;
|
|
return qd;
|
|
}
|
|
|
|
const std::array<double, 6> values{j1, j2, j3, j4, j5, j6};
|
|
for (std::size_t i = 0; i < std::min<std::size_t>(qd.size(), values.size()); ++i) {
|
|
qd[i] = degToRad(values[i]);
|
|
}
|
|
return qd;
|
|
}
|
|
|
|
CartesianPose HuayanRobot::readTcpPose_() const
|
|
{
|
|
CartesianPose pose;
|
|
if (!isConnected()) {
|
|
return pose;
|
|
}
|
|
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
double z = 0.0;
|
|
double rx = 0.0;
|
|
double ry = 0.0;
|
|
double rz = 0.0;
|
|
const int ret = HRIF_ReadActTcpPos(box_id_, robot_id_, x, y, z, rx, ry, rz);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] read tcp pose failed, code=" << ret;
|
|
return pose;
|
|
}
|
|
|
|
pose.x = mmToMeters(x);
|
|
pose.y = mmToMeters(y);
|
|
pose.z = mmToMeters(z);
|
|
pose.rx = degToRad(rx);
|
|
pose.ry = degToRad(ry);
|
|
pose.rz = degToRad(rz);
|
|
return pose;
|
|
}
|
|
|
|
CartesianVelocity HuayanRobot::readTcpVelocity_() const
|
|
{
|
|
CartesianVelocity velocity;
|
|
if (!isConnected()) {
|
|
return velocity;
|
|
}
|
|
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
double z = 0.0;
|
|
double rx = 0.0;
|
|
double ry = 0.0;
|
|
double rz = 0.0;
|
|
const int ret = HRIF_ReadActTcpVel(box_id_, robot_id_, x, y, z, rx, ry, rz);
|
|
if (ret != 0) {
|
|
CMVR_LOG(ERROR) << "[HuayanRobot] read tcp velocity failed, code=" << ret;
|
|
return velocity;
|
|
}
|
|
|
|
velocity.vx = mmToMeters(x);
|
|
velocity.vy = mmToMeters(y);
|
|
velocity.vz = mmToMeters(z);
|
|
velocity.wx = degToRad(rx);
|
|
velocity.wy = degToRad(ry);
|
|
velocity.wz = degToRad(rz);
|
|
return velocity;
|
|
}
|
|
|
|
std::vector<double> HuayanRobot::currentJointPositionDeg_() const
|
|
{
|
|
const auto q_rad = readJointPositionRad_();
|
|
std::vector<double> q_deg(q_rad.size(), 0.0);
|
|
for (std::size_t i = 0; i < q_rad.size(); ++i) {
|
|
q_deg[i] = radToDeg(q_rad[i]);
|
|
}
|
|
return q_deg;
|
|
}
|
|
|
|
std::string HuayanRobot::nextCommandId_() const
|
|
{
|
|
return id_ + "_" + std::to_string(++command_seq_);
|
|
}
|
|
|
|
Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeout_ms) const
|
|
{
|
|
const auto start = std::chrono::steady_clock::now();
|
|
while (true) {
|
|
bool done = false;
|
|
const int ret = HRIF_IsMotionDone(box_id_, robot_id_, done);
|
|
if (ret != 0) {
|
|
return hrResult_(ret, "IsMotionDone(" + context + ")");
|
|
}
|
|
const auto state = readHrState_();
|
|
if (state.valid) {
|
|
if (state.error != 0) {
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[HuayanRobot] " + context + " failed: robot error, code=" +
|
|
std::to_string(state.error_code));
|
|
}
|
|
|
|
if (state.emergency_stop != 0) {
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[HuayanRobot] " + context + " failed: emergency stop");
|
|
}
|
|
|
|
if (state.safeguard != 0) {
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[HuayanRobot] " + context + " failed: safeguard stop");
|
|
}
|
|
}
|
|
|
|
if (done) {
|
|
return Result::success();
|
|
}
|
|
|
|
const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
|
std::chrono::steady_clock::now() - start).count();
|
|
|
|
if (elapsed > timeout_ms) {
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
(void)HRIF_GrpStop(box_id_, robot_id_);
|
|
}
|
|
|
|
return Result::failure(
|
|
ArmErrorCode::CommandFailed,
|
|
"[HuayanRobot] " + context + " timeout");
|
|
}
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
}
|
|
}
|
|
|
|
} // namespace cmvr::device
|