Add Rust Bindings

.github/workflows/lint-format.yml

@@ -128,3 +128,41 @@ jobs:
 
       - name: Run clang-tidy
         run: wpiformat -no-format -tidy-all -compile-commands=build -tidy-extra-args std=c++23
+
+  rustfmt:
+    name: "cargo fmt"
+    runs-on: ubuntu-24.04
+    defaults:
+      run:
+        working-directory: rust
+    steps:
+      - uses: actions/checkout@v6
+
+      - name: Install Rust toolchain
+        run: |
+          rustup update stable
+          rustup default stable
+          rustup component add rustfmt
+
+      - name: Run cargo fmt
+        run: cargo fmt --all -- --check
+
+      - name: Generate diff
+        run: |
+          cargo fmt --all
+          git diff HEAD > ../cargo-fmt-fixes.patch
+        if: ${{ failure() }}
+
+      - uses: actions/upload-artifact@v6
+        with:
+          name: cargo fmt fixes
+          path: cargo-fmt-fixes.patch
+        if: ${{ failure() }}
+
+      - name: Write to job summary
+        run: |
+          echo '```diff' >> $GITHUB_STEP_SUMMARY
+          cat ../cargo-fmt-fixes.patch >> $GITHUB_STEP_SUMMARY
+          echo '' >> $GITHUB_STEP_SUMMARY
+          echo '```' >> $GITHUB_STEP_SUMMARY
+        if: ${{ failure() }}

.github/workflows/rust.yml

@@ -0,0 +1,51 @@
+name: Rust
+
+on: [pull_request, push]
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  native:
+    timeout-minutes: 20
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - name: Windows x86_64
+            os: windows-2025
+            cmake-args:
+          - name: Windows aarch64
+            os: windows-11-arm
+            cmake-args:
+          - name: Linux x86_64
+            os: ubuntu-24.04
+            cmake-args:
+          - name: Linux aarch64
+            os: ubuntu-24.04-arm
+            cmake-args:
+          - name: macOS universal
+            os: macos-15
+            cmake-args:
+
+    name: ${{ matrix.name }}
+    runs-on: ${{ matrix.os }}
+    steps:
+      - uses: actions/checkout@v5
+
+      - name: Make GCC 14 the default toolchain (Linux)
+        if: runner.os == 'Linux'
+        run: |
+          sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 200
+          sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 200
+
+      - name: Set up sccache
+        uses: mozilla-actions/sccache-action@v0.0.9
+        # sccache doesn't work with MSBuild
+        if: runner.os != 'Windows'
+
+      - run: cargo build
+        env:
+          RUSTC_WRAPPER: sccache
+          SCCACHE_GHA_ENABLED: true

.gitignore

@@ -17,7 +17,7 @@ CMakeUserPresets.json
 compile_commands.json
 
 # clangd cache
-.clangd/
+.clangd
 .cache/
 
 # Python
@@ -30,3 +30,7 @@ __pycache__/
 dist/
 
 *.spy
+
+# Rust
+Cargo.lock
+target/

Cargo.toml

@@ -0,0 +1,3 @@
+[workspace]
+members = ["rust"]
+resolver = "2"

examples/constrained_multitag/main.rs

