增加aubo_arm更多接口实现

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

@@ -4,6 +4,7 @@
 #include <chrono>
 #include <exception>
 #include <thread>
+#include <tuple>
 
 #include "common/base/logging/logger.h"
 
@@ -41,6 +42,30 @@ std::string vendorBrandName(const config::VendorRobotArmBrand brand)
 using arcs::common_interface::RobotModeType;
 using arcs::aubo_sdk::RobotInterfacePtr;
 
+constexpr int kAuboServoMode = 3;
+
+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)
 {
@@ -72,6 +97,31 @@ int waitArrival(const RobotInterfacePtr& robot_interface)
     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 {
@@ -360,16 +410,65 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o
 
 Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration)
 {
-    (void)velocity;
-    (void)acceleration;
-    (void)duration;
-    return unsupported_("speedJ");
+    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)
 {
-    (void)acceleration;
-    return stopMotion();
+    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)
@@ -414,17 +513,91 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options,
 
 Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
 {
-    (void)velocity;
-    (void)acceleration;
-    (void)duration;
-    (void)frame;
-    return unsupported_("speedL");
+    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(double acceleration)
 {
-    (void)acceleration;
-    return stopMotion();
+    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 > 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()
@@ -450,39 +623,166 @@ Result AuboArm::stopMotion()
 
 Result AuboArm::startServoMode(const ServoOptions& options)
 {
-    (void)options;
-    return unsupported_("startServoMode");
+    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)
 {
-    (void)target;
-    return unsupported_("servoJ");
+    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)
 {
-    (void)target;
-    (void)frame;
-    return unsupported_("servoL");
+    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)
 {
-    (void)velocity;
-    return unsupported_("servoSpeedJ");
+    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)
 {
-    (void)velocity;
-    (void)frame;
-    return unsupported_("servoSpeedL");
+    const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008;
+    return speedL(velocity, 1.2, period, frame);
 }
 
 Result AuboArm::stopServoMode()
 {
-    return Result::success();
+    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)
@@ -569,6 +869,7 @@ Result AuboArm::disconnect()
     sdk_.reset();
     connected_.store(false);
     busy_.store(false);
+    servo_mode_.store(false);
     return Result::success();
 }
 
@@ -580,23 +881,82 @@ Result AuboArm::shutdown()
 
 Result AuboArm::loadProgram(const std::string& program_name)
 {
-    (void)program_name;
-    return unsupported_("loadProgram");
+    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()
 {
-    return unsupported_("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()
 {
-    return unsupported_("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()
 {
-    return unsupported_("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,
@@ -605,8 +965,29 @@ std::vector<double> AuboArm::ik(const std::string& base_link,
 {
     (void)base_link;
     (void)ee_link;
-    (void)pose;
-    CMVR_LOG(ERROR) << "[AuboArm] ik is not implemented";
+    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 {};
 }
 
@@ -619,7 +1000,34 @@ CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_li
 
 CartesianPose AuboArm::fk(bool is_tcp)
 {
-    return getTcpPose(is_tcp ? FrameType::Base : FrameType::Tool);
+    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

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

@@ -28,7 +28,7 @@ public:
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override { return SafetyMode::Normal; }
-    ControlMode getControlMode() const override { return ControlMode::Position; }
+    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
 
     Result torqueOn() override;
     Result torqueOff() override;
@@ -97,8 +97,10 @@ private:
     std::string username_;
     std::string password_;
     double speed_scaling_{1.0};
+    ServoOptions servo_options_;
     std::atomic<bool> connected_{false};
     std::atomic<bool> busy_{false};
+    std::atomic<bool> servo_mode_{false};
     bool emergency_stopped_{false};
     mutable std::mutex mutex_;