增加华沿机器人机械臂组建并完成测试

cmvr-es/config/devices/arm/huayan_arm.pb.txt

@@ -0,0 +1,21 @@
+arm {
+  robot_arms {
+    id: "huayan_arm"
+
+    vendor {
+      brand: VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM
+      ip: "192.168.0.10"
+      port: 10003
+      model: "HuayanRobot"
+      dof: 6
+      joint_names: "joint_1"
+      joint_names: "joint_2"
+      joint_names: "joint_3"
+      joint_names: "joint_4"
+      joint_names: "joint_5"
+      joint_names: "joint_6"
+      base_frame: "Base"
+      tool_frame: "TCP"
+    }
+  }
+}

cmvr-es/config/manager/device_manager.pb.txt

@@ -7,14 +7,14 @@ device_manager {
     id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -28,14 +28,14 @@ device_manager {
     id: "ti5_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -45,6 +45,13 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "huayan_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/huayan_arm.pb.txt"
+    enable: true
+  }
+
   devices {
     id: "bio_head"
     type: DEVICE_TYPE_BIO_HEAD_ROBOT

cmvr-es/config/manager/task_manager.pb.txt

@@ -5,7 +5,7 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
     config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: true
+    enable: false
   }
   tasks {
     id: "grpc_server"

cmvr-es/devices/arm/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(motor_robot_arm)
 add_subdirectory(aubo_arm)
+add_subdirectory(huayan_arm)
 
 add_library(robot_arm INTERFACE)
 
@@ -9,6 +10,7 @@ target_link_libraries(robot_arm
         INTERFACE
         cmvr_es::device::motor_robot_arm
         cmvr_es::device::aubo_arm
+        cmvr_es::device::huayan_arm
         cmvr_es::proto
 )
 

cmvr-es/devices/arm/huayan_arm/CMakeLists.txt

@@ -0,0 +1,28 @@
+add_library(huayan_arm SHARED huayan_arm.cpp)
+
+set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0)
+
+target_include_directories(huayan_arm
+        PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+        PRIVATE
+        ${HUAYAN_ARM_SDK_DIR}/include
+)
+
+target_link_directories(huayan_arm
+        PRIVATE
+        ${HUAYAN_ARM_SDK_DIR}/lib
+)
+
+target_link_libraries(huayan_arm
+        PUBLIC
+        cmvr_es::proto
+        PRIVATE
+        HR_Pro
+        glog
+)
+
+add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm)
+
+install(TARGETS huayan_arm LIBRARY DESTINATION lib)
+install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib)

cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp

@@ -0,0 +1,885 @@
+#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

cmvr-es/devices/arm/huayan_arm/huayan_arm.h

