roboapi 0.4.2__py3-none-any.whl

roboapi/hardware/ur/native/tests/cartesian_speedl_unit.cpp

@@ -0,0 +1,113 @@
+// Pure Cartesian control-law regression.
+#include "ur_cpp/cartesian_speedl.hpp"
+
+#include <cassert>
+#include <cmath>
+#include <limits>
+#include <string>
+
+int main() {
+  Args args;
+  std::string error;
+  CartesianSpeedlLaw law(args);
+  assert(law.validate(error));
+
+  const vec6 actual{0.3, -0.2, 0.4, 0.0, 3.14159265, 0.0};
+  law.reset(actual, vec6{});
+  CartesianTick tick;
+  tick.reference_ = actual;
+  tick.reference_[0] += 0.01;
+  tick.actual_ = actual;
+  tick.dt_ = 1.0 / args.freq_;
+  CartesianCommand command;
+  assert(law.step(tick, command));
+  assert(finite6(command.twist_));
+  assert(command.fault_ == LoopFault::None);
+
+  Args invalid = args;
+  invalid.speedl_acc_trans_ = 0;
+  error.clear();
+  CartesianSpeedlLaw invalid_law(invalid);
+  assert(!invalid_law.validate(error));
+  assert(error.find("acceleration and jerk") != std::string::npos);
+
+  // Large, sustained tracking error and force keep flowing through all stages.
+  CartesianSpeedlLaw continuous(args);
+  continuous.reset(actual, {});
+  tick.reference_ = actual;
+  tick.reference_[0] += 0.5;
+  tick.actual_ = actual; // Deliberately stalled plant: no tracking-triggered pause.
+  const auto rotation =
+      quatMul(rotvecToQuat(0.8, 0, 0), rotvecToQuat(actual[3], actual[4], actual[5]));
+  quatToRotvec(rotation, tick.reference_[3], tick.reference_[4], tick.reference_[5]);
+  tick.wrench_[0] = 200.0;
+  vec6 acceleration{};
+  for (int n = 0; n < 1000; ++n) {
+    assert(continuous.step(tick, command));
+    assert(command.fault_ == LoopFault::None);
+    assert(norm3(command.twist_) <= args.cmd_vel_trans_ + 1e-8);
+    assert(norm3(command.twist_, 3) <= args.cmd_vel_rot_ + 1e-8);
+    assert(norm3(continuous.law_.vel_) <= args.tun_.max_vel_ + 1e-8);
+    for (int i = 0; i < 6; ++i) {
+      const double a = (i < 3 ? args.speedl_acc_trans_ : args.speedl_acc_rot_) / std::sqrt(3.0);
+      const double j = (i < 3 ? args.speedl_jerk_trans_ : args.speedl_jerk_rot_) / std::sqrt(3.0);
+      assert(std::abs(continuous.governor_.acceleration_[i]) <= a + 1e-8);
+      assert(std::abs(continuous.governor_.acceleration_[i] - acceleration[i]) <=
+             j * tick.dt_ + 1e-8);
+    }
+    acceleration = continuous.governor_.acceleration_;
+    if (n == 0)
+      assert(continuous.shaper_.last_cmd_[0] - actual[0] < 0.001);
+  }
+  assert(continuous.shaper_.last_cmd_[0] > actual[0] + 0.45);
+  assert(command.desired_[0] > actual[0] + 0.1);
+  assert(norm3(command.twist_) > 0.01);
+
+  // Explicit position returns must ignore force offsets, including after a policy run.
+  CartesianSpeedlLaw position(args), no_force(args);
+  position.reset(actual, {});
+  no_force.reset(actual, {});
+  tick.reference_ = actual;
+  tick.reference_[0] += 0.03;
+  tick.actual_ = actual;
+  tick.wrench_ = {100, -50, 80, 5, -3, 2};
+  CartesianCommand baseline;
+  for (int n = 0; n < 500; ++n) {
+    assert(position.step(tick, command, false));
+    auto unloaded = tick;
+    unloaded.wrench_ = {};
+    assert(no_force.step(unloaded, baseline, false));
+    assert(command.twist_ == baseline.twist_);
+    assert(command.desired_ == baseline.desired_);
+  }
+  assert(norm3(position.law_.off_) == 0);
+  continuous.reset(actual, {}); // The mode handoff resets from stationary measured feedback.
+  tick.reference_ = actual;
+  assert(continuous.step(tick, command, false));
+  assert(command.desired_ == actual && norm3(command.twist_) < 1e-12);
+
+  CartesianTick invalid_tick;
+  invalid_tick.reference_ = invalid_tick.actual_ = actual;
+  for (double dt : {0.0, -.002, .004, .015, std::numeric_limits<double>::quiet_NaN(),
+                    std::numeric_limits<double>::infinity()}) {
+    law.reset(actual, vec6{});
+    invalid_tick.dt_ = dt;
+    assert(!law.step(invalid_tick, command));
+    assert(command.fault_ == LoopFault::VirtualStateInvalid);
+    assert(command.twist_ == vec6{});
+  }
+  invalid_tick.dt_ = 1.0 / args.freq_;
+  for (int field = 0; field < 3; ++field)
+    for (double value : {std::numeric_limits<double>::quiet_NaN(),
+                         std::numeric_limits<double>::infinity()}) {
+      law.reset(actual, {});
+      invalid_tick.reference_ = invalid_tick.actual_ = actual;
+      invalid_tick.wrench_ = {};
+      auto &invalid = field == 0 ? invalid_tick.reference_
+                                : field == 1 ? invalid_tick.actual_ : invalid_tick.wrench_;
+      invalid[0] = value;
+      assert(!law.step(invalid_tick, command));
+      assert(command.fault_ == LoopFault::VirtualStateInvalid);
+      assert(command.twist_ == vec6{});
+    }
+}

