Fixing speed commands with motors

autonomy/behaviour/joint_command/JointCommand.md

@@ -22,12 +22,27 @@ Repeat in order (skip rate/smooth steps on the first message after startup):
    $$
    q \leftarrow \mathrm{clip}(q,\ q_{\min},\ q_{\max}).
    $$
-2. **Velocity limit** — cap change per control tick using previous moderated target $q^{\mathrm{prev}}_i$:
-   $$
-   \Delta q_{\max} = \frac{\texttt{velocity\_max}}{\texttt{control\_rate\_hz}}.
-   $$
-3. **Delta limit** — additional per-step cap `delta_max` (degrees/tick).
-4. **Low-pass** — exponential smoothing with $\alpha =$ `low_pass_alpha`:
+2. **Velocity/ramp limit** — one of two modes, selected per joint by `enable_trapezoidal_limit`:
+   - **Plain clamp** (`enable_trapezoidal_limit: false`, default) — cap change per control tick
+     using previous moderated target $q^{\mathrm{prev}}_i$:
+     $$
+     \Delta q_{\max} = \frac{\texttt{velocity\_max}}{\texttt{control\_rate\_hz}}.
+     $$
+     **Known issue:** this clamp's output still passes through the low-pass step below, which
+     re-discounts it. A low-pass with $\alpha=0.85$ cuts *steady-state* speed to roughly
+     $(1-\alpha)$ of the clamped value — `velocity_max: 40` (deg/s) in practice produces
+     **~6 deg/s**, not 40, once the low-pass settles. Confirmed numerically; this is why
+     `enable_trapezoidal_limit` exists.
+   - **Trapezoidal ramp** (`enable_trapezoidal_limit: true`) — accelerate at `accel_max`
+     (deg/s²) toward `velocity_max`, then decelerate at `accel_max` to land exactly on target
+     with no overshoot, re-planned every tick (the target itself may still be moving, e.g. IK
+     or teleop chasing a live cube). This mode **replaces** the plain clamp and the low-pass
+     step for that joint — running both would just reintroduce the same discounting bug.
+     Not yet bench-tested on real hardware; keep off until validated on your own arm at a low
+     `accel_max`, then raise gradually.
+3. **Delta limit** — additional per-step cap `delta_max` (degrees/tick), applied in both modes.
+4. **Low-pass** — exponential smoothing with $\alpha =$ `low_pass_alpha`, **only applied when
+   `enable_trapezoidal_limit` is false**:
    $$
    q \leftarrow \alpha\, q^{\mathrm{prev}} + (1-\alpha)\, q.
    $$
@@ -37,7 +52,18 @@ Repeat in order (skip rate/smooth steps on the first message after startup):
    q_{\mathrm{motor}} = \texttt{direction} \cdot (q - \texttt{zero\_offset}).
    $$
 
-Store $q$ as $q^{\mathrm{prev}}$ for the next message.
+Store $q$ as $q^{\mathrm{prev}}$ (and, for the trapezoidal mode, its ramp velocity) for the next message.
+
+### Structural limitation (both modes)
+
+A continuously streaming target source (IK chasing a moving cube, VR teleop, a policy) feeds
+the moderator a target that's only slightly ahead of the arm's current position *every tick* —
+so the ramp/clamp may never reach `velocity_max` regardless of its value, since there's rarely
+enough distance-to-target to justify cruising. This is structural, not a moderator bug. The
+robust fix is to send velocity/accel as part of the target and let the motor's own firmware
+servo loop (CAN `POSITION_VELOCITY`, see `can_node.cpp`) handle the ramp instead of inferring
+speed from position deltas on the ROS side — see the CAN interfacing docs for the deg/s→ERPM
+conversion needed to use that control type.
 
 ## Config files
 
@@ -57,9 +83,11 @@ Start conservative on hardware, then increase until motion is responsive without
 
 | Parameter | Effect |
 |-----------|--------|
-| `velocity_max` | Max joint speed (converted to °/tick) |
+| `velocity_max` | Max joint speed — cruise speed in trapezoidal mode, plain clamp otherwise |
+| `accel_max` | Trapezoidal mode only: acceleration/deceleration rate (deg/s²) |
 | `delta_max` | Hard cap on ° change per tick |