@@ -0,0 +1,143 @@
+//
+// Created by cmvr on 2026/6/29.
+//
+
+#ifndef CMVR_ES_HUAYAN_ARM_H
+#define CMVR_ES_HUAYAN_ARM_H
+
+#ifndef CMVR_ES_HUAYAN_ROBOT_H
+#define CMVR_ES_HUAYAN_ROBOT_H
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "cmvr/config/arm_config/arm_config.pb.h"
+#include "devices/arm/robot_arm.h"
+
+namespace cmvr::device {
+
+class HuayanRobot final : public RobotArm {
+public:
+    explicit HuayanRobot(const config::RobotArmConfig& cfg);
+    ~HuayanRobot() override;
+
+    std::string typeName() const override { return "HuayanRobot"; }
+    bool init() override;
+    bool stop() override;
+
+    RobotModel getRobotModel() const override { return model_; }
+    std::size_t getDof() const override { return model_.dof; }
+    ArmState getRobotState() const override;
+    JointGroupState getJointState() const override;
+    CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
+    RobotMode getRobotMode() const override;
+    SafetyMode getSafetyMode() const override;
+    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+
+    Result torqueOn() override;
+    Result torqueOff() override;
+    Result calibrateZeroQ(const std::string& joint_name) override;
+    Result emergencyStop() override;
+    Result protectiveStop() override { return emergencyStop(); }
+    Result setSpeedScaling(double scaling) override;
+    double getSpeedScaling() const override { return speed_scaling_; }
+    bool isProtectiveStopped() const override;
+    bool isEmergencyStopped() const override;
+    bool isFault() const override;
+
+    Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
+    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 speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
+    Result stopL(double acceleration) override;
+    Result stopMotion() override;
+
+    Result startServoMode(const ServoOptions& options) override;
+    Result servoJ(const JointPositionCommand& target) override;
+    Result servoL(const CartesianPose& target, FrameType frame = FrameType::Base) override;
+    Result servoSpeedJ(const JointVelocityCommand& velocity) override;
+    Result servoSpeedL(const CartesianVelocity& velocity, FrameType frame = FrameType::Base) override;
+    Result stopServoMode() override;
+
+    Result connect(const std::string& ip, int port) override;
+    Result disconnect() override;
+    bool isConnected() const override;
+    Result powerOn() override { return torqueOn(); }
+    Result powerOff() override { return torqueOff(); }
+    Result brakeRelease() override { return torqueOn(); }
+    Result shutdown() override;
+    Result clearFault() override;
+    Result unlockProtectiveStop() override { return clearFault(); }
+    Result loadProgram(const std::string& program_name) override;
+    Result playProgram() override;
+    Result pauseProgram() override;
+    Result stopProgram() override;
+
+    std::vector<double> ik(const std::string& base_link,
+                           const std::string& ee_link,
+                           const CartesianPose& pose) override;
+    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;
+    CartesianVelocity getSpeedLCommandTwistBase() const override;
+    bool busy() const override { return busy_.load(); }
+
+private:
+    struct HrState {
+        int moving{0};
+        int enabled{0};
+        int error{0};
+        int error_code{0};
+        int error_axis{0};
+        int brake{0};
+        int paused{0};
+        int emergency_stop{0};
+        int safeguard{0};
+        int electrified{0};
+        int connected_to_box{0};
+        int blending_done{0};
+        int in_pos{0};
+        bool valid{false};
+    };
+
+    Result ensureConnected_(const std::string& context) const;
+    Result unsupported_(const std::string& name) const;
+    Result hrResult_(int code, const std::string& context) const;
+    bool validDof_(std::size_t size, std::string& error) const;
+    HrState readHrState_() const;
+    std::vector<double> readJointPositionRad_() const;
+    std::vector<double> readJointVelocityRad_() const;
+    CartesianPose readTcpPose_() const;
+    CartesianVelocity readTcpVelocity_() const;
+    std::vector<double> currentJointPositionDeg_() const;
+    std::string nextCommandId_() const;
+    Result waitMotionDone_(const std::string& context, int timeout_ms) const;
+
+private:
+    config::RobotArmConfig cfg_;
+    config::VendorRobotArmBackendConfig vendor_cfg_;
+    RobotModel model_;
+    std::string ip_;
+    int port_{10003};
+    unsigned int box_id_{0};
+    unsigned int robot_id_{0};
+    std::string tcp_name_{"TCP"};
+    std::string ucs_name_{"Base"};
+    double speed_scaling_{1.0};
+    std::atomic<bool> connected_{false};
+    std::atomic<bool> busy_{false};
+    std::atomic<bool> servo_mode_{false};
+    mutable std::mutex mutex_;
+    mutable std::atomic<unsigned long long> command_seq_{0};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_HUAYAN_ROBOT_H
+
+
+#endif //CMVR_ES_HUAYAN_ARM_H

cmvr-es/devices/arm/robot_arm_factory.h

@@ -7,6 +7,7 @@
 #include "cmvr/config/arm_config/arm_config.pb.h"
 #include "common/base/logging/logger.h"
 #include "devices/arm/aubo_arm/include/aubo_arm.h"
+#include "devices/arm/huayan_arm/huayan_arm.h"
 #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
 
 namespace cmvr::device {
@@ -25,6 +26,9 @@ public:
             if (vendor.brand() == config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM) {
                 return std::make_shared<AuboArm>(cfg);
             }
+            if (vendor.brand() == config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM) {
+                return std::make_shared<HuayanRobot>(cfg);
+            }
             const std::string error =
                 "[RobotArmFactory]: Vendor RobotArm is not implemented yet: " + cfg.id() +
                 ", brand=" + config::VendorRobotArmBrand_Name(vendor.brand());

protos/cmvr/config/arm_config/arm_config.proto

@@ -30,6 +30,7 @@ message MotorRobotArmBackendConfig {
 enum VendorRobotArmBrand {
   VENDOR_ROBOT_ARM_BRAND_UNKNOWN = 0;
   VENDOR_ROBOT_ARM_BRAND_AUBO_ARM = 1;
+  VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM = 2;
 }
 
 message VendorRobotArmBackendConfig {

request.txt

@@ -33,5 +33,6 @@ third_party/modbus/3.1.11
 third_party/visp/3.7.0
 third_party/mainif/0.0.5
 third_party/matplotplusplus/1.2.0
+third_party/huayan_robot/v1.0