roboapi/hardware/ur/native/tests/control_modes_unit.cpp

@@ -0,0 +1,137 @@
+// Exercise the real control loop with in-memory robot IO; no UR controller is contacted.
+#include "ur_cpp/robot_io.hpp"
+
+#include <cassert>
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+namespace {
+struct Plant {
+  std::mutex mutex;
+  vec6 pose{0.3, -0.2, 0.4, 0, 3.14159265, 0};
+  vec6 wrench{100, -50, 80, 5, -3, 2};
+  vec6 command{};
+  unsigned writes = 0, keepalives = 0, zero_requests = 0;
+  bool stopped = false, accept_zero = true;
+} plant;
+
+struct TestIO : RobotIO {
+  TestIO() { program_running_ = true; }
+  bool tick(vec6 &pose, vec6 &q, vec6 &wrench) override {
+    std::this_thread::sleep_for(std::chrono::milliseconds(2));
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    pose = plant.pose;
+    wrench = plant.wrench;
+    q = {};
+    robot_time_ += 0.002;
+    return true;
+  }
+  bool writeTwist(const vec6 &command) override {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    plant.command = command;
+    ++plant.writes;
+    return true;
+  }
+  bool writeKeepalive() override {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    ++plant.keepalives;
+    return true;
+  }
+  bool zeroFT() override {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    ++plant.zero_requests;
+    return plant.accept_zero;
+  }
+  void stopControl() override {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    plant.stopped = true;
+  }
+};
+
+template <typename Predicate> void waitFor(Predicate ready) {
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
+  while (!ready()) {
+    assert(std::chrono::steady_clock::now() < deadline);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
+} // namespace
+
+std::unique_ptr<RobotIO> openRobotIo(const Args &, std::atomic<bool> &, std::string *) {
+  return std::make_unique<TestIO>();
+}
+
+int main() {
+  Args args;
+  ControlLoop loop(args);
+  loop.start();
+  assert(loop.waitReady(3.0));
+  const auto home = plant.pose;
+  loop.setPoseRef(home, false);
+  waitFor([&] { return loop.snapshot().mode_ == 5; });
+  unsigned first_writes;
+  {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    first_writes = plant.writes;
+  }
+  waitFor([&] {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    for (double value : plant.command) assert(std::abs(value) < 1e-12);
+    return plant.writes >= first_writes + 30;
+  });
+
+  // A refused zero is a warning, not a latched fault or a stop in either mode.
+  auto checkZero = [&](bool accepted) {
+    unsigned requests, writes;
+    const auto mode = loop.snapshot().mode_;
+    {
+      std::lock_guard<std::mutex> lock(plant.mutex);
+      plant.accept_zero = accepted;
+      requests = plant.zero_requests;
+      writes = plant.writes;
+    }
+    loop.zeroFt();
+    waitFor([&] {
+      std::lock_guard<std::mutex> lock(plant.mutex);
+      return plant.zero_requests == requests + 1 && plant.writes >= writes + 10;
+    });
+    const auto state = loop.snapshot();
+    assert(state.loop_alive_ && state.loop_ok_ && state.mode_ == mode);
+    assert(state.fault_ == LoopFault::None && state.last_error_.empty());
+  };
+  checkZero(false);
+  checkZero(true);
+
+  // Policy execution can enter admittance using the same connection.
+  loop.setPoseRef(home);
+  waitFor([&] { return loop.snapshot().mode_ == 1; });
+  waitFor([&] {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    return std::abs(plant.command[0]) > 1e-4;
+  });
+  checkZero(false);
+  checkZero(true);
+
+  // A subsequent Home stops the old stream and clears its accumulated force offset.
+  unsigned previous_keepalives;
+  {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    previous_keepalives = plant.keepalives;
+  }
+  loop.setPoseRef(home, false);
+  waitFor([&] { return loop.snapshot().mode_ == 5; });
+  {
+    std::lock_guard<std::mutex> lock(plant.mutex);
+    assert(plant.keepalives > previous_keepalives);
+    for (double value : plant.command) assert(std::abs(value) < 1e-12);
+  }
+  loop.stopMotion();
+  waitFor([&] { return loop.snapshot().mode_ == 0; });
+  loop.requestStop();
+  loop.join();
+  assert(plant.stopped);
+  assert(loop.snapshot().last_error_.empty());
+}

roboapi/hardware/ur/native/tests/streaming_pose_unit.cpp

@@ -0,0 +1,348 @@
+// Pure StreamingPoseShaper/cartesian_speedl regression tests.
+// Runs without robot I/O or wall-clock timing.
+#include "ur_cpp/cartesian_speedl.hpp"
+#include "ur_cpp/geom.hpp"
+#include "ur_cpp/streaming_pose.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <limits>
+#include <stdexcept>
+#include <string>
+
+namespace {
+
+constexpr double kDt = 0.002;
+constexpr double kGain = 15.0;
+const vec6 kOrigin{0.3, -0.2, 0.4, 0, 0, 0};
+
+void check(bool condition, const std::string &message) {
+  if (!condition)
+    throw std::runtime_error(message);
+}
+
+double poseAngle(const vec6 &a, const vec6 &b) {
+  const auto delta =
+      quatMul(rotvecToQuat(a[3], a[4], a[5]), quatConj(rotvecToQuat(b[3], b[4], b[5])));
+  vec6 error{};
+  quatToRotvec(delta, error[3], error[4], error[5]);
+  return norm3(error, 3);
+}
+
+struct BoundAudit {
+  vec6 previous_acceleration{};
+
+  void sample(const StreamingPoseShaper &shaper) {
+    check(shaper.errors_ == 0, "StreamingPoseShaper numeric failure");
+    check(finite6(shaper.last_cmd_) && finite6(shaper.last_vel_), "non-finite shaper state");
+    for (int axis = 0; axis < 6; ++axis) {
+      const double max_acceleration =
+          (axis < 3 ? shaper.args_.cmd_acc_trans_ : shaper.args_.cmd_acc_rot_) / std::sqrt(3.0);
+      const double max_jerk = 20.0 * max_acceleration;
+      check(std::abs(shaper.tracker_.acceleration_[axis]) <= max_acceleration + 1e-8,
+            "streaming acceleration bound exceeded");
+      check(std::abs(shaper.tracker_.acceleration_[axis] - previous_acceleration[axis]) /
+                    shaper.dt_ <=
+                max_jerk + 1e-6,
+            "streaming jerk bound exceeded");
+    }
+    check(norm3(shaper.last_vel_) <= shaper.args_.cmd_vel_trans_ + 1e-8 &&
+              norm3(shaper.last_vel_, 3) <= shaper.args_.cmd_vel_rot_ + 1e-8,
+          "streaming vector velocity bound exceeded");
+    previous_acceleration = shaper.tracker_.acceleration_;
+  }
+};
+
+void stationary() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  for (int i = 0; i < 1500; ++i) {
+    shaper.step(kOrigin);
+    bounds.sample(shaper);
+  }
+  for (int axis = 0; axis < 6; ++axis) {
+    check(std::abs(shaper.last_cmd_[axis] - kOrigin[axis]) < 1e-12, "stationary target moved");
+    check(std::abs(shaper.last_vel_[axis]) < 1e-12, "stationary target acquired speed");
+  }
+}
+
+void movingTarget() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  for (int i = 0; i < 3500; ++i) {
+    target[0] = kOrigin[0] + 0.2 * i * kDt;
+    shaper.step(target);
+    bounds.sample(shaper);
+  }
+  const double lag = target[0] - shaper.last_cmd_[0];
+  check(std::isfinite(lag) && lag >= 0 && lag < 0.06,
+        "continuous moving target retained excessive lag");
+  check(shaper.last_vel_[0] > 0.1, "continuous target did not produce forward speed");
+}
+
+void smallStep() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  target[0] += .01;
+  double overshoot = 0, tail_speed = 0;
+  for (int i = 0; i < 2500; ++i) {
+    shaper.step(target);
+    bounds.sample(shaper);
+    overshoot = std::max(overshoot, shaper.last_cmd_[0] - target[0]);
+    if (i >= 2000)
+      tail_speed = std::max(tail_speed, std::abs(shaper.last_vel_[0]));
+  }
+  check(overshoot <= .002, "10mm step exceeds 2mm overshoot gate");
+  check(std::abs(shaper.last_cmd_[0] - target[0]) < 1e-5 && tail_speed < 1e-4,
+        "10mm step failed to settle");
+}
+
+void movingStop() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  double overshoot = 0, tail_speed = 0;
+  for (int i = 0; i < 3500; ++i) {
+    if (i < 750)
+      target[0] += .2 * kDt;
+    shaper.step(target);
+    bounds.sample(shaper);
+    if (i >= 750)
+      overshoot = std::max(overshoot, shaper.last_cmd_[0] - target[0]);
+    if (i >= 3000)
+      tail_speed = std::max(tail_speed, std::abs(shaper.last_vel_[0]));
+  }
+  check(overshoot <= .003, "moving 0.2m/s stop exceeds 3mm overshoot gate");
+  check(std::abs(shaper.last_cmd_[0] - target[0]) < 1e-5 && tail_speed < 1e-4,
+        "moving target stop failed to settle");
+}
+
+void largeStepAndReversal() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  double excursion = 0, tail_speed = 0;
+  for (int i = 0; i < 7000; ++i) {
+    target[0] = kOrigin[0] + (i < 1500 ? .1 : i < 3000 ? -.1 : 0);
+    shaper.step(target);
+    bounds.sample(shaper);
+    excursion = std::max(excursion, std::abs(shaper.last_cmd_[0] - kOrigin[0]));
+    if (i >= 6500)
+      tail_speed = std::max(tail_speed, std::abs(shaper.last_vel_[0]));
+  }
+  check(excursion < .103, "100mm reversal exceeds 3mm overshoot gate");
+  check(std::abs(shaper.last_cmd_[0] - kOrigin[0]) < 1e-5 && tail_speed < 1e-4,
+        "100mm reversal failed to settle");
+}
+
+void crossingPi() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  vec6 start = kOrigin, target = kOrigin;
+  start[5] = M_PI - .03;
+  target[5] = -M_PI + .03;
+  shaper.sync(start);
+  BoundAudit bounds;
+  vec6 previous = start;
+  double angular_path = 0, tail_speed = 0;
+  for (int i = 0; i < 2500; ++i) {
+    shaper.step(target);
+    bounds.sample(shaper);
+    const double delta = poseAngle(shaper.last_cmd_, previous);
+    check(delta <= Args{}.cmd_vel_rot_ * kDt + 1e-8, "cross-pi physical rotation jumped");
+    angular_path += delta;
+    previous = shaper.last_cmd_;
+    if (i >= 2000)
+      tail_speed = std::max(tail_speed, norm3(shaper.last_vel_, 3));
+  }
+  check(angular_path < .09, "cross-pi followed a long rotation branch");
+  check(poseAngle(shaper.last_cmd_, target) < 1e-5 && tail_speed < 1e-4,
+        "cross-pi target failed to settle");
+}
+
+void multiAxisExactPi() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  const double u = 1 / std::sqrt(3.0);
+  for (int i = 0; i < 6500; ++i) {
+    const double t = std::min(i * kDt, 4.0);
+    target[0] = kOrigin[0] + .01 * std::sin(t * M_PI);
+    target[1] = kOrigin[1] + .01 * (std::cos(t * M_PI) - 1);
+    target[2] = kOrigin[2] + .005 * std::sin(t * M_PI / 2);
+    const double angle = M_PI * std::min(t / 4.0, 1.0);
+    for (int axis = 3; axis < 6; ++axis)
+      target[axis] = u * (i % 2 == 0 && angle > 1e-6 ? angle - 2 * M_PI : angle);
+    const vec6 old = shaper.last_cmd_;
+    shaper.step(target);
+    bounds.sample(shaper);
+    check(poseAngle(old, shaper.last_cmd_) <= Args{}.cmd_vel_rot_ * kDt + 1e-8,
+          "multi-axis exact-pi representation physically jumped");
+  }
+  check(poseAngle(shaper.last_cmd_, target) < 1e-5 && norm3(shaper.last_vel_, 3) < 1e-4,
+        "multi-axis exact-pi target failed to settle");
+}
+
+void nonCommutingRotation() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  BoundAudit bounds;
+  vec6 target = kOrigin;
+  vec6 previous = kOrigin;
+  for (int i = 0; i < 2500; ++i) {
+    const double t = i * kDt;
+    target[3] = .45 * std::sin(.73 * t);
+    target[4] = .35 * std::sin(1.11 * t + .4);
+    target[5] = .30 * std::sin(.91 * t - .7);
+    shaper.step(target);
+    bounds.sample(shaper);
+    check(poseAngle(previous, shaper.last_cmd_) <= Args{}.cmd_vel_rot_ * kDt + 1e-8,
+          "noncommuting rotation jumped between ticks");
+    previous = shaper.last_cmd_;
+  }
+  const vec6 final_target = target;
+  for (int i = 0; i < 2500; ++i) {
+    shaper.step(final_target);
+    bounds.sample(shaper);
+  }
+  check(poseAngle(shaper.last_cmd_, final_target) < 1e-5, "noncommuting rotation failed to settle");
+  check(norm3(shaper.last_vel_, 3) < 1e-4, "noncommuting rotation retained angular motion");
+}
+
+void intermediateBrakingPeak() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin, vec6{.02, 0, 0, 0, 0, 0});
+  // This is an intentionally difficult state: the target is close, but the
+  // current velocity and acceleration already carry an intermediate stopping
+  // peak. The envelope must inspect the whole trajectory, not just endpoint.
+  shaper.tracker_.in_.current_velocity[0] = .02;
+  shaper.tracker_.in_.current_acceleration[0] = -1.7;
+  shaper.last_vel_[0] = .02;
+  const double error = .0001;
+  const double proposed = kGain * error;
+
+  const double limited = shaper.stoppingLimitedGoal(0, proposed, error);
+  check(limited == 0, "intermediate brake crossing did not select bounded stop");
+}
+
+void invalidAndReset() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  shaper.sync(kOrigin);
+  vec6 invalid = kOrigin;
+  invalid[1] = std::numeric_limits<double>::quiet_NaN();
+  shaper.step(invalid);
+  check(shaper.errors_ > 0 && shaper.last_cmd_ == kOrigin,
+        "NaN target was accepted or changed command");
+  shaper.sync(kOrigin);
+  check(shaper.errors_ == 0, "explicit reset did not clear shaper failure");
+  for (int axis : {0, 3}) {
+    for (double value : {-1e308, 1e308}) {
+      shaper.sync(kOrigin);
+      vec6 huge = kOrigin;
+      huge[axis] = value;
+      shaper.step(huge);
+      check(shaper.errors_ > 0 && shaper.last_cmd_ == kOrigin, "huge finite pose was accepted");
+    }
+  }
+}
+
+void rotationIntegrationAgainstSubsteps() {
+  StreamingPoseShaper shaper(Args{}, kGain);
+  vec6 start = kOrigin;
+  start[3] = .3;
+  start[4] = -.4;
+  start[5] = .2;
+  shaper.sync(start);
+  BoundAudit bounds;
+  Quat integrated = rotvecToQuat(start[3], start[4], start[5]);
+  double max_error = 0;
+  constexpr int substeps = 64;
+  for (int i = 0; i < 1600; ++i) {
+    const double t = i * kDt;
+    vec6 target = start;
+    target[3] += .25 * std::sin(1.3 * t);
+    target[4] += .22 * std::sin(.9 * t);
+    target[5] += .3 * std::sin(1.7 * t);
+    // Preserve the pre-step state: the governor mutates current v/a.
+    auto input = shaper.tracker_.in_;
+    input.current_position = {};
+    shaper.step(target);
+    bounds.sample(shaper);
+    input.target_velocity = shaper.tracker_.in_.target_velocity;
+    ruckig::Trajectory<6> accepted;
+    check(shaper.tracker_.otg_.calculate(input, accepted) >= 0,
+          "independent accepted-trajectory reconstruction failed");
+    vec6 p{}, velocity{}, acceleration{};
+    for (int sub = 0; sub < substeps; ++sub) {
+      accepted.at_time(kDt * (sub + .5) / substeps, p, velocity, acceleration);
+      const double h = kDt / substeps;
+      integrated =
+          quatMul(rotvecToQuat(h * velocity[3], h * velocity[4], h * velocity[5]), integrated);
+    }
+    vec6 numerical = kOrigin;
+    quatToRotvec(integrated, numerical[3], numerical[4], numerical[5]);
+    max_error = std::max(max_error, poseAngle(numerical, shaper.last_cmd_));
+  }
+  check(max_error < 1e-5, "spatial quaternion integration disagrees with 64 midpoint substeps");
+  std::cout << "changing_axis_integrator_max_error_rad=" << max_error << '\n';
+}
+
+void productionLawUsesFixedGain() {
+  CartesianSpeedlLaw law(Args{});
+  std::string error;
+  check(law.validate(error), "production cartesian_speedl validation failed: " + error);
+  check(std::abs(law.shaper_.gain_ - kGain) < 1e-12,
+        "production Cartesian controller did not use the configured reference gain");
+  law.reset(kOrigin, {});
+  CartesianTick tick;
+  tick.reference_ = tick.actual_ = kOrigin;
+  tick.dt_ = kDt;
+  CartesianCommand command;
+  for (int i = 0; i < 1000; ++i) {
+    check(law.step(tick, command), "production cartesian_speedl stationary step failed");
+    check(command.fault_ == LoopFault::None && finite6(command.twist_),
+          "production cartesian_speedl produced invalid stationary output");
+  }
+}
+
+} // namespace
+
+int main() {
+  const struct {
+    const char *name;
+    void (*run)();
+  } tests[] = {{"stationary", stationary},
+               {"moving_target", movingTarget},
+               {"step_10mm", smallStep},
+               {"moving_stop", movingStop},
+               {"step_100mm_reversal", largeStepAndReversal},
+               {"cross_pi", crossingPi},
+               {"multi_axis_exact_pi", multiAxisExactPi},
+               {"noncommuting_rotation", nonCommutingRotation},
+               {"rotation_substeps", rotationIntegrationAgainstSubsteps},
+               {"intermediate_braking_peak", intermediateBrakingPeak},
+               {"invalid_reset", invalidAndReset},
+               {"production_fixed_gain", productionLawUsesFixedGain}};
+  unsigned failures = 0;
+  for (const auto &test : tests) {
+    try {
+      test.run();
+      std::cout << "PASS " << test.name << '\n';
+    } catch (const std::exception &error) {
+      ++failures;
+      std::cerr << "FAIL " << test.name << ": " << error.what() << '\n';
+    }
+  }
+  std::cout << "streaming_pose_unit failures=" << failures << '/'
+            << (sizeof(tests) / sizeof(tests[0])) << '\n';
+  return failures == 0 ? EXIT_SUCCESS : EXIT_FAILURE;
+}