-| `low_pass_alpha` | Higher → smoother/slower (e.g. `0.85`) |
+| `low_pass_alpha` | Higher → smoother/slower (e.g. `0.85`); ignored when trapezoidal mode is on |
+| `enable_trapezoidal_limit` | Switch from plain clamp+low-pass to the trapezoidal ramp (per-joint) |
 | `enable_*` | Toggle each stage without recompiling |
 
 ## Launch

autonomy/behaviour/joint_command/MOVE_ARM_RUNBOOK.md

@@ -93,6 +93,11 @@ docker exec watod_hy-jc-dry bash -c 'pkill -f "install/joint_command/lib"'  # ar
 ## Notes
 - **Speed / smoothing:** edit `config/safety_limits.yaml` (`velocity_max`, `low_pass_alpha`,
   the `enable_*` flags), then rebuild (Setup) + restart the node. Currently 40°/s, clamp off.
+  Note: with `enable_trapezoidal_limit: false` (default), the low-pass stage discounts
+  steady-state speed to roughly `(1-low_pass_alpha)` of `velocity_max` — 40°/s + alpha 0.85
+  behaves like ~6°/s in practice, not 40. `enable_trapezoidal_limit: true` (plus `accel_max`)
+  gives you the actual configured cruise speed, but has not been bench-tested yet — start with
+  a low `accel_max` and a clear arm the first time you flip it on.
 - **Didn't move?** `ros2 topic info /interfacing/motorCMD` shows `Publisher count: 0` → node
   lost discovery; restart it (safe — it re-seeds from live feedback). Discovery drops are
   mostly triggered by churning many short-lived `ros2 topic pub/echo/hz` processes on the host

autonomy/behaviour/joint_command/config/safety_limits.yaml

@@ -2,26 +2,23 @@ safety:
   units:
     angle: "deg"
     velocity: "deg_s"
-    # mit_kp/mit_kd: only used when joint_command.yaml's control_type is MIT_CONTROL (0).
-    # Per-motor units (kp in [0,500], kd in [0,5] for AK-series -- see
-    # can/config/mit_profiles.yaml). Unset/0 (the default below) is a deliberate safe no-op:
-    # zero stiffness/damping means the joint stays free even under MIT_CONTROL. Set explicitly
-    # per joint to enable compliant holding; start LOW (e.g. kp<10) and increase gradually.
+    # mit_kp/mit_kd: used only under MIT_CONTROL. Per-motor ranges (kp:[0,500], kd:[0,5] for
+    # AK-series, see can/config/mit_profiles.yaml). Default 0/0 = joint free even under MIT_CONTROL.
+    # Set explicitly per joint to hold; start low (kp<10) and increase gradually.
+
+    # Position clamp DISABLED for now -- re-enable once hardware_mapping.yaml limits are re-verified.
+    # velocity_max kept low deliberately for testing. Velocity limits should also be written to motor firmware
+    # enable_trapezoidal_limit (default false): reactive accel/cruise/decel ramp, replaces the
+    # clamp+low-pass stack above (which under-delivers steady-state speed). Untested on hardware.
 
-  # NOTE (test config): position clamp is DISABLED. Originally this was because
-  # hardware_mapping.yaml's limits were unmeasured placeholders (±90 etc); that's no
-  # longer true -- calibrate_arm.py's software zero-mode has now been run for real,
-  # producing real per-joint limits (see hardware_mapping.yaml). TODO: re-enable
-  # enable_position_clamp:true now that proper calibration is in place, and re-verify
-  # each joint's limits are current before flipping it (re-run calibration after any
-  # power cycle first -- see hardware_mapping.yaml's header note on encoder drift).
-  # velocity_max is still a deliberate slow crawl for testing; raise once trusted.
   global:
     enable_position_clamp: false
     enable_velocity_limit: true
     enable_delta_limit: true
     enable_low_pass: true
+    enable_trapezoidal_limit: false
     velocity_max: 40.0
+    accel_max: 60.0
     delta_max: 2.4
     low_pass_alpha: 0.85
     mit_kp: 0.0   # 0 = MIT_CONTROL is a safe no-op until set per-joint below

autonomy/behaviour/joint_command/include/joint_command_core.hpp

