roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/ur/native/src/control_loop.cpp

@@ -0,0 +1,512 @@
+#include "ur_cpp/control_loop.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include <exception>
+#include <memory>
+#include <pthread.h>
+#include <sched.h>
+#include <thread>
+#include <unistd.h>
+
+#include "ur_cpp/cartesian_speedl.hpp"
+#include "ur_cpp/geom.hpp"
+#include "ur_cpp/robot_io.hpp"
+
+using steady = std::chrono::steady_clock;
+
+static uint64_t nowNs() {
+  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::nanoseconds>(
+                                   std::chrono::steady_clock::now().time_since_epoch())
+                                   .count());
+}
+
+enum Mode : uint8_t {
+  MODE_IDLE = 0,
+  MODE_TRACK_POSE = 1,
+  MODE_SWITCHING = 4,
+  MODE_TRACK_POSITION = 5,
+};
+
+static const char *modeName(uint8_t m) {
+  switch (m) {
+  case MODE_IDLE:
+    return "IDLE";
+  case MODE_TRACK_POSE:
+    return "TRACK_POSE";
+  case MODE_TRACK_POSITION:
+    return "TRACK_POSITION";
+  case MODE_SWITCHING:
+    return "SWITCHING";
+  default:
+    return "?";
+  }
+}
+
+struct RollingDt {
+  std::array<double, 512> buf_{};
+  size_t n_ = 0, head_ = 0;
+  double max_ms_ = 0;
+  void push(double ms) {
+    buf_[head_] = ms;
+    head_ = (head_ + 1) % buf_.size();
+    n_ = std::min(n_ + 1, buf_.size());
+    max_ms_ = std::max(max_ms_, ms);
+  }
+  void fill(Snapshot &s) const {
+    if (n_ < 2)
+      return;
+    double mean = 0;
+    for (size_t i = 0; i < n_; ++i)
+      mean += buf_[i];
+    mean /= static_cast<double>(n_);
+    double var = 0;
+    for (size_t i = 0; i < n_; ++i)
+      var += (buf_[i] - mean) * (buf_[i] - mean);
+    std::array<double, 512> tmp = buf_;
+    std::sort(tmp.begin(), tmp.begin() + static_cast<long>(n_));
+    s.dt_mean_ms_ = mean;
+    s.dt_std_ms_ = std::sqrt(var / static_cast<double>(n_));
+    s.dt_p99_ms_ = tmp[static_cast<size_t>(0.99 * (n_ - 1))];
+    s.dt_max_ms_ = max_ms_;
+    if (mean > 0)
+      s.host_hz_ = 1000.0 / mean;
+  }
+  void fillRtdeHz(Snapshot &s) const {
+    if (n_ < 2)
+      return;
+    double mean = 0;
+    for (size_t i = 0; i < n_; ++i)
+      mean += buf_[i];
+    mean /= static_cast<double>(n_);
+    if (mean > 0)
+      s.rtde_hz_ = 1000.0 / mean;
+  }
+};
+
+bool validateArgs(const Args &a, std::string &error) {
+  return CartesianSpeedlLaw(a).validate(error);
+}
+
+static void tryRealtimePriority(int prio) {
+  if (prio <= 0)
+    return;
+  sched_param sp{};
+  sp.sched_priority = prio;
+  if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp) == 0)
+    std::printf("[rt] SCHED_FIFO priority %d acquired\n", prio);
+  else
+    std::printf("[rt] SCHED_FIFO unavailable (%s) — default scheduler\n", std::strerror(errno));
+}
+
+void ControlLoop::watchdogMain() {
+  constexpr double stall_s = 2.0;
+  constexpr uint64_t limit_ns = static_cast<uint64_t>(stall_s * 1e9);
+  while (!watchdog_quit_) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    if (watchdog_quit_)
+      return;
+    const uint64_t last = last_tick_ns_.load();
+    if (last != 0 && nowNs() - last > limit_ns) {
+      std::fprintf(stderr, "[halt] loop stalled > %.1f s — hard-exit\n", stall_s);
+      std::fflush(nullptr);
+      ::_exit(3);
+    }
+  }
+}
+
+// One RTDE period: intent → guards → write → snapshot.
+struct LoopCycle {
+  ControlLoop &rt_;
+  RobotIO &io_;
+  const Args &args_;
+
+  vec6 tcp_pose_{}, q_{}, wrench_{};
+  Mode mode_ = MODE_IDLE;
+  uint8_t logged_mode_ = 0xFF;
+  vec6 ref_pose_{};
+  CartesianSpeedlLaw control_;
+  double switch_started_ = 0, settled_since_ = 0;
+  RollingDt dts_;
+  RollingDt robot_dts_;
+  LoopFault fault_ = LoopFault::None;
+  steady::time_point last_ = steady::now();
+  uint64_t ticks_ = 0;
+  bool ok_ = true;
+  double last_robot_time_ = 0;
+
+  LoopCycle(ControlLoop &runtime, RobotIO &robot)
+      : rt_(runtime), io_(robot), args_(runtime.args_), control_(runtime.args_) {}
+
+  bool latched() const { return fault_ != LoopFault::None; }
+
+  void trip(LoopFault fault) {
+    if (!latched())
+      fault_ = fault;
+  }
+
+  bool failReady(const std::string &msg) {
+    std::lock_guard<std::mutex> lock(rt_.snap_mu_);
+    rt_.snap_.loop_ok_ = false;
+    rt_.snap_.last_error_ = msg;
+    return false;
+  }
+
+  // First robot sample: seed refs/clocks so tick 1 holds here, not at origin.
+  bool initializeFromRobot() {
+    if (!io_.tick(tcp_pose_, q_, wrench_))
+      return failReady("no robot data");
+    ref_pose_ = tcp_pose_;
+    control_.reset(tcp_pose_, vec6{});
+    last_robot_time_ = io_.robot_time_;
+    last_ = steady::now();
+    std::printf("[loop] robot at %.0f Hz — holding [%.4f %.4f %.4f]\n",
+                args_.freq_, tcp_pose_[0], tcp_pose_[1], tcp_pose_[2]);
+    std::fflush(stdout);
+    return true;
+  }
+
+  bool prepareMode(Mode requested) {
+    if (mode_ == requested)
+      return true;
+    if (mode_ != MODE_SWITCHING) {
+      dropToIdle();
+      mode_ = MODE_SWITCHING;
+      switch_started_ = io_.robot_time_;
+      settled_since_ = 0;
+      // Send keepalive first: URScript joins/brakes the old motion thread.
+      return false;
+    }
+    if (io_.robot_time_ - switch_started_ > 2.0) {
+      trip(LoopFault::ModeSwitchTimeout);
+      return false;
+    }
+    const auto still = [](double speed) { return std::abs(speed) <= 0.005; };
+    if (!std::all_of(io_.qd_.begin(), io_.qd_.end(), still) ||
+        !std::all_of(io_.tcp_speed_.begin(), io_.tcp_speed_.end(), still)) {
+      settled_since_ = 0;
+      return false;
+    }
+    if (settled_since_ == 0)
+      settled_since_ = io_.robot_time_;
+    if (io_.robot_time_ - settled_since_ < 0.05)
+      return false;
+    // Seed from post-stop feedback, never from the old source's snapshot.
+    control_.reset(tcp_pose_, io_.tcp_speed_);
+    mode_ = requested;
+    return true;
+  }
+
+  void applyMode(const ControlLoop::Intent &intent) {
+    using Kind = ControlLoop::Kind;
+    switch (intent.kind_) {
+    case Kind::TrackPose:
+      if (prepareMode(MODE_TRACK_POSE))
+        ref_pose_ = intent.pose_;
+      break;
+    case Kind::TrackPosition:
+      if (prepareMode(MODE_TRACK_POSITION))
+        ref_pose_ = intent.pose_;
+      break;
+    case Kind::Idle:
+      dropToIdle();
+      break;
+    }
+  }
+
+  void applyIntent() {
+    using Kind = ControlLoop::Kind;
+    const auto intent = rt_.takeIntent();
+    if ((intent.kind_ == Kind::TrackPose || intent.kind_ == Kind::TrackPosition) && !finite6(intent.pose_))
+      trip(LoopFault::VirtualStateInvalid);
+
+    if (intent.zero_ft_) {
+      if (io_.zeroFT())
+        control_.resetAdmittance();
+      else
+        std::fprintf(stderr, "[warning] zero_ftsensor refused; continuing without zeroing\n");
+    }
+    if (intent.stop_motion_)
+      dropToIdle();
+    else if (!latched())
+      applyMode(intent);
+  }
+
+  void dropToIdle() { mode_ = MODE_IDLE; }
+
+  void applyGuards() {
+    if (!finite6(tcp_pose_) || !finite6(q_) || !finite6(wrench_) || !finite6(io_.tcp_speed_) ||
+        !finite6(io_.qd_))
+      trip(LoopFault::VirtualStateInvalid);
+    if (latched())
+      dropToIdle();
+  }
+
+  void logMode() {
+    if (mode_ == logged_mode_)
+      return;
+    std::printf("[loop] mode %s -> %s\n", logged_mode_ == 0xFF ? "INIT" : modeName(logged_mode_),
+                modeName(mode_));
+    std::fflush(stdout);
+    logged_mode_ = mode_;
+  }
+
+  bool writeCommand(double dt) {
+    bool written = false;
+    switch (mode_) {
+    case MODE_TRACK_POSE:
+    case MODE_TRACK_POSITION: {
+      CartesianCommand command;
+      if (!control_.step({ref_pose_, tcp_pose_, wrench_, dt}, command, mode_ == MODE_TRACK_POSE)) {
+        trip(command.fault_);
+        return failCycle();
+      }
+      written = io_.writeTwist(command.twist_);
+      break;
+    }
+    case MODE_IDLE:
+    case MODE_SWITCHING:
+      written = io_.writeKeepalive();
+      break;
+    }
+    if (written)
+      return true;
+    trip(LoopFault::RobotIoFailed);
+    std::fprintf(stderr, "[halt] robot write failed (program stopped / protective stop?)\n");
+    return failCycle();
+  }
+
+  void publishSnap() {
+    Snapshot s;
+    s.q_ = q_;
+    s.qd_ = io_.qd_;
+    s.tcp_ = tcp_pose_;
+    s.tcp_speed_ = io_.tcp_speed_;
+    s.wrench_ = wrench_;
+    s.mode_ = mode_;
+    s.fault_ = fault_;
+    s.safety_mode_ = io_.safety_mode_;
+    s.ticks_ = ticks_;
+    dts_.fill(s);
+    robot_dts_.fillRtdeHz(s);
+    s.program_running_ = io_.program_running_.load();
+    s.loop_alive_ = true;
+    s.loop_ok_ = ok_;
+    std::lock_guard<std::mutex> lock(rt_.snap_mu_);
+    rt_.snap_ = s;
+  }
+
+  bool failCycle() {
+    ok_ = false;
+    return false;
+  }
+
+  bool readRobot() {
+    if (io_.tick(tcp_pose_, q_, wrench_)) {
+      rt_.last_tick_ns_ = nowNs();
+      return true;
+    }
+    const auto read_fault = (io_.safety_mode_ != 1 && io_.safety_mode_ != 2)
+                                ? LoopFault::RobotSafetyStop
+                                : LoopFault::RobotIoFailed;
+    trip(read_fault);
+    std::fprintf(stderr, "[halt] %s safety_mode=%d\n",
+                 read_fault == LoopFault::RobotSafetyStop ? "robot protective/safety stop"
+                                                          : "lost robot stream",
+                 io_.safety_mode_);
+    return failCycle();
+  }
+
+  // Pace off the robot clock. A host hitch can make wall_dt jump while robot_dt
+  // stays 2 ms — that is not a dropped sample.
+  bool pace(double &dt) {
+    const auto now = steady::now();
+    const double wall_dt = std::chrono::duration<double>(now - last_).count();
+    last_ = now;
+    dt = 1.0 / args_.freq_;
+    const double robot_dt = io_.robot_time_ - last_robot_time_;
+    last_robot_time_ = io_.robot_time_;
+    if (ticks_ > 0) {
+      dts_.push(wall_dt * 1e3);
+      robot_dts_.push(robot_dt * 1e3);
+    }
+    ++ticks_;
+    if (!std::isfinite(robot_dt) || (ticks_ > 1 && std::abs(robot_dt - dt) > dt * .1)) {
+      trip(LoopFault::SampleDiscontinuity);
+      std::fprintf(stderr, "[halt] discontinuous robot samples dt=%.6f wall=%.6f\n", robot_dt,
+                   wall_dt);
+      return failCycle();
+    }
+    if (wall_dt > .010)
+      std::fprintf(stderr, "[warn] host tick hitch wall=%.6f s (robot dt=%.6f)\n", wall_dt,
+                   robot_dt);
+    return true;
+  }
+
+  bool step() {
+    if (!readRobot())
+      return false;
+    double dt = 0;
+    if (!pace(dt))
+      return false;
+    applyIntent();
+    if (rt_.stop_)
+      return false;
+    applyGuards();
+    logMode();
+    if (!writeCommand(dt))
+      return false;
+    publishSnap();
+    return true;
+  }
+
+  void finish() {
+    std::lock_guard<std::mutex> lock(rt_.snap_mu_);
+    rt_.snap_.loop_alive_ = false;
+    rt_.snap_.loop_ok_ = ok_;
+    rt_.snap_.fault_ = fault_;
+    if (!ok_ && rt_.snap_.last_error_.empty())
+      rt_.snap_.last_error_ = fault_ != LoopFault::None ? loopFaultName(fault_) : "loop_exit";
+  }
+};
+
+ControlLoop::ControlLoop(Args args) : args_(std::move(args)) {}
+
+ControlLoop::~ControlLoop() {
+  requestStop();
+  join();
+}
+
+bool ControlLoop::start() {
+  if (thread_.joinable())
+    return true;
+  stop_ = false;
+  watchdog_quit_ = false;
+  thread_ = std::thread([this] { threadMain(); });
+  return true;
+}
+
+void ControlLoop::requestStop() { stop_ = true; }
+
+void ControlLoop::join() {
+  if (thread_.joinable())
+    thread_.join();
+}
+
+void ControlLoop::setPoseRef(const vec6 &tcp, bool admittance) {
+  std::lock_guard<std::mutex> lock(intent_mu_);
+  intent_.kind_ = admittance ? Kind::TrackPose : Kind::TrackPosition;
+  intent_.pose_ = tcp;
+}
+
+void ControlLoop::zeroFt() {
+  std::lock_guard<std::mutex> lock(intent_mu_);
+  intent_.zero_ft_ = true;
+}
+
+void ControlLoop::stopMotion() {
+  std::lock_guard<std::mutex> lock(intent_mu_);
+  intent_.kind_ = Kind::Idle;
+  intent_.stop_motion_ = true;
+}
+
+ControlLoop::Intent ControlLoop::takeIntent() {
+  std::lock_guard<std::mutex> lock(intent_mu_);
+  Intent out = intent_;
+  // A command racing with stop must not erase it or resume on the next tick.
+  if (out.stop_motion_)
+    intent_.kind_ = Kind::Idle;
+  intent_.zero_ft_ = false;
+  intent_.stop_motion_ = false;
+  return out;
+}
+
+Snapshot ControlLoop::snapshot() const {
+  std::lock_guard<std::mutex> lock(snap_mu_);
+  return snap_;
+}
+
+bool ControlLoop::waitReady(double timeout_s) {
+  const auto deadline = steady::now() + std::chrono::duration<double>(timeout_s);
+  while (steady::now() < deadline) {
+    const auto s = snapshot();
+    if (s.loop_alive_)
+      return true;
+    if (!s.loop_ok_ && !s.last_error_.empty())
+      return false;
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+  return false;
+}
+
+void ControlLoop::threadMain() {
+  std::setvbuf(stdout, nullptr, _IOLBF, 0);
+  // Keep these outside the try block: unwinding a joinable watchdog terminates
+  // the process, and the robot session must survive until stopControl is tried.
+  std::unique_ptr<RobotIO> io;
+  std::thread watchdog;
+  const auto reportException = [this](const char *phase, const char *detail) {
+    std::fprintf(stderr, "[halt] %s: %s\n", phase, detail);
+    std::lock_guard<std::mutex> lock(snap_mu_);
+    snap_.loop_alive_ = false;
+    snap_.loop_ok_ = false;
+    if (snap_.fault_ == LoopFault::None)
+      snap_.fault_ = LoopFault::RobotIoFailed;
+    if (snap_.last_error_.empty())
+      snap_.last_error_ = std::string(phase) + ": " + detail;
+  };
+  try {
+    const auto run = [&] {
+      std::string err;
+      io = openRobotIo(args_, stop_, &err);
+      if (!io) {
+        std::lock_guard<std::mutex> lock(snap_mu_);
+        snap_.loop_ok_ = false;
+        snap_.last_error_ = err;
+        return;
+      }
+
+      tryRealtimePriority(args_.rt_prio_);
+      LoopCycle cycle(*this, *io);
+      if (!cycle.initializeFromRobot())
+        return;
+
+      last_tick_ns_ = nowNs();
+      watchdog = std::thread(&ControlLoop::watchdogMain, this);
+      while (!stop_ && cycle.step()) {
+      }
+      watchdog_quit_ = true;
+      watchdog.join();
+      cycle.finish();
+
+      Snapshot fin{};
+      cycle.dts_.fill(fin);
+      cycle.robot_dts_.fillRtdeHz(fin);
+      std::printf("[exit] %llu ticks, rtde %.1f Hz host %.1f Hz, dt mean %.3f ms std %.3f ms "
+                  "p99 %.2f ms max %.2f ms\n",
+                  static_cast<unsigned long long>(cycle.ticks_), fin.rtde_hz_, fin.host_hz_,
+                  fin.dt_mean_ms_, fin.dt_std_ms_, fin.dt_p99_ms_, fin.dt_max_ms_);
+    };
+    run();
+  } catch (const std::exception &exc) {
+    reportException("control_loop_exception", exc.what());
+  } catch (...) {
+    reportException("control_loop_exception", "unknown exception");
+  }
+
+  watchdog_quit_ = true;
+  if (watchdog.joinable())
+    watchdog.join();
+  if (io) {
+    try {
+      io->stopControl();
+    } catch (const std::exception &exc) {
+      reportException("stop_control_exception", exc.what());
+    } catch (...) {
+      reportException("stop_control_exception", "unknown exception");
+    }
+  }
+}

roboapi/hardware/ur/native/src/robot_io_urcl.cpp

@@ -0,0 +1,143 @@
+#include "ur_cpp/robot_io.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cstdio>
+#include <thread>
+#include <vector>
+
+#include <ur_client_library/log.h>
+#include <ur_client_library/primary/robot_message/error_code_message.h>
+#include <ur_client_library/rtde/rtde_client.h>
+#include <ur_client_library/types.h>
+#include <ur_client_library/ur/datatypes.h>
+#include <ur_client_library/ur/ur_driver.h>
+
+namespace {
+
+urcl::vector6d_t toUrcl(const vec6 &in) {
+  urcl::vector6d_t v{};
+  std::copy(in.begin(), in.end(), v.begin());
+  return v;
+}
+
+void fromUrcl(const urcl::vector6d_t &in, vec6 &out) {
+  std::copy(in.begin(), in.end(), out.begin());
+}
+
+} // namespace
+
+struct UrclIO : RobotIO {
+  std::atomic<bool> *stop_ = nullptr;
+  std::unique_ptr<urcl::UrDriver> driver_;
+  std::unique_ptr<urcl::rtde_interface::DataPackage> pkg_;
+  std::chrono::steady_clock::time_point last_error_log_{};
+
+  explicit UrclIO(std::atomic<bool> &s) : stop_(&s) {}
+
+  bool init(const Args &args) {
+    urcl::setLogLevel(urcl::LogLevel::INFO);
+    const std::vector<std::string> out_recipe = {
+        "timestamp",   "actual_q",  "actual_TCP_pose", "actual_TCP_force",
+        "safety_mode", "actual_qd", "actual_TCP_speed"};
+    // No RTDE inputs are written. Leave their ownership available to other clients.
+    const std::vector<std::string> in_recipe{};
+    urcl::UrDriverConfiguration cfg;
+    cfg.robot_ip = args.robot_ip_;
+    cfg.script_file = args.script_file_;
+    cfg.output_recipe = out_recipe;
+    cfg.input_recipe = in_recipe;
+    cfg.headless_mode = true;
+    cfg.handle_program_state = [this](bool running) {
+      program_running_ = running;
+      std::printf("[robot] external-control program %s\n", running ? "RUNNING" : "STOPPED");
+      std::fflush(stdout);
+    };
+    driver_ = std::make_unique<urcl::UrDriver>(cfg);
+    driver_->resetRTDEClient(out_recipe, in_recipe, args.freq_);
+    for (int i = 0; i < 100 && !program_running_ && !(stop_ && stop_->load()); ++i) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      if (i == 50)
+        driver_->sendRobotProgram();
+    }
+    if (!program_running_) {
+      std::fprintf(
+          stderr,
+          "external-control program did not start (remote control? another RTDE client?)\n");
+      return false;
+    }
+    driver_->startRTDECommunication(false);
+    // URCL reallocates a DataPackage on every blocking read if this pointer is
+    // empty. Keep one recipe-sized buffer for the life of the session.
+    pkg_ = std::make_unique<urcl::rtde_interface::DataPackage>(out_recipe);
+    return true;
+  }
+
+  void drainPrimaryErrors() {
+    // Drain the primary error queue even when logging is rate-limited;
+    // otherwise repeated protective-stop messages accumulate without bound.
+    const auto codes = driver_->getErrorCodes();
+    if (codes.empty())
+      return;
+    using clock = std::chrono::steady_clock;
+    const auto now = clock::now();
+    if (now - last_error_log_ < std::chrono::seconds(1))
+      return;
+    last_error_log_ = now;
+    const auto &c = codes.back();
+    if (c.report_level == urcl::ReportLevel::DEBUG ||
+        c.report_level == urcl::ReportLevel::DEVL_DEBUG)
+      return;
+    std::fprintf(stderr, "[robot] %s code=%d %s (%zu queued)\n",
+                 c.to_string.empty() ? c.text.c_str() : c.to_string.c_str(),
+                 static_cast<int>(c.message_code), c.text.c_str(), codes.size());
+  }
+
+  bool tick(vec6 &tcp_pose, vec6 &q, vec6 &wrench) override {
+    drainPrimaryErrors();
+    if (!driver_->getDataPackageBlocking(pkg_))
+      return false;
+    urcl::vector6d_t v{};
+    if (!pkg_->getData("actual_TCP_pose", v))
+      return false;
+    fromUrcl(v, tcp_pose);
+    if (!pkg_->getData("actual_q", v))
+      return false;
+    fromUrcl(v, q);
+    if (!pkg_->getData("actual_TCP_force", v))
+      return false;
+    fromUrcl(v, wrench);
+    if (!pkg_->getData("timestamp", robot_time_) || !pkg_->getData("safety_mode", safety_mode_))
+      return false;
+    if (!pkg_->getData("actual_TCP_speed", v))
+      return false;
+    fromUrcl(v, tcp_speed_);
+    if (!pkg_->getData("actual_qd", v))
+      return false;
+    fromUrcl(v, qd_);
+    if (safety_mode_ != 1 && safety_mode_ != 2)
+      return false;
+    return true;
+  }
+
+  bool writeTwist(const vec6 &tw) override {
+    return driver_->writeJointCommand(toUrcl(tw), urcl::comm::ControlMode::MODE_SPEEDL,
+                                      urcl::RobotReceiveTimeout::millisec(100));
+  }
+  bool writeKeepalive() override {
+    return driver_->writeKeepalive(urcl::RobotReceiveTimeout::millisec(1000));
+  }
+  bool zeroFT() override { return driver_->zeroFTSensor(); }
+  void stopControl() override { driver_->stopControl(); }
+};
+
+std::unique_ptr<RobotIO> openLiveRobotIo(const Args &args, std::atomic<bool> &stop,
+                                         std::string *error) {
+  auto urio = std::make_unique<UrclIO>(stop);
+  if (!urio->init(args)) {
+    if (error)
+      *error = "external_control_start_failed";
+    return nullptr;
+  }
+  return urio;
+}