roboapi 0.4.2__py3-none-any.whl

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

@@ -0,0 +1,369 @@
+// Cartesian reference generator, independent of robot I/O and wall-clock timing.
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+
+#include "ur_cpp/motion_primitives.hpp"
+
+inline double streamingPosePeriod(const Args &args) {
+  return std::isfinite(args.freq_) && args.freq_ > 0.0 ? 1.0 / args.freq_ : 0.002;
+}
+
+// A moving-target follower that retains Ruckig velocity and acceleration
+// between updates. last_vel_ is a spatial
+// twist; it must not be converted from a rotvec rate a second time.
+struct StreamingPoseShaper {
+  Args args_;
+  double gain_ = 15.0;
+  double dt_ = 0.002;
+  TwistGovernor tracker_;
+  vec6 last_cmd_{}, last_vel_{}, previous_error_{};
+  Quat orientation_{1.0, 0.0, 0.0, 0.0};
+  bool have_last_ = false;
+  bool errors_ = false;
+
+  struct PositionBounds {
+    double min = std::numeric_limits<double>::infinity();
+    double max = -std::numeric_limits<double>::infinity();
+    bool valid = false;
+  };
+
+  explicit StreamingPoseShaper(const Args &args, double gain = 15.0)
+      : args_(args), gain_(gain), dt_(streamingPosePeriod(args)),
+        tracker_(dt_, args.cmd_vel_trans_, args.cmd_vel_rot_, args.cmd_acc_trans_,
+                 args.cmd_acc_rot_, 20.0 * args.cmd_acc_trans_, 20.0 * args.cmd_acc_rot_) {}
+
+  static bool plausiblePose(const vec6 &pose) {
+    // Bound intermediate arithmetic before rotation-vector conversion;
+    // physical workspace bounds are checked by the control law.
+    constexpr double kNumericPoseLimit = 1e6;
+    return finite6(pose) && std::all_of(pose.begin(), pose.end(),
+                                        [](double v) { return std::abs(v) <= kNumericPoseLimit; });
+  }
+
+  bool validLimits(int axis) const {
+    const double maxv = axis < 3 ? args_.cmd_vel_trans_ : args_.cmd_vel_rot_;
+    const double maxa = axis < 3 ? args_.cmd_acc_trans_ : args_.cmd_acc_rot_;
+    return std::isfinite(maxv) && maxv > 0.0 && std::isfinite(maxa) && maxa > 0.0 &&
+           std::isfinite(20.0 * maxa) && std::isfinite(dt_) && dt_ > 0.0 && std::isfinite(gain_) &&
+           gain_ > 0.0;
+  }
+
+  void sync(const vec6 &pose, const vec6 &velocity = {}) {
+    last_cmd_ = pose;
+    last_vel_ = velocity;
+    previous_error_ = {};
+    orientation_ =
+        plausiblePose(pose) ? rotvecToQuat(pose[3], pose[4], pose[5]) : Quat{1.0, 0.0, 0.0, 0.0};
+    tracker_.reset(velocity);
+    have_last_ = plausiblePose(pose) && finite6(velocity);
+    errors_ = !have_last_;
+  }
+
+  vec6 reject() {
+    errors_ = true;
+    return last_cmd_;
+  }
+
+  static bool addPoint(PositionBounds &bounds, double p) {
+    if (!std::isfinite(p))
+      return false;
+    bounds.min = std::min(bounds.min, p);
+    bounds.max = std::max(bounds.max, p);
+    bounds.valid = true;
+    return true;
+  }
+
+  // Add a cubic jerk segment, clipped to [0, horizon].  Every field of a
+  // positive-duration segment must be finite: silently skipping a malformed
+  // segment would turn a numerical failure into an apparent safe stop.
+  static bool addSegmentBounds(PositionBounds &bounds, double p, double v, double a, double j,
+                               double duration, double horizon, double &end_p, double &end_v,
+                               double &end_a) {
+    if (!std::isfinite(duration) || duration < 0.0 || !std::isfinite(horizon) || horizon < 0.0)
+      return false;
+    if (!std::isfinite(p) || !std::isfinite(v) || !std::isfinite(a) || !std::isfinite(j))
+      return false;
+    const double used = std::min(duration, horizon);
+    auto eval = [&](long double t) {
+      return static_cast<double>(p + t * (v + t * (a / 2.0L + t * j / 6.0L)));
+    };
+    auto add = [&](long double t) {
+      if (!std::isfinite(t))
+        return false;
+      if (t < 0.0L || t > static_cast<long double>(used))
+        return true;
+      return addPoint(bounds, eval(t));
+    };
+    if (!add(0.0) || !add(used))
+      return false;
+
+    // Position extrema occur where v(t)=0.  Avoid Profile::pf and
+    // get_position_extrema(): velocity-interface profiles do not initialize
+    // those fields consistently in the pinned Ruckig commit.
+    if (j == 0.0) {
+      if (a != 0.0 && !add(-static_cast<long double>(v) / a))
+        return false;
+    } else {
+      const long double qa = static_cast<long double>(j) / 2.0L;
+      const long double qb = a, qc = v;
+      const long double discriminant = qb * qb - 4.0L * qa * qc;
+      if (!std::isfinite(discriminant))
+        return false;
+      if (discriminant >= 0.0L) {
+        const long double root = std::sqrt(discriminant);
+        // Stable quadratic roots: avoid cancellation when |a| ~= sqrt(D).
+        const long double q = -0.5L * (qb + std::copysign(root, qb));
+        if (!std::isfinite(root))
+          return false;
+        if (q == 0.0L) {
+          if (!add(-qb / (2.0L * qa)))
+            return false;
+        } else if (!add(q / qa) || !add(qc / q)) {
+          return false;
+        }
+      }
+    }
+    // Never evaluate a future part of a segment outside the requested horizon.
+    end_p = eval(used);
+    end_v = static_cast<double>(v + static_cast<long double>(used) *
+                                        (a + static_cast<long double>(used) * j / 2.0L));
+    end_a = static_cast<double>(a + static_cast<long double>(used) * j);
+    return std::isfinite(end_p) && std::isfinite(end_v) && std::isfinite(end_a);
+  }
+
+  // Bounds over a trajectory's first `horizon` seconds. Arrival shaping
+  // evaluates the next period and the subsequent deceleration to the target.  The horizon may
+  // exceed the trajectory duration: at_time() then extrapolates its final velocity and
+  // acceleration, including when the entire trajectory has zero duration.
+  static bool trajectoryBounds(const ruckig::Trajectory<1> &trajectory, double horizon,
+                               double fallback_position, PositionBounds &bounds) {
+    bounds = {};
+    if (!std::isfinite(horizon) || horizon < 0.0 || !std::isfinite(fallback_position) ||
+        !std::isfinite(trajectory.get_duration()) || trajectory.get_duration() < 0.0)
+      return false;
+    const auto profiles = trajectory.get_profiles();
+    if (profiles.size() != 1)
+      return false;
+    const auto &profile = profiles[0][0];
+    if (!std::isfinite(profile.brake.duration) || profile.brake.duration < 0.0)
+      return false;
+
+    // Explicit endpoint checks cover zero-duration trajectories and ensure the
+    // caller's initial position agrees with Ruckig's actual local coordinates.
+    std::array<double, 1> p{}, v{}, a{};
+    trajectory.at_time(0.0, p, v, a);
+    if (!std::isfinite(p[0]) || !std::isfinite(v[0]) || !std::isfinite(a[0]) ||
+        std::abs(p[0] - fallback_position) > 1e-9 * (1.0 + std::abs(fallback_position)) ||
+        !addPoint(bounds, p[0]))
+      return false;
+    trajectory.at_time(horizon, p, v, a);
+    if (!std::isfinite(p[0]) || !std::isfinite(v[0]) || !std::isfinite(a[0]) ||
+        !addPoint(bounds, p[0]))
+      return false;
+
+    double elapsed = 0.0;
+    auto add = [&](double p, double v, double a, double j, double duration) {
+      double end_p = 0.0, end_v = 0.0, end_a = 0.0;
+      return addSegmentBounds(bounds, p, v, a, j, duration, horizon - elapsed, end_p, end_v, end_a);
+    };
+
+    // A BrakeProfile with duration==0 need not initialize any of its arrays.
+    // With an active brake, t[] is initialized but p/v/a of zero-length slots
+    // is not.  Check durations first, before passing value-array arguments.
+    if (profile.brake.duration > 0.0) {
+      for (std::size_t i = 0; i < 2; ++i) {
+        const double duration = profile.brake.t[i];
+        if (!std::isfinite(duration) || duration < 0.0)
+          return false;
+        if (duration > 0.0 && elapsed < horizon &&
+            !add(profile.brake.p[i], profile.brake.v[i], profile.brake.a[i], profile.brake.j[i],
+                 duration))
+          return false;
+        elapsed += duration;
+      }
+      if (!std::isfinite(elapsed) ||
+          std::abs(elapsed - profile.brake.duration) > 1e-10 * (1.0 + elapsed))
+        return false;
+    }
+    // Pin to the same start time used by Trajectory::at_time, not a rounded
+    // sum of the pre-brake segments.
+    elapsed = profile.brake.duration;
+    double main_duration = 0.0;
+    for (std::size_t i = 0; i < 7; ++i) {
+      const double duration = profile.t[i];
+      if (!std::isfinite(duration) || duration < 0.0)
+        return false;
+      if (duration > 0.0 && elapsed < horizon &&
+          !add(profile.p[i], profile.v[i], profile.a[i], profile.j[i], duration))
+        return false;
+      main_duration += duration;
+      if (!std::isfinite(main_duration) || !std::isfinite(profile.t_sum[i]) ||
+          std::abs(main_duration - profile.t_sum[i]) > 1e-10 * (1.0 + main_duration))
+        return false;
+      elapsed = profile.brake.duration + profile.t_sum[i];
+    }
+    if (!std::isfinite(elapsed) || elapsed > trajectory.get_duration() + 1e-10 * (1.0 + elapsed))
+      return false;
+
+    // p/v/a.back() are initialized by successful velocity-profile validation,
+    // including zero-duration profiles.  Never use the unrelated Profile::pf.
+    // This tail is required both for synchronization and duration < dt_.
+    if (elapsed < horizon) {
+      const double tail = horizon - elapsed;
+      if (!add(profile.p.back(), profile.v.back(), profile.a.back(), 0.0, tail))
+        return false;
+    }
+    return bounds.valid && std::isfinite(bounds.min) && std::isfinite(bounds.max);
+  }
+
+  double stoppingLimitedGoal(int axis, double goal, double error) {
+    if (axis < 0 || axis >= 6 || !validLimits(axis) || !std::isfinite(goal) ||
+        !std::isfinite(error)) {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    const double maxv = axis < 3 ? args_.cmd_vel_trans_ : args_.cmd_vel_rot_;
+    const double maxa = (axis < 3 ? args_.cmd_acc_trans_ : args_.cmd_acc_rot_) / std::sqrt(3.0);
+    if (!std::isfinite(maxv) || !std::isfinite(maxa) || maxv <= 0.0 || maxa <= 0.0) {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    goal = std::clamp(goal, -maxv, maxv);
+    ruckig::Ruckig<1> solver(dt_);
+    ruckig::InputParameter<1> input;
+    input.control_interface = ruckig::ControlInterface::Velocity;
+    input.synchronization = ruckig::Synchronization::None;
+    input.current_position = {0.0};
+    input.target_position = {0.0};
+    input.current_velocity = {tracker_.in_.current_velocity[axis]};
+    input.current_acceleration = {tracker_.in_.current_acceleration[axis]};
+    input.target_acceleration = {0.0};
+    input.max_velocity = {maxv};
+    input.max_acceleration = {maxa};
+    input.max_jerk = {20.0 * maxa};
+    if (!std::isfinite(input.current_velocity[0]) ||
+        !std::isfinite(input.current_acceleration[0])) {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+    if (goal == 0.0 && input.current_velocity[0] == 0.0 && input.current_acceleration[0] == 0.0)
+      return 0.0;
+    const double direction = std::copysign(1.0, error);
+    enum class Fit { WithinTarget, Overshoot, Invalid };
+    auto feasible = [&](double velocity) {
+      if (!std::isfinite(velocity)) {
+        return Fit::Invalid;
+      }
+      auto test = input;
+      test.target_velocity = {velocity};
+      ruckig::Trajectory<1> motion, brake;
+      if (solver.calculate(test, motion) < 0) {
+        return Fit::Invalid;
+      }
+      std::array<double, 1> p{}, v{}, a{};
+      motion.at_time(dt_, p, v, a);
+      if (!std::isfinite(p[0]) || !std::isfinite(v[0]) || !std::isfinite(a[0])) {
+        return Fit::Invalid;
+      }
+      auto brake_input = test;
+      brake_input.current_position = p;
+      brake_input.current_velocity = v;
+      brake_input.current_acceleration = a;
+      brake_input.target_velocity = {0.0};
+      brake_input.target_acceleration = {0.0};
+      if (solver.calculate(brake_input, brake) < 0) {
+        return Fit::Invalid;
+      }
+      PositionBounds motion_bounds, brake_bounds;
+      if (!trajectoryBounds(motion, dt_, 0.0, motion_bounds) ||
+          !trajectoryBounds(brake, brake.get_duration(), p[0], brake_bounds)) {
+        return Fit::Invalid;
+      }
+      const double motion_forward =
+          error == 0.0 ? std::max(std::abs(motion_bounds.min), std::abs(motion_bounds.max))
+                       : (direction > 0.0 ? motion_bounds.max : -motion_bounds.min);
+      const double brake_forward =
+          error == 0.0 ? std::max(std::abs(brake_bounds.min), std::abs(brake_bounds.max))
+                       : (direction > 0.0 ? brake_bounds.max : -brake_bounds.min);
+      return std::isfinite(motion_forward) && std::isfinite(brake_forward) &&
+                     std::max(motion_forward, brake_forward) <= std::abs(error) + 1e-10
+                 ? Fit::WithinTarget
+                 : Fit::Overshoot;
+    };
+    if (feasible(goal) != Fit::Overshoot)
+      return goal;
+    const auto stop = feasible(0.0);
+    if (stop == Fit::Invalid)
+      return goal;
+    if (stop == Fit::Overshoot)
+      return 0.0;
+    double lo = 0.0, hi = 1.0;
+    for (int i = 0; i < 16; ++i) {
+      const double mid = (lo + hi) / 2.0;
+      const auto fit = feasible(mid * goal);
+      if (fit == Fit::Invalid)
+        return goal;
+      if (fit == Fit::WithinTarget)
+        lo = mid;
+      else
+        hi = mid;
+    }
+    return lo * goal;
+  }
+
+  vec6 step(const vec6 &target) {
+    errors_ = false;
+    if (!have_last_ || !plausiblePose(target) || !std::isfinite(gain_) || gain_ <= 0.0 ||
+        !std::isfinite(args_.freq_) || args_.freq_ <= 0.0)
+      return reject();
+
+    vec6 goal{}, geometric_error{};
+    for (int i = 0; i < 3; ++i)
+      geometric_error[i] = target[i] - last_cmd_[i];
+    const auto delta =
+        quatMul(rotvecToQuat(target[3], target[4], target[5]), quatConj(orientation_));
+    vec6 error{};
+    quatToRotvec(delta, error[3], error[4], error[5], &previous_error_);
+    previous_error_ = error;
+    for (int i = 3; i < 6; ++i)
+      geometric_error[i] = error[i];
+    if (!finite6(geometric_error))
+      return reject();
+    for (int i = 0; i < 6; ++i) {
+      const double proposed = gain_ * geometric_error[i];
+      if (!std::isfinite(proposed))
+        return reject();
+      goal[i] = stoppingLimitedGoal(i, proposed, geometric_error[i]);
+    }
+    if (!finite6(goal) || !tracker_.step(goal))
+      return reject();
+    const auto &displacement = tracker_.displacement_;
+    const auto &velocity = tracker_.sent_;
+
+    vec6 next = last_cmd_;
+    for (int i = 0; i < 3; ++i)
+      next[i] += displacement[i];
+    // Spatial angular velocity uses left quaternion multiplication.  This is
+    // a local spatial-twist integration; non-commuting multi-axis error and
+    // SO(3) branch continuity remain explicit offline acceptance gates.
+    auto next_orientation =
+        quatMul(rotvecToQuat(displacement[3], displacement[4], displacement[5]), orientation_);
+    const double qnorm = std::hypot(std::hypot(next_orientation.w_, next_orientation.x_),
+                                    std::hypot(next_orientation.y_, next_orientation.z_));
+    if (!std::isfinite(qnorm) || qnorm < 1e-12)
+      return reject();
+    next_orientation.w_ /= qnorm;
+    next_orientation.x_ /= qnorm;
+    next_orientation.y_ /= qnorm;
+    next_orientation.z_ /= qnorm;
+    quatToRotvec(next_orientation, next[3], next[4], next[5], &last_cmd_);
+    if (!plausiblePose(next))
+      return reject();
+    last_cmd_ = next;
+    last_vel_ = velocity;
+    orientation_ = next_orientation;
+    return last_cmd_;
+  }
+};