@@ -0,0 +1,119 @@
+//! Rust port of `examples/constrained_multitag/main.py`.
+//!
+//! Determines a robot pose from the corner pixel locations of several
+//! AprilTags. The robot pose is constrained to be on the floor (`z = 0`).
+
+use hafgufa::math::{cos, sin};
+use hafgufa::{Problem, VariableArena, solve};
+use ndarray::{Array2, array};
+
+fn main() {
+    // Camera calibration.
+    let fx = 600.0_f64;
+    let fy = 600.0_f64;
+    let cx = 300.0_f64;
+    let cy = 150.0_f64;
+
+    let arena = VariableArena::new();
+    let mut problem = Problem::new(&arena);
+
+    // Robot pose.
+    let robot_x = problem.decision_variable();
+    let robot_y = problem.decision_variable();
+    let robot_z = arena.constant(0.0);
+    let robot_theta = problem.decision_variable();
+
+    // Cache the trig variables so the expression graph shares the nodes.
+    let sin_theta = sin(robot_theta);
+    let cos_theta = cos(robot_theta);
+
+    // 4×4 field-to-robot homogeneous transform.
+    let zero = arena.constant(0.0);
+    let one = arena.constant(1.0);
+    let mut field2robot = arena.zeros(4, 4);
+    field2robot.set_variable(0, 0, cos_theta);
+    field2robot.set_variable(0, 1, -sin_theta);
+    field2robot.set_variable(0, 2, zero);
+    field2robot.set_variable(0, 3, robot_x);
+    field2robot.set_variable(1, 0, sin_theta);
+    field2robot.set_variable(1, 1, cos_theta);
+    field2robot.set_variable(1, 2, zero);
+    field2robot.set_variable(1, 3, robot_y);
+    field2robot.set_variable(2, 0, zero);
+    field2robot.set_variable(2, 1, zero);
+    field2robot.set_variable(2, 2, one);
+    field2robot.set_variable(2, 3, robot_z);
+    field2robot.set_variable(3, 0, zero);
+    field2robot.set_variable(3, 1, zero);
+    field2robot.set_variable(3, 2, zero);
+    field2robot.set_variable(3, 3, one);
+
+    // Robot frame is ENU, camera frame is SDE.
+    let robot2camera = arena.array(&array![
+        [0.0, 0.0, 1.0, 0.0],
+        [-1.0, 0.0, 0.0, 0.0],
+        [0.0, -1.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0],
+    ]);
+
+    let field2camera = &field2robot * &robot2camera;
+
+    // 4×1 (x, y, z, 1) points in the field frame.
+    let field2points = [
+        arena.array(&Array2::from_shape_vec((4, 1), vec![2.0, 0.0 - 0.08255, 0.4, 1.0]).unwrap()),
+        arena.array(&Array2::from_shape_vec((4, 1), vec![2.0, 0.0 + 0.08255, 0.4, 1.0]).unwrap()),
+    ];
+
+    // Hand-calibrated observations: the pixel locations we'd see for a
+    // camera located at (0, 0, 0).
+    let point_observations: [(f64, f64); 2] = [(325.0, 30.0), (275.0, 30.0)];
+
+    // Initial guess — we expect the solver to converge to (0, 0, 0).
+    robot_x.set_value(-0.1);
+    robot_y.set_value(0.0);
+    robot_theta.set_value(0.2);
+
+    // camera2field such that field2camera * camera2field = I.
+    let identity = arena.array(&Array2::eye(4));
+    let camera2field = solve(&field2camera, identity);
+
+    // Cost: sum of squared reprojection errors.
+    let mut cost = arena.constant(0.0);
+    for (field2point, (u_observed, v_observed)) in
+        field2points.iter().zip(point_observations.iter())
+    {
+        // camera2point = camera2field * field2point (4×1)
+        let camera2point = &camera2field * field2point;
+
+        let x = camera2point.get(0, 0);
+        let y = camera2point.get(1, 0);
+        let z = camera2point.get(2, 0);
+
+        println!("camera2point = {}, {}, {}", x.value(), y.value(), z.value());
+
+        let big_x = x / z;
+        let big_y = y / z;
+
+        let u = fx * big_x + cx;
+        let v = fy * big_y + cy;
+
+        println!("Expected u {}, saw {}", u.value(), u_observed);
+        println!("Expected v {}, saw {}", v.value(), v_observed);
+
+        let u_err = u - *u_observed;
+        let v_err = v - *v_observed;
+
+        cost = cost + u_err * u_err + v_err * v_err;
+    }
+
+    problem.minimize(cost);
+
+    match problem.solve(hafgufa::Options::default().diagnostics(true)) {
+        Ok(()) => println!("exit status: success"),
+        Err(e) => println!("exit status: {e}"),
+    }
+
+    println!("x = {} m", robot_x.value());
+    println!("y = {} m", robot_y.value());
+    println!("\u{3b8} = {} rad", robot_theta.value());
+}

examples/current_manager/main.rs