@@ -9,6 +9,7 @@
 #include "common_msgs/msg/motor_cmd.hpp"
 #include "yaml-cpp/yaml.h"
 
+// Static per-joint hardware facts (motor ID, direction, zero offset, limits).
 struct JointConfig {
   int8_t motor_id{0};
   double lower_limit{-180.0};
@@ -18,6 +19,7 @@
   bool limit_range{false};
 };
 
+// Tunable per-joint safety/moderation settings, reloadable from YAML.
 struct JointSafetyConfig {
   bool enable_position_clamp{true};
   bool enable_velocity_limit{true};
@@ -27,16 +29,23 @@
   double delta_max{2.0};       // degrees / control step
   double low_pass_alpha{0.85}; // q_out = alpha * q_prev + (1-alpha) * q_cmd
 
-  // MIT_CONTROL (compliant holding) gains. Only used when armPoseToMotorCmds is called with
-  // control_type=MIT_CONTROL; ignored for POSITION_LOOP etc. Default 0/0 is deliberately a
-  // safe no-op (zero stiffness/damping = motor free) -- a joint must be explicitly configured
-  // with nonzero mit_kp/mit_kd to actually hold under MIT. Units match the CubeMars AK-series
-  // manual's MIT protocol range for this motor (see can/config/mit_profiles.yaml): kp in
-  // [0,500], kd in [0,5] -- can_node clamps to the exact per-motor range before sending.
+  // Reactive trapezoidal ramp: accel_max -> cruise at velocity_max -> decelerate onto target,
+  // re-planned every tick since the target can keep moving (IK/teleop). Replaces the plain
+  // velocity clamp + low-pass when enabled (both under-deliver speed -- see JointCommand.md).
+  // delta_max still applies independently. Off by default: not yet bench-tested on hardware.
+
+  bool enable_trapezoidal_limit{false};
+  double accel_max{60.0}; // degrees / second^2
+
+  // MIT_CONTROL PD gains -- ignored for POSITION_LOOP etc. Default 0/0 = joint free until set
+  // per-joint. Ranges kp:[0,500], kd:[0,5] (AK-series, see can/config/mit_profiles.yaml);
+  // can_node clamps to exact per-motor range before sending.
+
   double mit_kp{0.0};
   double mit_kd{0.0};
 };
 
+// Public API: load hardware config, load safety config, run one moderation tick, seed from feedback.
 class JointCommandCore {
 public:
   bool loadFromYaml(const YAML::Node& config, const std::string& arm_side);
@@ -60,8 +69,9 @@
 
   size_t jointCount() const {
     return joints_.size();
   }
 
+// Private helpers (stateless math) + the mutable per-joint state vectors.
 private:
   static JointConfig loadJointConfig(const YAML::Node& joint_node);
   static JointSafetyConfig loadJointSafetyConfig(const YAML::Node& joint_node,
@@ -70,10 +80,13 @@
   static double applyCalibration(double angle, const JointConfig& joint);
   static double clampStep(double target, double previous, double delta_max);
   static double applyLowPass(double target, double previous, double alpha);
+  static double stepTrapezoidal(double target, double prev_pos, double& prev_vel,
+                                 double velocity_max, double accel_max, double dt);
 
   std::vector<JointConfig> joints_;
   std::vector<JointSafetyConfig> safety_;
   std::vector<double> prev_targets_;
+  std::vector<double> prev_velocities_; // degrees / second, one per joint, for the trapezoidal ramp
   bool have_prev_targets_{false};
   double control_rate_hz_{50.0};
 };

autonomy/behaviour/joint_command/src/joint_command_core.cpp

@@ -3,8 +3,10 @@
 #include <algorithm>
 #include <cmath>
 #include <map>
 #include <stdexcept>
 
+
+// Function made to parse one joint's YAML block into a JointConfig.
 JointConfig JointCommandCore::loadJointConfig(const YAML::Node& joint_node) {
   JointConfig joint;
   joint.motor_id = static_cast<int8_t>(joint_node["can_id"].as<int>());
@@ -16,6 +18,7 @@
   return joint;
 }
 
+// Loads all 6 joints for an arm side, resets safety/state to defaults.
 bool JointCommandCore::loadFromYaml(const YAML::Node& config, const std::string& arm_side) {
   joints_.clear();
 
@@ -49,10 +52,12 @@
   // Previously this started false, letting the first command bypass all rate limiting
   // and jump straight to its target -- visible as a sudden snap before smooth tracking.
   prev_targets_.assign(joints_.size(), 0.0);
+  prev_velocities_.assign(joints_.size(), 0.0);
   have_prev_targets_ = true;
   return true;
 }
 
+// Overwrites the assumed-0 start pose with real measured motor angles.
 size_t JointCommandCore::seedPrevTargetsFromFeedback(const std::map<int, double>& motor_positions) {
   std::vector<double> seeded(joints_.size(), 0.0);
   size_t matched = 0;
@@ -69,21 +74,28 @@
     ++matched;
   }
   prev_targets_ = std::move(seeded);
+  // Real feedback carries no velocity sample here, so the ramp starts from rest each time it's
+  // re-seeded (e.g. after a hand-move) -- safe, since starting from 0 can only under-accelerate,
+  // never overshoot.
+  prev_velocities_.assign(joints_.size(), 0.0);
   have_prev_targets_ = true;
   return matched;
 }
 
