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 fc2d7a71..97270398 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h @@ -34,7 +34,7 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override { return ControlMode::Position; } + ControlMode getControlMode() const override; bool supportsActionQueueMotion() const noexcept override { return true; } Result listBaseFrame(std::vector& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override; 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 f6645c3d..f19621f3 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -733,6 +733,39 @@ SafetyMode AuboArm::getSafetyMode() const return static_cast(hardware_safety_mode_.load()); } +ControlMode AuboArm::getControlMode() const +{ + if (!connected_.load()) { + return ControlMode::None; + } + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return ControlMode::Position; + } + const auto robot_interface = + sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return ControlMode::Position; + } + + const auto robot_state = robot_interface->getRobotState(); + if (robot_state && + robot_state->getRobotModeType() == + arcs::common_interface::RobotModeType::BackDrive) { + return ControlMode::Freedrive; + } + } catch (const std::exception&) { + // Keep telemetry available when a controller version does not support + // one of the optional hand-guiding status queries. + } +#endif + + return ControlMode::Position; +} + bool AuboArm::isEmergencyStopped() const { return emergency_stopped_.load() || hardware_emergency_stopped_.load(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 77613b38..8c64de69 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -283,6 +283,34 @@ Result HuayanRobot::setSpeedScaling(const double scaling) return Result::success(); } +ControlMode HuayanRobot::getControlMode() const +{ + if (!isConnected()) { + return ControlMode::None; + } + + int tri_stage_enabled = 0; + int tri_stage_mode = 1; + int force_control_state = 0; + int tri_stage_result = -1; + int force_state_result = -1; + { + std::lock_guard lock(mutex_); + tri_stage_result = HRIF_ReadTriStageSwitch( + box_id_, robot_id_, tri_stage_enabled, tri_stage_mode); + force_state_result = HRIF_ReadForceControlState( + box_id_, robot_id_, force_control_state); + } + const bool tri_stage_freedrive = + tri_stage_result == 0 && tri_stage_enabled != 0 && tri_stage_mode == 0; + const bool force_freedrive = + force_state_result == 0 && force_control_state == 3; + if (tri_stage_freedrive || force_freedrive) { + return ControlMode::Freedrive; + } + return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; +} + bool HuayanRobot::isProtectiveStopped() const { const auto state = readHrState_(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 850f22b9..0ebccc17 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -38,7 +38,7 @@ public: 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; } + ControlMode getControlMode() const override; bool supportsActionQueueMotion() const noexcept override { return true; } Result listBaseFrame(std::vector& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override;