roboapi 0.4.2__py3-none-any.whl

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

@@ -0,0 +1,170 @@
+#pragma once
+
+#include <algorithm>
+#include <string>
+
+#include "ur_cpp/motion_primitives.hpp"
+#include "ur_cpp/streaming_pose.hpp"
+
+struct CartesianTick {
+  vec6 reference_{};
+  vec6 actual_{};
+  vec6 wrench_{};
+  double dt_ = 0;
+};
+
+struct CartesianCommand {
+  vec6 desired_{};
+  vec6 twist_{};
+  LoopFault fault_ = LoopFault::None;
+};
+
+// Reference shaping, admittance, position feedback and final twist shaping.
+class CartesianSpeedlLaw {
+public:
+  Args args_;
+  AdmittanceLaw law_;
+  StreamingPoseShaper shaper_;
+  TwistGovernor governor_;
+
+  explicit CartesianSpeedlLaw(const Args &a)
+      : args_(a), law_(a.tun_), shaper_(a, a.pos_kp_),
+        governor_(streamingPosePeriod(a), a.cmd_vel_trans_, a.cmd_vel_rot_, a.speedl_acc_trans_,
+                  a.speedl_acc_rot_, a.speedl_jerk_trans_, a.speedl_jerk_rot_) {
+    law_.initNotch(a.freq_);
+  }
+
+  bool validate(std::string &error) const {
+    if (!std::isfinite(args_.freq_) || args_.freq_ < 50 || args_.freq_ > 500) {
+      error = "cartesian speed control frequency must be within [50, 500]";
+      return false;
+    }
+    for (double v :
+         {args_.tun_.zeta_, args_.tun_.filter_tau_, args_.tun_.notch_freq_,
+          args_.tun_.deadband_force_, args_.tun_.deadband_torque_, args_.tun_.max_dt_,
+          args_.tun_.max_vel_, args_.tun_.relaxation_hz_, args_.pos_kp_, args_.cmd_vel_trans_,
+          args_.cmd_vel_rot_, args_.cmd_acc_trans_, args_.cmd_acc_rot_, args_.speedl_acc_trans_,
+          args_.speedl_acc_rot_, args_.speedl_jerk_trans_, args_.speedl_jerk_rot_})
+      if (!std::isfinite(v) || v < 0) {
+        error = "a numeric cartesian speed control parameter is negative or non-finite";
+        return false;
+      }
+    if (args_.tun_.max_dt_ <= 0 || args_.cmd_vel_trans_ <= 0 || args_.cmd_vel_rot_ <= 0 ||
+        args_.cmd_acc_trans_ <= 0 || args_.cmd_acc_rot_ <= 0) {
+      error = "invalid velocity/acceleration or max_dt";
+      return false;
+    }
+    const double hmax = std::max(1 / args_.freq_, args_.tun_.max_dt_);
+    const double wc = 2 * M_PI * args_.tun_.relaxation_hz_;
+    if (!std::isfinite(wc)) {
+      error = "invalid relaxation_hz";
+      return false;
+    }
+    for (int i = 0; i < 6; ++i) {
+      const auto M = args_.tun_.M_[i], K = args_.tun_.K_[i], ratio = args_.tun_.hf_mass_ratio_[i];
+      if (!std::isfinite(M) || M <= 0 || !std::isfinite(K) || K < 0 || !std::isfinite(ratio) ||
+          ratio < 1) {
+        error = "invalid mass/stiffness/hf_mass_ratio";
+        return false;
+      }
+      const double damping = 2 * args_.tun_.zeta_ * std::sqrt(M) * std::sqrt(K);
+      if (!std::isfinite(damping)) {
+        error = "invalid damping";
+        return false;
+      }
+      if (ratio > 1) {
+        const double extra = (ratio - 1) * M;
+        const double d0 = damping - extra * wc;
+        if (wc <= 0 || !std::isfinite(M + extra) || !std::isfinite(extra * wc * wc) ||
+            !std::isfinite(d0) || d0 <= K * hmax / 2) {
+          error = "invalid Maxwell tuning: D0 must exceed K*hmax/2";
+          return false;
+        }
+      }
+    }
+    if (args_.speedl_acc_trans_ <= 0 || args_.speedl_acc_rot_ <= 0 ||
+        args_.speedl_jerk_trans_ <= 0 || args_.speedl_jerk_rot_ <= 0) {
+      error = "speedl acceleration and jerk limits must be positive";
+      return false;
+    }
+    return true;
+  }
+
+  void reset(const vec6 &actual, const vec6 &velocity) {
+    law_.reset();
+    shaper_.sync(actual);
+    governor_.reset(velocity);
+  }
+
+  void resetAdmittance() { law_.reset(); }
+
+  bool step(const CartesianTick &in, CartesianCommand &out, bool admittance = true) {
+    out = {};
+    if (!StreamingPoseShaper::plausiblePose(in.reference_) ||
+        !StreamingPoseShaper::plausiblePose(in.actual_) || !finite6(in.wrench_) ||
+        !std::isfinite(in.dt_) || in.dt_ <= 0 || std::abs(in.dt_ - 1.0 / args_.freq_) > 1e-9) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    const vec6 shaped = shaper_.step(in.reference_);
+    if (shaper_.errors_) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    if (admittance && !law_.predict(in.wrench_, in.dt_)) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    vec6 desired = shaped;
+    vec6 correction{}, raw_command{};
+    vec6 ref_velocity = shaper_.last_vel_;
+    // StreamingPoseShaper already returns spatial angular velocity.
+    const vec6 offset = admittance ? law_.off_ : vec6{};
+    const auto roff = rotvecToQuat(offset[3], offset[4], offset[5]);
+    ref_velocity = rotateAngular(roff, ref_velocity);
+    for (int i = 0; i < 3; ++i)
+      desired[i] += offset[i];
+    const auto rotation = quatMul(roff, rotvecToQuat(shaped[3], shaped[4], shaped[5]));
+    quatToRotvec(rotation, desired[3], desired[4], desired[5]);
+    const auto er =
+        quatMul(rotation, quatConj(rotvecToQuat(in.actual_[3], in.actual_[4], in.actual_[5])));
+    vec6 error{};
+    for (int i = 0; i < 3; ++i)
+      error[i] = desired[i] - in.actual_[i];
+    quatToRotvec(er, error[3], error[4], error[5]);
+    for (int i = 0; i < 6; ++i) {
+      const double cap = (i < 3 ? args_.cmd_vel_trans_ : args_.cmd_vel_rot_) * .25 / std::sqrt(3.);
+      const double c = std::clamp(args_.pos_kp_ * error[i], -cap, cap);
+      correction[i] = c;
+      raw_command[i] = ref_velocity[i] + (admittance ? law_.vel_[i] : 0.0) + correction[i];
+    }
+    if (!finite6(shaped) || !finite6(desired) || !finite6(correction) || !finite6(raw_command)) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    vec6 residual{};
+    if (!governor_.step(raw_command)) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    for (int i = 0; i < 6; ++i)
+      residual[i] = governor_.sent_[i] - raw_command[i];
+    if (admittance && !law_.commit(residual, in.dt_)) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    for (int i = 0; i < 3; ++i)
+      desired[i] = shaped[i] + (admittance ? law_.off_[i] : 0.0);
+    const auto accepted_offset = admittance ? law_.off_ : vec6{};
+    const auto accepted = quatMul(rotvecToQuat(accepted_offset[3], accepted_offset[4], accepted_offset[5]),
+                                  rotvecToQuat(shaped[3], shaped[4], shaped[5]));
+    quatToRotvec(accepted, desired[3], desired[4], desired[5]);
+    if (!finite6(desired) || !finite6(governor_.sent_)) {
+      out.fault_ = LoopFault::VirtualStateInvalid;
+      return false;
+    }
+    out.desired_ = desired;
+    out.twist_ = governor_.sent_;
+    return true;
+  }
+};

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