+// Hard-limit clip, and arm-frame<->motor-frame conversion.
 double JointCommandCore::clampAngle(double angle, const JointConfig& joint) {
   if (!joint.limit_range) {
     return angle;
   }
   return std::clamp(angle, joint.lower_limit, joint.upper_limit);
 }
 
+// Applies calibration lol
 double JointCommandCore::applyCalibration(double angle, const JointConfig& joint) {
   return static_cast<double>(joint.direction) * (angle - joint.zero_offset);
 }
 
+// Applies a joint's YAML overrides on top of a base/default config.
 JointSafetyConfig JointCommandCore::loadJointSafetyConfig(const YAML::Node& joint_node,
                                                           const JointSafetyConfig& base) {
   JointSafetyConfig cfg = base;
@@ -119,9 +131,16 @@
   if (joint_node["mit_kd"]) {
     cfg.mit_kd = joint_node["mit_kd"].as<double>();
   }
+  if (joint_node["enable_trapezoidal_limit"]) {
+    cfg.enable_trapezoidal_limit = joint_node["enable_trapezoidal_limit"].as<bool>();
+  }
+  if (joint_node["accel_max"]) {
+    cfg.accel_max = joint_node["accel_max"].as<double>();
+  }
   return cfg;
 }
 
+// Resolves global defaults then per-joint overrides for all 6 joints.
 bool JointCommandCore::loadSafetyFromYaml(const YAML::Node& safety_cfg, double control_rate_hz) {
   if (joints_.empty()) {
     return false;
@@ -147,6 +166,7 @@
   return true;
 }
 
+// Generic rate-limit-toward-target, and exponential smoothing.
 double JointCommandCore::clampStep(double target, double previous, double delta_max) {
   return previous + std::clamp(target - previous, -delta_max, delta_max);
 }
@@ -155,6 +175,42 @@
   return alpha * previous + (1.0 - alpha) * target;
 }
 
