implement aubo speed motion commands

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

@@ -92,6 +92,12 @@ public:
     bool busy() const override { return busy_.load(); }
 
 private:
+    enum class ActiveSpeedMotion {
+        None,
+        Joint,
+        Linear,
+    };
+
     Result unsupported_(const std::string& name) const;
     bool validDof_(std::size_t size, std::string& error) const;
     Result ensureConnected_(const std::string& context) const;
@@ -114,6 +120,8 @@ private:
     double speed_scaling_{1.0};
     std::atomic<bool> connected_{false};
     std::atomic<bool> busy_{false};
+    std::atomic<ActiveSpeedMotion> active_speed_motion_{
+        ActiveSpeedMotion::None};
     std::atomic<bool> emergency_stopped_{false};
     std::atomic<bool> hardware_emergency_stopped_{false};
     std::atomic<int> hardware_safety_mode_{0};

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

@@ -692,10 +692,85 @@ 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;
+    std::string error;
+    if (!validDof_(velocity.velocity.size(), error)) {
+        return Result::failure(ArmErrorCode::InvalidDof, error);
+    }
+    const auto ready = ensureMotionReady_("speedJ");
+    if (!ready.ok()) {
+        return ready;
+    }
+    if (busy_.exchange(true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "[AuboArm] arm is busy: " + id_);
+    }
+    active_speed_motion_.store(ActiveSpeedMotion::Joint);
+    const auto clear_owned_motion = [this]() {
+        auto expected = ActiveSpeedMotion::Joint;
+        if (active_speed_motion_.compare_exchange_strong(
+                expected, ActiveSpeedMotion::None)) {
+            busy_.store(false);
+        }
+    };
+
+#if defined(CMVR_HAS_AUBO_SDK)
+    try {
+        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        if (robot_names.empty()) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] robot name list is empty");
+        }
+        const auto robot_interface =
+            sdk_->rpc_client->getRobotInterface(robot_names.front());
+        if (!robot_interface || !robot_interface->getMotionControl()) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] motion control interface is unavailable");
+        }
+
+        const auto motion_control = robot_interface->getMotionControl();
+        motion_control->setSpeedFraction(speed_scaling_);
+        const double resolved_acceleration =
+            acceleration > 0.0 ? acceleration : 1.5;
+        const double resolved_duration = duration > 0.0 ? duration : 0.1;
+        const int ret = motion_control->speedJoint(
+            velocity.velocity, resolved_acceleration, resolved_duration);
+
+        if (active_speed_motion_.load() != ActiveSpeedMotion::Joint) {
+            // StopAll/stopMotion may race with the blocking SDK call. Stop a
+            // second time after it returns so a late submission cannot leave
+            // the robot moving.
+            (void)motion_control->stopJoint(resolved_acceleration);
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedJ canceled by stopMotion or hardware safety event");
+        }
+        if (ret != arcs::common_interface::AUBO_OK) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] speedJ failed: sdk ret=" +
+                    std::to_string(ret));
+        }
+
+        // Aubo speedJoint keeps following the accepted velocity after this
+        // call returns. Keep busy_ and the motion kind latched until an
+        // explicit stopJ/stopMotion request clears them.
+        return Result::success();
+    } catch (const std::exception& e) {
+        clear_owned_motion();
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            std::string{"[AuboArm] speedJ failed: "} + e.what());
+    }
+#else
+    clear_owned_motion();
     return unsupported_("speedJ");
+#endif
 }
 
 Result AuboArm::stopJ(double acceleration)