@@ -0,0 +1,126 @@
+#pragma once
+
+#include <array>
+#include <atomic>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <thread>
+
+using vec6 = std::array<double, 6>;
+
+struct Tuning {
+  double M_[6] = {4, 4, 4, .05, .05, .05};
+  double K_[6] = {50, 50, 50, 2.8, 2.8, 2.8};
+  double zeta_ = 3.5, filter_tau_ = .005, notch_freq_ = 0;
+  double deadband_force_ = 2.5, deadband_torque_ = .075, max_dt_ = .015;
+  double max_vel_ = .35;
+  double hf_mass_ratio_[6] = {1.5, 1.5, 1.5, 2, 2, 2};
+  double relaxation_hz_ = 5;
+};
+
+struct Args {
+  std::string robot_ip_;
+  std::string script_file_;
+  double freq_ = 500.0;
+  int rt_prio_ = 0;
+  double cmd_vel_trans_ = 1.0, cmd_vel_rot_ = 2.5;
+  double cmd_acc_trans_ = 3.0, cmd_acc_rot_ = 8.0;
+  double pos_kp_ = 15;
+  double speedl_acc_trans_ = 25, speedl_acc_rot_ = 30;
+  double speedl_jerk_trans_ = 2000, speedl_jerk_rot_ = 2000;
+  Tuning tun_;
+};
+
+enum class LoopFault : uint32_t {
+  None = 0,        // No fault
+  RobotSafetyStop, // Pendant protective/emergency stop; feedback failed and safety_mode is not Normal/Reduced
+  SampleDiscontinuity, // RTDE packet loss or robot_dt differs from the control period by more than 10%
+  RobotIoFailed,       // Failed speedl/keepalive write or lost data stream, excluding safety stops
+  VirtualStateInvalid, // Non-finite force/pose/dt or failed admittance calculation
+  ModeSwitchTimeout = 6, // No sustained stationary feedback after exiting the previous mode
+};
+
+inline const char *loopFaultName(LoopFault f) {
+  switch (f) {
+  case LoopFault::None:
+    return "";
+  case LoopFault::RobotSafetyStop:
+    return "robot_safety_stop";
+  case LoopFault::SampleDiscontinuity:
+    return "sample_discontinuity";
+  case LoopFault::RobotIoFailed:
+    return "robot_io_failed";
+  case LoopFault::VirtualStateInvalid:
+    return "virtual_state_invalid";
+  case LoopFault::ModeSwitchTimeout:
+    return "mode_switch_timeout";
+  }
+  return "loop_fault";
+}
+
+struct Snapshot {
+  vec6 q_{}, qd_{}, tcp_{}, tcp_speed_{}, wrench_{};
+  uint8_t mode_ = 0;
+  LoopFault fault_ = LoopFault::None;
+  int32_t safety_mode_ = 1;
+  uint64_t ticks_ = 0;
+  double dt_mean_ms_ = 0, dt_std_ms_ = 0, dt_p99_ms_ = 0, dt_max_ms_ = 0;
+  double rtde_hz_ = 0, host_hz_ = 0;
+  bool program_running_ = false;
+  bool loop_alive_ = false;
+  bool loop_ok_ = true;
+  std::string last_error_;
+};
+
+bool validateArgs(const Args &a, std::string &error);
+
+struct LoopCycle;
+
+// RTDE-paced URCL session. IPC commands set the latest intent; this thread
+// is the only writer of reverse-interface commands (keepalive / speedl).
+class ControlLoop {
+public:
+  explicit ControlLoop(Args args);
+  ~ControlLoop();
+
+  ControlLoop(const ControlLoop &) = delete;
+  ControlLoop &operator=(const ControlLoop &) = delete;
+
+  bool start();
+  void requestStop();
+  void join();
+
+  void setPoseRef(const vec6 &tcp, bool admittance = true);
+  void zeroFt();
+  void stopMotion();
+
+  Snapshot snapshot() const;
+  bool waitReady(double timeout_s);
+
+private:
+  friend struct LoopCycle;
+
+  enum class Kind : uint8_t { Idle, TrackPose, TrackPosition };
+
+  struct Intent {
+    Kind kind_ = Kind::Idle;
+    vec6 pose_{};
+    bool zero_ft_ = false;
+    bool stop_motion_ = false;
+  };
+
+  void threadMain();
+  void watchdogMain();
+  Intent takeIntent();
+
+  Args args_;
+  std::thread thread_;
+  std::atomic<bool> stop_{false};
+  std::atomic<bool> watchdog_quit_{false};
+  std::atomic<uint64_t> last_tick_ns_{0};
+  mutable std::mutex intent_mu_;
+  Intent intent_{};
+  mutable std::mutex snap_mu_;
+  Snapshot snap_{};
+};

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

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+#include "ur_cpp/control_loop.hpp"
+
+inline double norm3(const vec6 &v, int o = 0) { return std::hypot(v[o], v[o + 1], v[o + 2]); }
+
+inline bool finite6(const vec6 &v) {
+  return std::all_of(v.begin(), v.end(), [](double x) { return std::isfinite(x); });
+}
+
+struct Quat {
+  double w_, x_, y_, z_;
+};
+
+inline Quat rotvecToQuat(double rx, double ry, double rz) {
+  const double angle = std::sqrt(rx * rx + ry * ry + rz * rz);
+  if (angle < 1e-12)
+    return {1.0, 0.0, 0.0, 0.0};
+  const double s = std::sin(angle / 2.0) / angle;
+  return {std::cos(angle / 2.0), rx * s, ry * s, rz * s};
+}
+
+inline Quat quatMul(const Quat &a, const Quat &b) {
+  return {a.w_ * b.w_ - a.x_ * b.x_ - a.y_ * b.y_ - a.z_ * b.z_,
+          a.w_ * b.x_ + a.x_ * b.w_ + a.y_ * b.z_ - a.z_ * b.y_,
+          a.w_ * b.y_ - a.x_ * b.z_ + a.y_ * b.w_ + a.z_ * b.x_,
+          a.w_ * b.z_ + a.x_ * b.y_ - a.y_ * b.x_ + a.z_ * b.w_};
+}
+
+inline Quat quatConj(const Quat &q) { return {q.w_, -q.x_, -q.y_, -q.z_}; }
+
+inline void quatToRotvec(Quat q, double &rx, double &ry, double &rz,
+                         const vec6 *previous = nullptr) {
+  const double n = std::hypot(std::hypot(q.w_, q.x_), std::hypot(q.y_, q.z_));
+  if (!std::isfinite(n) || n < 1e-15) {
+    rx = ry = rz = std::numeric_limits<double>::quiet_NaN();
+    return;
+  }
+  q.w_ /= n, q.x_ /= n, q.y_ /= n, q.z_ /= n;
+  if (std::abs(q.w_) <= 1e-12) {
+    double choice = 0;
+    if (previous)
+      choice = q.x_ * (*previous)[3] + q.y_ * (*previous)[4] + q.z_ * (*previous)[5];
+    if (!std::isfinite(choice) || std::abs(choice) < 1e-12) {
+      choice = q.x_;
+      if (std::abs(q.y_) > std::abs(choice))
+        choice = q.y_;
+      if (std::abs(q.z_) > std::abs(choice))
+        choice = q.z_;
+    }
+    if (choice < 0) {
+      q.x_ = -q.x_;
+      q.y_ = -q.y_;
+      q.z_ = -q.z_;
+    }
+    q.w_ = 0;
+  } else if (q.w_ < 0)
+    q.w_ = -q.w_, q.x_ = -q.x_, q.y_ = -q.y_, q.z_ = -q.z_;
+  const double sin_half = std::sqrt(q.x_ * q.x_ + q.y_ * q.y_ + q.z_ * q.z_);
+  if (sin_half < 1e-12) {
+    rx = ry = rz = 0.0;
+    return;
+  }
+  const double angle = 2.0 * std::atan2(sin_half, q.w_);
+  rx = q.x_ / sin_half * angle;
+  ry = q.y_ / sin_half * angle;
+  rz = q.z_ / sin_half * angle;
+}