+// Accelerate/cruise/decelerate ramp onto a (possibly moving) target, no overshoot.
+double JointCommandCore::stepTrapezoidal(double target, double prev_pos, double& prev_vel,
+                                         double velocity_max, double accel_max, double dt) {
+  const double error = target - prev_pos;
+  const double direction = (error > 0.0) ? 1.0 : (error < 0.0 ? -1.0 : 0.0);
+  const double accel_step = accel_max * dt;
+
+  double desired_vel;
+  if (direction == 0.0 || (prev_vel != 0.0 && (prev_vel > 0.0) != (direction > 0.0))) {
+    // Already there, or still moving the wrong way (target reversed under us) -- brake to a
+    // stop before committing to the new direction, so we never yank straight through zero.
+    const double braked = std::abs(prev_vel) - accel_step;
+    desired_vel = (braked > 0.0) ? std::copysign(braked, prev_vel) : 0.0;
+  } else {
+    const double stopping_distance = (prev_vel * prev_vel) / (2.0 * accel_max);
+    if (std::abs(error) <= stopping_distance) {
+      // Close enough that cruising another tick would overshoot -- decelerate.
+      desired_vel = direction * std::max(0.0, std::abs(prev_vel) - accel_step);
+    } else {
+      // Free to speed up toward cruise.
+      desired_vel = direction * std::min(velocity_max, std::abs(prev_vel) + accel_step);
+    }
+  }
+
+  double step = desired_vel * dt;
+  if (std::abs(step) >= std::abs(error)) {
+    // Never overshoot within a single tick; land exactly on target and stop.
+    step = error;
+    desired_vel = 0.0;
+  }
+
+  prev_vel = desired_vel;
+  return prev_pos + step;
+}
+
+// Setup: validates message shape, flattens angles, defensive state resizing.
 std::vector<common_msgs::msg::MotorCmd>
 JointCommandCore::armPoseToMotorCmds(const common_msgs::msg::ArmPose& pose, int8_t control_type) {
   if (joints_.size() != 6) {
@@ -182,7 +238,11 @@
     prev_targets_.assign(joints_.size(), 0.0);
     have_prev_targets_ = true;
   }
+  if (prev_velocities_.size() != joints_.size()) {
+    prev_velocities_.assign(joints_.size(), 0.0);
+  }
 
+  // Per-joint moderation loop: clamp -> velocity-limit (trapezoidal or plain+low-pass) -> delta-limit -> clamp.
   for (size_t i = 0; i < joints_.size(); ++i) {
     const JointSafetyConfig& safety = safety_[i];
 
@@ -192,14 +252,21 @@
     }
 
     if (have_prev_targets_) {
-      if (safety.enable_velocity_limit && control_rate_hz_ > 0.0) {
+      if (safety.enable_trapezoidal_limit && control_rate_hz_ > 0.0) {
+        // Replaces the plain velocity clamp and low-pass below: both of those re-discount the
+        // ramp's own speed (a low-pass after a velocity clamp silently cuts steady-state speed
+        // by ~(1-alpha) -- see JointCommand.md), which is the bug this profile fixes.
+        target = stepTrapezoidal(target, prev_targets_[i], prev_velocities_[i],
+                                  std::abs(safety.velocity_max), std::abs(safety.accel_max),
+                                  1.0 / control_rate_hz_);
+      } else if (safety.enable_velocity_limit && control_rate_hz_ > 0.0) {
         const double velocity_step = std::abs(safety.velocity_max) / control_rate_hz_;
         target = clampStep(target, prev_targets_[i], velocity_step);
       }
       if (safety.enable_delta_limit) {
         target = clampStep(target, prev_targets_[i], std::abs(safety.delta_max));
       }
-      if (safety.enable_low_pass) {
+      if (!safety.enable_trapezoidal_limit && safety.enable_low_pass) {
         target = applyLowPass(target, prev_targets_[i], safety.low_pass_alpha);
       }
     }
@@ -209,6 +276,7 @@
     }
     next_targets[i] = target;
 
+    // Build MotorCmd: calibrate, branch MIT vs. position-loop encoding, save state for next tick.
     const double calibrated_deg = applyCalibration(target, joints_[i]);
 
     common_msgs::msg::MotorCmd cmd;

autonomy/interfacing/can/include/can_node.hpp

@@ -70,6 +70,16 @@ class CanNode : public rclcpp::Node {
   void loadMitProfiles();
   static uint32_t packMitValue(double phys, double min, double max, unsigned bits);
 
+  // POSITION_VELOCITY's PosVelSpeed/PosVelAccel are wire-encoded in ERPM (mechanical RPM *
+  // gear_ratio * pole_pairs), not deg/s -- see humanoid.dbc. Pole pairs (21) and gear ratio
+  // (9:1) below are from CubeMars's product pages, NOT the manual or a physical label --
+  // verify before trusting for real motion. mit_profiles_ doubles as the "validated AK-series
+  // motor" gate, so GL40/unknown models are refused rather than silently mis-scaled.
+
+  static constexpr double kAkSeriesPolePairs = 21.0;
+  static constexpr double kAkSeriesGearRatio = 9.0;
+  static double degPerSecToErpm(double deg_per_sec);
+
   // Subscribers and publishers
   std::unordered_map<std::string, rclcpp::SubscriptionBase::SharedPtr> _subscribers;