@@ -0,0 +1,83 @@
+//! Rust port of `examples/current_manager/main.py`.
+
+use hafgufa::{Problem, VariableArena, VariableMatrix, subject_to};
+
+struct CurrentManager<'a> {
+    problem: Problem<'a>,
+    desired_currents: VariableMatrix<'a>,
+    allocated_currents: VariableMatrix<'a>,
+}
+
+impl<'a> CurrentManager<'a> {
+    /// Builds the QP once. [`calculate`](Self::calculate) then reuses it
+    /// with new desired-current values on every call.
+    fn new(arena: &'a VariableArena, current_tolerances: &[f64], max_current: f64) -> Self {
+        let n = current_tolerances.len() as i32;
+
+        let mut problem = Problem::new(arena);
+
+        // Fresh 1x1 "constant" slots for desired currents; values get
+        // updated each tick via `set_value_at` so the cost expression
+        // built here keeps referencing them. Matches the Python binding's
+        // pattern of using a decision-variable-shaped matrix initialized
+        // to +inf so Sleipnir doesn't constant-fold during graph
+        // construction.
+        let mut desired_currents = arena.zeros(n, 1);
+        for i in 0..n {
+            desired_currents.set_scalar(i, 0, f64::INFINITY);
+        }
+
+        let allocated_currents = problem.decision_variable_matrix(n, 1);
+
+        // Cost: sum_i ((desired_i - allocated_i) / tol_i)^2.
+        let mut cost = arena.constant(0.0);
+        let mut current_sum = arena.constant(0.0);
+
+        for i in 0..n {
+            let desired = desired_currents.get(i, 0);
+            let allocated = allocated_currents.get(i, 0);
+            let error = desired - allocated;
+            let tol = current_tolerances[i as usize];
+            cost = cost + error * error / (tol * tol);
+
+            current_sum = current_sum + allocated;
+
+            subject_to!(problem, allocated >= 0.0);
+        }
+        problem.minimize(cost);
+
+        subject_to!(problem, current_sum <= max_current);
+
+        Self {
+            problem,
+            desired_currents,
+            allocated_currents,
+        }
+    }
+
+    fn calculate(&mut self, desired_currents: &[f64]) -> Vec<f64> {
+        let n = self.desired_currents.rows() as usize;
+        assert_eq!(n, desired_currents.len());
+
+        // Update the desired-current constant nodes in place so the cost
+        // expression built in `new()` still references them.
+        for (i, value) in desired_currents.iter().enumerate() {
+            self.desired_currents.set_value_at(i as i32, 0, *value);
+        }
+
+        self.problem
+            .solve(Default::default())
+            .expect("current-manager QP failed to solve");
+
+        (0..n)
+            .map(|i| self.allocated_currents.value_at(i as i32, 0).max(0.0))
+            .collect()
+    }
+}
+
+fn main() {
+    let arena = VariableArena::new();
+    let mut manager = CurrentManager::new(&arena, &[1.0, 5.0, 10.0, 5.0], 40.0);
+    let currents = manager.calculate(&[25.0, 10.0, 5.0, 0.0]);
+    println!("currents = {currents:?}");
+}

examples/flywheel_direct_transcription/main.rs

@@ -0,0 +1,52 @@
+//! Rust port of `examples/flywheel_direct_transcription/main.py`.
+
+use hafgufa::{Problem, VariableArena, subject_to};
+use ndarray::Array2;
+
+fn main() {
+    let total_time = 5.0_f64;
+    let dt = 0.005_f64;
+    let n: i32 = (total_time / dt) as i32;
+
+    // Flywheel model: states=[velocity], inputs=[voltage].
+    let a = (-dt).exp();
+    let b = 1.0 - a;
+
+    let arena = VariableArena::new();
+    let mut problem = Problem::new(&arena);
+    let x = problem.decision_variable_matrix(1, n + 1);
+    let u = problem.decision_variable_matrix(1, n);
+
+    // Dynamics constraint for every step. `a * x_curr + b * u_curr` goes
+    // through the IntoMatrixOperand-generic ops, so operands pass by value.
+    for k in 0..n {
+        let x_next = x.block(0, k + 1, 1, 1);
+        let x_curr = x.block(0, k, 1, 1);
+        let u_curr = u.block(0, k, 1, 1);
+        subject_to!(problem, x_next == a * x_curr + b * u_curr);
+    }
+
+    // Initial state is zero.
+    subject_to!(problem, x.block(0, 0, 1, 1) == 0.0);
+
+    // Input bounds. `u` is still a VariableMatrix (not Copy) so it's
+    // borrowed each time.
+    subject_to!(problem, &u >= -12.0);
+    subject_to!(problem, &u <= 12.0);
+
+    // Cost: track r = 10 at every state.
+    let r = arena.array(&Array2::from_elem((1, (n + 1) as usize), 10.0));
+    let diff = r - &x;
+    let cost = &diff * diff.t();
+    problem.minimize_matrix(&cost);
+
+    match problem.solve(Default::default()) {
+        Ok(()) => println!("exit status: success"),
+        Err(e) => println!("exit status: {e}"),
+    }
+
+    let mut x = x;
+    let mut u = u;
+    println!("x\u{2080} = {}", x.value_at(0, 0));
+    println!("u\u{2080} = {}", u.value_at(0, 0));
+}