roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/ur/native/include/ur_cpp/motion_primitives.hpp

@@ -0,0 +1,350 @@
+// Deterministic Cartesian control primitives, independent of robot IO.
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+#include <optional>
+
+#include <ruckig/ruckig.hpp>
+
+#include "ur_cpp/control_loop.hpp"
+#include "ur_cpp/geom.hpp"
+
+inline vec6 rotateAngular(const Quat &q, const vec6 &v) {
+  vec6 r = v;
+  const auto w = quatMul(quatMul(q, {0, v[3], v[4], v[5]}), quatConj(q));
+  r[3] = w.x_;
+  r[4] = w.y_;
+  r[5] = w.z_;
+  return r;
+}
+
+struct AdmittanceLaw {
+  Tuning tun_;
+  vec6 filt_{}, vel_{}, off_{}, before_{}, force_{};
+  vec6 Mh_{}, D0_{}, kr_{}, maxwell_{}, maxwell_before_{}, candidate_{};
+  bool first_ = true;
+  bool notch_on_ = false;
+  double nb_[2][3]{}, na_[2][2]{}, nx_[2][6][2]{}, ny_[2][6][2]{};
+
+  explicit AdmittanceLaw(const Tuning &t) : tun_(t) {
+    const double wc = 2 * M_PI * t.relaxation_hz_;
+    for (int i = 0; i < 6; ++i) {
+      const double damping = 2 * t.zeta_ * std::sqrt(t.M_[i]) * std::sqrt(t.K_[i]);
+      const double extra = (t.hf_mass_ratio_[i] - 1) * t.M_[i];
+      Mh_[i] = t.M_[i] + extra;
+      D0_[i] = damping - extra * wc;
+      kr_[i] = extra * wc * wc;
+    }
+  }
+  void initNotch(double fs) {
+    notch_on_ = tun_.notch_freq_ > 0;
+    for (int s = 0; s < 2; ++s) {
+      const double w = 2 * M_PI * tun_.notch_freq_ * (s + 1) / fs;
+      if (w >= .9 * M_PI) {
+        nb_[s][0] = 1;
+        continue;
+      }
+      const double alpha = std::sin(w) / 8, den = 1 + alpha;
+      nb_[s][0] = nb_[s][2] = 1 / den;
+      nb_[s][1] = -2 * std::cos(w) / den;
+      na_[s][0] = nb_[s][1];
+      na_[s][1] = (1 - alpha) / den;
+    }
+  }
+  void reset() {
+    filt_ = {};
+    vel_ = {};
+    off_ = {};
+    before_ = {};
+    force_ = {};
+    maxwell_ = {};
+    maxwell_before_ = {};
+    candidate_ = {};
+    std::memset(nx_, 0, sizeof(nx_));
+    std::memset(ny_, 0, sizeof(ny_));
+    first_ = true;
+  }
+  // Predict velocity, without committing a fictitious unconstrained position.
+  bool predict(const vec6 &wrench, double dt) {
+    if (!finite6(wrench) || !finite6(off_) || !finite6(vel_) || !finite6(maxwell_) ||
+        !std::isfinite(dt) || dt <= 0)
+      return false;
+    before_ = off_;
+    maxwell_before_ = maxwell_;
+    vec6 w = wrench;
+    if (notch_on_)
+      for (int s = 0; s < 2; ++s)
+        for (int i = 0; i < 6; ++i) {
+          const double y = nb_[s][0] * w[i] + nb_[s][1] * nx_[s][i][0] + nb_[s][2] * nx_[s][i][1] -
+                           na_[s][0] * ny_[s][i][0] - na_[s][1] * ny_[s][i][1];
+          nx_[s][i][1] = nx_[s][i][0];
+          nx_[s][i][0] = w[i];
+          ny_[s][i][1] = ny_[s][i][0];
+          ny_[s][i][0] = y;
+          w[i] = y;
+        }
+    for (int i = 0; i < 6; ++i) {
+      filt_[i] = first_ ? w[i] : filt_[i] + dt / (tun_.filter_tau_ + dt) * (w[i] - filt_[i]);
+      const double db = i < 3 ? tun_.deadband_force_ : tun_.deadband_torque_;
+      force_[i] = std::copysign(std::max(0.0, std::abs(filt_[i]) - db), filt_[i]);
+      // One implicit solve for damping + Maxwell stress, with the previous
+      // geometric spring. No speculative stress or offset is committed here.
+      const double g = 1 / (1 + dt * 2 * M_PI * tun_.relaxation_hz_);
+      const double den = Mh_[i] + dt * D0_[i] + dt * dt * kr_[i] * g;
+      if (!std::isfinite(den) || den <= 0)
+        return false;
+      vel_[i] =
+          (Mh_[i] * vel_[i] + dt * (force_[i] - tun_.K_[i] * off_[i] - g * maxwell_before_[i])) /
+          den;
+    }
+    first_ = false;
+    if (!finite6(filt_) || !finite6(force_) || !finite6(vel_))
+      return false;
+    const double n = norm3(vel_);
+    if (tun_.max_vel_ > 0 && n > tun_.max_vel_)
+      for (int i = 0; i < 3; ++i)
+        vel_[i] *= tun_.max_vel_ / n;
+    candidate_ = vel_;
+    return true;
+  }
+  // Only cancel an unexecuted component of the virtual motion. A P/reference
+  // shaping residual must not create offset motion, change its direction, or
+  // amplify it. Translation and rotation have independent dimensional groups.
+  bool commit(const vec6 &governor_residual, double dt) {
+    if (!finite6(governor_residual) || !finite6(candidate_) || !std::isfinite(dt) || dt <= 0)
+      return false;
+    vec6 accepted{}, next = before_, stress{};
+    for (int o : {0, 3}) {
+      const double n = norm3(candidate_, o);
+      double alpha = 0;
+      if (n > 1e-12) {
+        double along = 0;
+        for (int i = o; i < o + 3; ++i)
+          along += governor_residual[i] * (candidate_[i] / n);
+        alpha = std::clamp(1 + along / n, 0., 1.);
+      }
+      for (int i = o; i < o + 3; ++i)
+        accepted[i] = alpha * candidate_[i];
+    }
+    if (!finite6(accepted))
+      return false;
+    for (int i = 0; i < 3; ++i)
+      next[i] = before_[i] + dt * accepted[i];
+    const auto r = quatMul(rotvecToQuat(dt * accepted[3], dt * accepted[4], dt * accepted[5]),
+                           rotvecToQuat(before_[3], before_[4], before_[5]));
+    quatToRotvec(r, next[3], next[4], next[5], &before_);
+    const double g = 1 / (1 + dt * 2 * M_PI * tun_.relaxation_hz_);
+    for (int i = 0; i < 6; ++i)
+      stress[i] = g * (maxwell_before_[i] + dt * kr_[i] * accepted[i]);
+    if (!finite6(next) || !finite6(stress))
+      return false;
+    vel_ = accepted;
+    off_ = next;
+    maxwell_ = stress;
+    return true;
+  }
+};
+
+// Continuous vector velocity / acceleration / jerk shaping. Candidate rejection
+// never latches motion: advance the accepted trajectory and retry next period.
+struct TwistGovernor {
+  ruckig::Ruckig<6> otg_;
+  ruckig::InputParameter<6> in_;
+  vec6 sent_{}, acceleration_{}, displacement_{};
+  double dt_, vt_, vr_;
+
+  TwistGovernor(double h, double v, double w, double a, double alpha, double j, double jw)
+      : otg_(h), dt_(h), vt_(v), vr_(w) {
+    in_.control_interface = ruckig::ControlInterface::Velocity;
+    in_.synchronization = ruckig::Synchronization::None;
+    in_.current_position = {};
+    in_.target_position = {};
+    for (int i = 0; i < 6; ++i) {
+      in_.max_velocity[i] = i < 3 ? v : w;
+      in_.max_acceleration[i] = (i < 3 ? a : alpha) / std::sqrt(3.0);
+      in_.max_jerk[i] = (i < 3 ? j : jw) / std::sqrt(3.0);
+      in_.target_acceleration[i] = 0;
+    }
+  }
+
+  void reset(const vec6 &velocity = {}) {
+    in_.current_position = {};
+    in_.current_velocity = sent_ = velocity;
+    in_.current_acceleration = acceleration_ = {};
+    displacement_ = {};
+    accepted_.reset();
+    elapsed_ = 0;
+  }
+
+  static bool velocitySegmentInside(const vec6 &v, const vec6 &a, const vec6 &j, double duration,
+                                    int offset, double limit, int depth = 0) {
+    // A quadratic velocity segment has three Bernstein control points. The
+    // convex hull bounds its norm at every instant, including between ticks.
+    vec6 b0 = v, b1 = v, b2 = v;
+    for (int i = offset; i < offset + 3; ++i) {
+      b1[i] += a[i] * duration / 2;
+      b2[i] += a[i] * duration + j[i] * duration * duration / 2;
+    }
+    if (!finite6(b0) || !finite6(b1) || !finite6(b2))
+      return false;
+    const double cap = limit + 1e-8;
+    if (norm3(b0, offset) > cap || norm3(b2, offset) > cap)
+      return false;
+    if (norm3(b1, offset) <= cap)
+      return true;
+    if (depth >= 20)
+      return false; // conservative on unresolved near-limit arcs
+    vec6 mid_v = v, mid_a = a;
+    for (int i = offset; i < offset + 3; ++i) {
+      mid_v[i] += a[i] * duration / 2 + j[i] * duration * duration / 8;
+      mid_a[i] += j[i] * duration / 2;
+    }
+    return velocitySegmentInside(v, a, j, duration / 2, offset, limit, depth + 1) &&
+           velocitySegmentInside(mid_v, mid_a, j, duration / 2, offset, limit, depth + 1);
+  }
+
+  bool vectorVelocityInside(const ruckig::Trajectory<6> &trajectory) const {
+    const auto profiles = trajectory.get_profiles();
+    const double duration = trajectory.get_duration();
+    if (profiles.size() != 1 || !std::isfinite(duration) || duration < 0)
+      return false;
+    // Include Ruckig's initial acceleration correction, when present.
+    std::array<double, 64> times{};
+    std::size_t count = 0;
+    times[count++] = 0;
+    times[count++] = duration;
+    for (const auto &profile : profiles[0]) {
+      if (!std::isfinite(profile.brake.duration) || profile.brake.duration < 0)
+        return false;
+      if (profile.brake.duration > 0) {
+        double t = 0;
+        for (double h : profile.brake.t) {
+          t += h;
+          if (t > 0 && t < duration)
+            times[count++] = t;
+        }
+      }
+      for (double t : profile.t_sum) {
+        t += profile.brake.duration;
+        if (t > 0 && t < duration)
+          times[count++] = t;
+      }
+    }
+    std::sort(times.begin(), times.begin() + count);
+    // On entry from another mode, recover an existing overspeed continuously.
+    const double trans = std::max(vt_, norm3(in_.current_velocity));
+    const double rot = std::max(vr_, norm3(in_.current_velocity, 3));
+    for (std::size_t k = 0; k + 1 < count; ++k) {
+      const double start = times[k], end = times[k + 1];
+      if (end - start <= 1e-12)
+        continue;
+      vec6 p{}, v{}, a{}, j{};
+      trajectory.at_time(start, p, v, a);
+      if (!finite6(p) || !finite6(v) || !finite6(a))
+        return false;
+      const double mid = (start + end) / 2;
+      for (int axis = 0; axis < 6; ++axis) {
+        const auto &profile = profiles[0][axis];
+        if (mid < profile.brake.duration) {
+          const int phase = mid < profile.brake.t[0] ? 0 : 1;
+          j[axis] = profile.brake.j[phase];
+        } else {
+          for (int phase = 0; phase < 7; ++phase)
+            if (mid - profile.brake.duration < profile.t_sum[phase]) {
+              j[axis] = profile.j[phase];
+              break;
+            }
+        }
+      }
+      if (!velocitySegmentInside(v, a, j, end - start, 0, trans) ||
+          !velocitySegmentInside(v, a, j, end - start, 3, rot))
+        return false;
+    }
+    vec6 p{}, v{}, a{};
+    trajectory.at_time(duration, p, v, a);
+    return finite6(p) && finite6(v) && finite6(a) && norm3(v) <= trans + 1e-8 &&
+           norm3(v, 3) <= rot + 1e-8;
+  }
+
+  bool step(const vec6 &target) {
+    if (!finite6(target) || !finite6(in_.current_velocity) || !finite6(in_.current_acceleration) ||
+        !std::isfinite(dt_) || dt_ <= 0)
+      return false;
+    in_.current_position = {};
+    in_.target_velocity = target;
+    for (int offset : {0, 3}) {
+      const double n = std::hypot(target[offset], target[offset + 1], target[offset + 2]);
+      const double cap = offset == 0 ? vt_ : vr_;
+      if (n > cap)
+        for (int i = offset; i < offset + 3; ++i)
+          in_.target_velocity[i] *= cap / n;
+    }
+    const auto desired = in_.target_velocity;
+    ruckig::Trajectory<6> candidate;
+    auto calculate = [&] {
+      return otg_.calculate(in_, candidate) >= 0 && vectorVelocityInside(candidate);
+    };
+    bool found = calculate();
+    if (!found) {
+      // Preserve the vector envelope during turns with independent axis timing.
+      in_.target_velocity = {};
+      found = calculate();
+      if (found) {
+        double low = 0, high = 1;
+        ruckig::Trajectory<6> best = candidate;
+        for (int iteration = 0; iteration < 16; ++iteration) {
+          const double scale = (low + high) / 2;
+          for (int i = 0; i < 6; ++i)
+            in_.target_velocity[i] = desired[i] * scale;
+          if (calculate()) {
+            low = scale;
+            best = candidate;
+          } else {
+            high = scale;
+          }
+        }
+        candidate = best;
+        for (int i = 0; i < 6; ++i)
+          in_.target_velocity[i] = desired[i] * low;
+      }
+    }
+    if (found) {
+      accepted_ = candidate;
+      elapsed_ = 0;
+    }
+    // A failed solve leaves the existing plan advancing, including its velocity
+    // and acceleration. There is no fault, zero-velocity target latch or pause.
+    if (!advance())
+      return false;
+    in_.current_velocity = sent_;
+    in_.current_acceleration = acceleration_;
+    return true;
+  }
+
+private:
+  std::optional<ruckig::Trajectory<6>> accepted_;
+  double elapsed_ = 0;
+
+  bool advance() {
+    if (accepted_) {
+      vec6 previous{}, velocity{}, acceleration{}, position{};
+      accepted_->at_time(elapsed_, previous, velocity, acceleration);
+      elapsed_ += dt_;
+      accepted_->at_time(elapsed_, position, velocity, acceleration);
+      if (!finite6(previous) || !finite6(position) || !finite6(velocity) || !finite6(acceleration))
+        return false;
+      for (int i = 0; i < 6; ++i)
+        displacement_[i] = position[i] - previous[i];
+      sent_ = velocity;
+      acceleration_ = acceleration;
+      if (elapsed_ >= accepted_->get_duration())
+        accepted_.reset();
+    } else {
+      for (int i = 0; i < 6; ++i)
+        displacement_[i] = sent_[i] * dt_;
+    }
+    return finite6(displacement_) && finite6(sent_) && finite6(acceleration_);
+  }
+};

roboapi/hardware/ur/native/include/ur_cpp/robot_io.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <atomic>
+#include <cstdint>
+#include <memory>
+#include <string>
+
+#include "ur_cpp/control_loop.hpp"
+
+// Reverse-interface writer. The RTDE thread is the only caller of write*.
+struct RobotIO {
+  double robot_time_ = 0;
+  int32_t safety_mode_ = 1;
+  vec6 tcp_speed_{}, qd_{};
+  std::atomic<bool> program_running_{false};
+  virtual ~RobotIO() = default;
+  virtual bool tick(vec6 &tcp_pose, vec6 &q, vec6 &wrench) = 0;
+  virtual bool writeTwist(const vec6 &tw) = 0;
+  virtual bool writeKeepalive() = 0;
+  virtual bool zeroFT() = 0;
+  virtual void stopControl() = 0;
+};
+
+// The worker opens URCL headless external-control; unit tests provide in-memory IO.
+// On failure, error is set and the pointer is null.
+std::unique_ptr<RobotIO> openRobotIo(const Args &args, std::atomic<bool> &stop, std::string *error);