@@ -842,11 +917,125 @@ Result AuboArm::moveL(const CartesianPose& target,
 
 Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame)
 {
-    (void)velocity;
-    (void)acceleration;
-    (void)duration;
-    (void)frame;
+    if (frame == FrameType::User) {
+        return Result::failure(
+            ArmErrorCode::UnsupportedCommand,
+            "[AuboArm] speedL User frame requires a configured user coordinate frame");
+    }
+    const auto ready = ensureMotionReady_("speedL");
+    if (!ready.ok()) {
+        return ready;
+    }
+    if (busy_.exchange(true)) {
+        return Result::failure(
+            ArmErrorCode::RobotNotReady,
+            "[AuboArm] arm is busy: " + id_);
+    }
+    active_speed_motion_.store(ActiveSpeedMotion::Linear);
+    const auto clear_owned_motion = [this]() {
+        auto expected = ActiveSpeedMotion::Linear;
+        if (active_speed_motion_.compare_exchange_strong(
+                expected, ActiveSpeedMotion::None)) {
+            busy_.store(false);
+        }
+    };
+
+#if defined(CMVR_HAS_AUBO_SDK)
+    try {
+        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        if (robot_names.empty()) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] robot name list is empty");
+        }
+        const auto robot_interface =
+            sdk_->rpc_client->getRobotInterface(robot_names.front());
+        if (!robot_interface || !robot_interface->getMotionControl()) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::RobotNotReady,
+                "[AuboArm] motion control interface is unavailable");
+        }
+
+        std::vector<double> linear_velocity{
+            velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0};
+        std::vector<double> angular_velocity{
+            velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0};
+        if (frame == FrameType::Tool) {
+            if (!robot_interface->getRobotState()) {
+                clear_owned_motion();
+                return Result::failure(
+                    ArmErrorCode::RobotNotReady,
+                    "[AuboArm] robot state interface is unavailable");
+            }
+            auto tool_frame =
+                robot_interface->getRobotState()->getTcpPose();
+            if (tool_frame.size() < 6) {
+                clear_owned_motion();
+                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;
+            const auto math = sdk_->rpc_client->getMath();
+            if (!math) {
+                clear_owned_motion();
+                return Result::failure(
+                    ArmErrorCode::RobotNotReady,
+                    "[AuboArm] math interface is unavailable");
+            }
+            linear_velocity = math->poseTrans(tool_frame, linear_velocity);
+            angular_velocity = math->poseTrans(tool_frame, angular_velocity);
+            if (linear_velocity.size() < 3 ||
+                angular_velocity.size() < 3) {
+                clear_owned_motion();
+                return Result::failure(
+                    ArmErrorCode::CommandFailed,
+                    "[AuboArm] speedL failed: frame conversion returned an invalid velocity");
+            }
+        }
+
+        const std::vector<double> speed{
+            linear_velocity[0], linear_velocity[1], linear_velocity[2],
+            angular_velocity[0], angular_velocity[1], angular_velocity[2],
+        };
+        const auto motion_control = robot_interface->getMotionControl();
+        motion_control->setSpeedFraction(speed_scaling_);
+        const double resolved_acceleration =
+            acceleration > 0.0 ? acceleration : 1.2;
+        const double resolved_duration = duration > 0.0 ? duration : 0.5;
+        const int ret = motion_control->speedLine(
+            speed, resolved_acceleration, resolved_duration);
+
+        if (active_speed_motion_.load() != ActiveSpeedMotion::Linear) {
+            (void)motion_control->stopLine(
+                resolved_acceleration, resolved_acceleration);
+            return Result::failure(
+                ArmErrorCode::CommandRejected,
+                "[AuboArm] speedL canceled by stopMotion or hardware safety event");
+        }
+        if (ret != arcs::common_interface::AUBO_OK) {
+            clear_owned_motion();
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] speedL failed: sdk ret=" +
+                    std::to_string(ret));
+        }
+
+        return Result::success();
+    } catch (const std::exception& e) {
+        clear_owned_motion();
+        return Result::failure(
+            ArmErrorCode::CommandFailed,
+            std::string{"[AuboArm] speedL failed: "} + e.what());
+    }
+#else
+    clear_owned_motion();
     return unsupported_("speedL");
+#endif
 }
 
 Result AuboArm::stopL(std::optional<double> acceleration)
@@ -862,23 +1051,47 @@ Result AuboArm::stopMotion()
     }
 #if defined(CMVR_HAS_AUBO_SDK)
     if (!connected_.load() || !sdk_ || !sdk_->rpc_client) {
+        active_speed_motion_.store(ActiveSpeedMotion::None);
+        busy_.store(false);
         return Result::success();
     }
+    const auto active_speed =
+        active_speed_motion_.exchange(ActiveSpeedMotion::None);
+    busy_.store(false);
     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);
+        if (!robot_interface || !robot_interface->getMotionControl()) {
+            return Result::success();
+        }
+        const auto motion_control = robot_interface->getMotionControl();
+        int ret = arcs::common_interface::AUBO_OK;
+        switch (active_speed) {
+        case ActiveSpeedMotion::Joint:
+            ret = motion_control->stopJoint(1.5);
+            break;
+        case ActiveSpeedMotion::Linear:
+            ret = motion_control->stopLine(1.2, 1.2);
+            break;
+        case ActiveSpeedMotion::None:
+            ret = motion_control->stopMove(true, true);
+            break;
+        }
+        if (ret != arcs::common_interface::AUBO_OK) {
+            return Result::failure(
+                ArmErrorCode::CommandFailed,
+                "[AuboArm] stopMotion failed: sdk ret=" +
+                    std::to_string(ret));
         }
-        busy_.store(false);
         return Result::success();
     } catch (const std::exception& e) {
         return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what());
     }
 #else
+    active_speed_motion_.store(ActiveSpeedMotion::None);
     busy_.store(false);
     return Result::success();
 #endif
@@ -939,6 +1152,8 @@ Result AuboArm::connect(const std::string& ip, const int port)
         sdk_->rpc_client->login(username_, password_);
         ip_ = ip;
         port_ = port > 0 ? port : 30004;
+        active_speed_motion_.store(ActiveSpeedMotion::None);
+        busy_.store(false);
         connected_.store(true);
         hardware_safety_mode_.store(static_cast<int>(SafetyMode::Unknown));
         hardware_emergency_stopped_.store(false);
@@ -985,6 +1200,7 @@ Result AuboArm::disconnect()
     sdk_.reset();
 #endif
     connected_.store(false);
+    active_speed_motion_.store(ActiveSpeedMotion::None);
     busy_.store(false);
     hardware_emergency_stopped_.store(false);
     hardware_estop_latched_.store(false);
@@ -1060,6 +1276,12 @@ void AuboArm::autoEnableMonitorLoop_()
                                     : "RobotEmergencyStop")
                             << ", source=" << emergency_source;
                     }
+                    // The controller has already stopped the physical motion.
+                    // Clear the retained direct-speed command as well so a
+                    // blocking speedJoint/speedLine call observes cancellation
+                    // when it returns, and recovery does not leave the arm
+                    // permanently busy.
+                    active_speed_motion_.store(ActiveSpeedMotion::None);
                     busy_.store(false);
                 } else if (hardware_estop_latched_.load() &&
                            isHardwareEmergencyStopReleased(