roboapi/hardware/ur/native/tests/velocity_governor_unit.cpp

@@ -0,0 +1,65 @@
+// Ruckig trajectory regression without robot I/O.
+#include "ur_cpp/motion_primitives.hpp"
+
+#include <cassert>
+#include <limits>
+
+int main() {
+  constexpr double dt = .002;
+  TwistGovernor governor(dt, 1, 2.5, 25, 30, 2000, 2000);
+  governor.reset();
+  vec6 previous_acceleration{};
+  for (int n = 0; n < 3000; ++n) {
+    const double angle = n * .009;
+    vec6 target{4 * std::cos(angle), 4 * std::sin(angle), 1.0,
+                5 * std::sin(angle), 5 * std::cos(angle), 2.0};
+    assert(governor.step(target));
+    assert(norm3(governor.sent_) <= 1 + 1e-8);
+    assert(norm3(governor.sent_, 3) <= 2.5 + 1e-8);
+    for (int axis = 0; axis < 6; ++axis) {
+      assert(std::abs(governor.acceleration_[axis]) <=
+             (axis < 3 ? 25 : 30) / std::sqrt(3.0) + 1e-8);
+      assert(std::abs(governor.acceleration_[axis] - previous_acceleration[axis]) <=
+             2000 / std::sqrt(3.0) * dt + 1e-8);
+    }
+    previous_acceleration = governor.acceleration_;
+  }
+
+  // Inject a failed candidate calculation while a valid trajectory is moving.
+  // The accepted trajectory must advance, then accept a new target on recovery.
+  governor.reset();
+  for (int n = 0; n < 10; ++n)
+    assert(governor.step({.6, 0, 0, 0, 0, 0}));
+  const double before = governor.sent_[0];
+  const auto jerk = governor.in_.max_jerk;
+  governor.in_.max_jerk[0] = 0;
+  for (int n = 0; n < 10; ++n) {
+    assert(governor.step({-.2, 0, 0, 0, 0, 0}));
+    assert(governor.displacement_[0] > 0);
+  }
+  assert(governor.sent_[0] > before);
+  governor.in_.max_jerk = jerk;
+  for (int n = 0; n < 500; ++n)
+    assert(governor.step({-.2, 0, 0, 0, 0, 0}));
+  assert(std::abs(governor.sent_[0] + .2) < 1e-8);
+
+  // A long trajectory remains an ordinary shaping calculation.
+  TwistGovernor slow(dt, 1, 2.5, .003, .003, .06, .06);
+  slow.reset();
+  assert(slow.step({1, 0, 0, 0, 0, 0}));
+  assert(slow.sent_[0] > 0);
+  ruckig::Trajectory<6> trajectory;
+  assert(slow.otg_.calculate(slow.in_, trajectory) >= 0);
+  assert(trajectory.get_duration() > 30);
+
+  // Ruckig may correct initial acceleration after entering the mode.
+  governor.reset();
+  governor.in_.current_acceleration[0] = 20;
+  assert(governor.step({.5, 0, 0, 0, 0, 0}));
+  assert(governor.sent_[0] > 0);
+  assert(governor.acceleration_[0] < 20);
+
+  vec6 invalid{};
+  invalid[0] = std::numeric_limits<double>::quiet_NaN();
+  assert(!governor.step(invalid));
+}