diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h index 1544b7cd..44c41971 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/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 connected_{false}; std::atomic busy_{false}; + std::atomic active_speed_motion_{ + ActiveSpeedMotion::None}; std::atomic emergency_stopped_{false}; std::atomic hardware_emergency_stopped_{false}; std::atomic hardware_safety_mode_{0}; diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp index 679861c9..092f7125 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/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 linear_velocity{ + velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0}; + std::vector 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 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 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(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(