new control robot

2026-09-13 10:07:19 +03:00
parent 80c87682ea
commit 229de6d686
13 changed files with 1519 additions and 0 deletions
@@ -0,0 +1,89 @@
+#pragma once
+
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+
+namespace iiwa_controller
+{
+
+constexpr std::size_t COMMAND_GUARD_JOINTS = 7;
+
+struct CommandGuardLimits
+{
+  std::array<double, COMMAND_GUARD_JOINTS> min_position{};
+  std::array<double, COMMAND_GUARD_JOINTS> max_position{};
+  std::array<double, COMMAND_GUARD_JOINTS> max_velocity{};
+};
+
+enum class CommandGuardResult
+{
+  OK,
+  INVALID_PERIOD,
+  NON_FINITE,
+  POSITION_LIMIT,
+  VELOCITY_LIMIT,
+};
+
+inline const char * commandGuardResultName(const CommandGuardResult result)
+{
+  switch (result) {
+    case CommandGuardResult::OK: return "OK";
+    case CommandGuardResult::INVALID_PERIOD: return "INVALID_PERIOD";
+    case CommandGuardResult::NON_FINITE: return "NON_FINITE";
+    case CommandGuardResult::POSITION_LIMIT: return "POSITION_LIMIT";
+    case CommandGuardResult::VELOCITY_LIMIT: return "VELOCITY_LIMIT";
+    default: return "UNKNOWN";
+  }
+}
+
+class CommandGuard
+{
+public:
+  CommandGuard()
+  {
+    limits_.min_position.fill(-std::numeric_limits<double>::infinity());
+    limits_.max_position.fill(std::numeric_limits<double>::infinity());
+    limits_.max_velocity.fill(std::numeric_limits<double>::infinity());
+  }
+
+  explicit CommandGuard(const CommandGuardLimits & limits)
+  : limits_(limits) {}
+
+  CommandGuardResult validate(
+    const std::array<double, COMMAND_GUARD_JOINTS> & command,
+    const std::array<double, COMMAND_GUARD_JOINTS> & previous_command,
+    const double sample_time,
+    const bool previous_command_initialized) const
+  {
+    if (!std::isfinite(sample_time) || sample_time <= 0.0) {
+      return CommandGuardResult::INVALID_PERIOD;
+    }
+
+    for (std::size_t i = 0; i < COMMAND_GUARD_JOINTS; ++i) {
+      if (!std::isfinite(command[i])) {
+        return CommandGuardResult::NON_FINITE;
+      }
+      if (command[i] < limits_.min_position[i] || command[i] > limits_.max_position[i]) {
+        return CommandGuardResult::POSITION_LIMIT;
+      }
+      if (previous_command_initialized) {
+        if (!std::isfinite(previous_command[i]) || !std::isfinite(limits_.max_velocity[i])) {
+          return CommandGuardResult::NON_FINITE;
+        }
+        const double max_delta = limits_.max_velocity[i] * sample_time;
+        if (std::abs(command[i] - previous_command[i]) > max_delta + 1e-9) {
+          return CommandGuardResult::VELOCITY_LIMIT;
+        }
+      }
+    }
+
+    return CommandGuardResult::OK;
+  }
+
+private:
+  CommandGuardLimits limits_{};
+};
+
+}  // namespace iiwa_controller
@@ -0,0 +1,99 @@
+#pragma once
+
+#include <cmath>
+#include <cstddef>
+
+#include "iiwa_controller/FRIClient.h"
+
+namespace iiwa_controller
+{
+
+enum class StateGuardResult
+{
+  OK,
+  INVALID_SNAPSHOT,
+  SESSION_STATE,
+  CONNECTION_QUALITY,
+  SAFETY_STOP,
+  OPERATION_MODE,
+  DRIVE_STATE,
+  CLIENT_COMMAND_MODE,
+  CONTROL_MODE,
+  TRACKING_PERFORMANCE,
+};
+
+inline const char * stateGuardResultName(const StateGuardResult result)
+{
+  switch (result) {
+    case StateGuardResult::OK: return "OK";
+    case StateGuardResult::INVALID_SNAPSHOT: return "INVALID_SNAPSHOT";
+    case StateGuardResult::SESSION_STATE: return "SESSION_STATE";
+    case StateGuardResult::CONNECTION_QUALITY: return "CONNECTION_QUALITY";
+    case StateGuardResult::SAFETY_STOP: return "SAFETY_STOP";
+    case StateGuardResult::OPERATION_MODE: return "OPERATION_MODE";
+    case StateGuardResult::DRIVE_STATE: return "DRIVE_STATE";
+    case StateGuardResult::CLIENT_COMMAND_MODE: return "CLIENT_COMMAND_MODE";
+    case StateGuardResult::CONTROL_MODE: return "CONTROL_MODE";
+    case StateGuardResult::TRACKING_PERFORMANCE: return "TRACKING_PERFORMANCE";
+    default: return "UNKNOWN";
+  }
+}
+
+class StateGuard
+{
+public:
+  StateGuardResult validateSnapshot(const IIWAStateSnapshot & snapshot) const
+  {
+    for (std::size_t i = 0; i < FRIClient::N_JOINTS; ++i) {
+      if (!std::isfinite(snapshot.measured_pos[i]) ||
+          !std::isfinite(snapshot.measured_tau[i]) ||
+          !std::isfinite(snapshot.external_tau[i])) {
+        return StateGuardResult::INVALID_SNAPSHOT;
+      }
+    }
+    if (!std::isfinite(snapshot.sample_time) || snapshot.sample_time <= 0.0 ||
+        snapshot.time_stamp_nano_sec >= 1000000000U ||
+        !std::isfinite(snapshot.tracking_performance)) {
+      return StateGuardResult::INVALID_SNAPSHOT;
+    }
+    return StateGuardResult::OK;
+  }
+
+  StateGuardResult validatePositionState(
+    const IIWAStateSnapshot & snapshot,
+    const KUKA::FRI::ESessionState session_state) const
+  {
+    auto result = validateSnapshot(snapshot);
+    if (result != StateGuardResult::OK) {
+      return result;
+    }
+    if (session_state != KUKA::FRI::COMMANDING_ACTIVE) {
+      return StateGuardResult::SESSION_STATE;
+    }
+    if (snapshot.quality < KUKA::FRI::GOOD) {
+      return StateGuardResult::CONNECTION_QUALITY;
+    }
+    if (snapshot.safety_state != KUKA::FRI::NORMAL_OPERATION) {
+      return StateGuardResult::SAFETY_STOP;
+    }
+    if (snapshot.operation_mode != KUKA::FRI::AUTOMATIC_MODE) {
+      return StateGuardResult::OPERATION_MODE;
+    }
+    if (snapshot.drive_state != KUKA::FRI::ACTIVE) {
+      return StateGuardResult::DRIVE_STATE;
+    }
+    if (snapshot.client_command_mode != KUKA::FRI::POSITION) {
+      return StateGuardResult::CLIENT_COMMAND_MODE;
+    }
+    if (snapshot.control_mode != KUKA::FRI::POSITION_CONTROL_MODE &&
+        snapshot.control_mode != KUKA::FRI::JOINT_IMP_CONTROL_MODE) {
+      return StateGuardResult::CONTROL_MODE;
+    }
+    if (snapshot.tracking_performance < 0.5) {
+      return StateGuardResult::TRACKING_PERFORMANCE;
+    }
+    return StateGuardResult::OK;
+  }
+};
+
+}  // namespace iiwa_controller
@@ -0,0 +1,18 @@
+#pragma once
+
+#include <cmath>
+
+namespace iiwa_controller
+{
+
+// Continuous-time first-order low-pass filter discretization used by
+// lbr_fri_ros2_stack. tau == 0 intentionally disables smoothing.
+inline double exponentialFilterAlpha(const double tau, const double sample_time)
+{
+  if (tau <= 0.0) {
+    return 1.0;
+  }
+  return 1.0 - std::exp(-sample_time / tau);
+}
+
+}  // namespace iiwa_controller
@@ -0,0 +1,120 @@
+#include <array>
+#include <cmath>
+#include <limits>
+
+#include <gtest/gtest.h>
+
+#include "iiwa_controller/CommandGuard.hpp"
+#include "iiwa_controller/FRIClient.h"
+#include "iiwa_controller/StateGuard.hpp"
+#include "iiwa_controller/filters.hpp"
+
+namespace iiwa_controller::test
+{
+
+TEST(ExponentialFilter, UsesContinuousTimeAlpha)
+{
+  EXPECT_NEAR(exponentialFilterAlpha(0.04, 0.01), 1.0 - std::exp(-0.25), 1e-12);
+  EXPECT_DOUBLE_EQ(exponentialFilterAlpha(0.0, 0.01), 1.0);
+}
+
+CommandGuard makeGuard()
+{
+  CommandGuardLimits limits;
+  limits.min_position.fill(-1.0);
+  limits.max_position.fill(1.0);
+  limits.max_velocity.fill(2.0);
+  return CommandGuard(limits);
+}
+
+TEST(CommandGuard, AcceptsFiniteCommandWithinPositionAndVelocityLimits)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 0.01;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::OK);
+}
+
+TEST(CommandGuard, RejectsPositionLimitViolation)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 1.01;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::POSITION_LIMIT);
+}
+
+TEST(CommandGuard, RejectsVelocityLimitViolation)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = 0.1;
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::VELOCITY_LIMIT);
+}
+
+TEST(CommandGuard, RejectsNonFiniteCommand)
+{
+  const auto guard = makeGuard();
+  std::array<double, FRIClient::N_JOINTS> previous{};
+  std::array<double, FRIClient::N_JOINTS> command{};
+  command[0] = std::numeric_limits<double>::quiet_NaN();
+
+  EXPECT_EQ(
+    guard.validate(command, previous, 0.01, true),
+    CommandGuardResult::NON_FINITE);
+}
+
+IIWAStateSnapshot nominalSnapshot()
+{
+  IIWAStateSnapshot snapshot;
+  snapshot.sample_time = 0.01;
+  snapshot.quality = KUKA::FRI::EXCELLENT;
+  snapshot.safety_state = KUKA::FRI::NORMAL_OPERATION;
+  snapshot.operation_mode = KUKA::FRI::AUTOMATIC_MODE;
+  snapshot.drive_state = KUKA::FRI::ACTIVE;
+  snapshot.client_command_mode = KUKA::FRI::POSITION;
+  snapshot.control_mode = KUKA::FRI::POSITION_CONTROL_MODE;
+  snapshot.tracking_performance = 0.9;
+  return snapshot;
+}
+
+TEST(StateGuard, AcceptsNominalPositionState)
+{
+  const StateGuard guard;
+  EXPECT_EQ(
+    guard.validatePositionState(nominalSnapshot(), KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::OK);
+}
+
+TEST(StateGuard, ReportsSafetyStopSeparately)
+{
+  auto snapshot = nominalSnapshot();
+  snapshot.safety_state = KUKA::FRI::SAFETY_STOP_LEVEL_1;
+
+  EXPECT_EQ(
+    StateGuard().validatePositionState(snapshot, KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::SAFETY_STOP);
+}
+
+TEST(StateGuard, ReportsWrongControlModeSeparately)
+{
+  auto snapshot = nominalSnapshot();
+  snapshot.control_mode = KUKA::FRI::NO_CONTROL;
+
+  EXPECT_EQ(
+    StateGuard().validatePositionState(snapshot, KUKA::FRI::COMMANDING_ACTIVE),
+    StateGuardResult::CONTROL_MODE);
+}
+
+}  // namespace iiwa_controller::test
@@ -0,0 +1,34 @@
+from pathlib import Path
+
+
+ROOT = Path(__file__).resolve().parents[1]
+HEADER = (ROOT / "include/iiwa_controller/FRIClient.h").read_text()
+FRI_CLIENT = (ROOT / "src/FRIClient.cpp").read_text()
+HARDWARE = (ROOT / "src/IIWAHardwareInterface.cpp").read_text()
+
+
+def test_fri_data_exchange_does_not_use_a_mutex_in_the_rt_path():
+    assert "#include <mutex>" not in HEADER
+    assert "lock_guard<std::mutex>" not in FRI_CLIENT
+
+
+def test_cleanup_releases_application_before_its_dependencies():
+    cleanup = HARDWARE[HARDWARE.index("on_cleanup"):]
+    assert cleanup.index("app_.reset()") < cleanup.index("connection_.reset()")
+    assert cleanup.index("connection_.reset()") < cleanup.index("fri_client_.reset()")
+
+
+def test_fri_step_failure_stops_the_session():
+    step_pos = HARDWARE.index("app_->step()")
+    after_step = HARDWARE[step_pos:step_pos + 700]
+    assert "fri_running_.store(false" in after_step
+
+
+def test_activation_timeout_is_an_error():
+    timeout_pos = HARDWARE.index("FRI не подключился")
+    after_timeout = HARDWARE[timeout_pos:timeout_pos + 500]
+    assert "CallbackReturn::ERROR" in after_timeout
+
+
+def test_commanding_snapshot_keeps_measured_position_from_fri():
+    assert "snapshot_.measured_pos = filtered_pos_" not in FRI_CLIENT