#MoonMCL — MoonBit Motion Control Library

import {
  "moonbitstack/moonmcl" @mctl,
}

// A jerk-limited move over twelve metres.
let profile = @mctl.scurve(distance=12.0, v_max=2.0, a_max=1.0, j_max=4.0)
let end = profile.sample(profile.duration())
assert_eq((end.position - 12.0).abs() < 1.0e-6, true)
assert_eq(end.velocity.abs() < 1.0e-6, true)

moon add moonbitstack/moonmcl
moon run src/examples/01-pid

#The contracts

import {
  "moonbitstack/moonmcl" @mctl,
}

let plant = @mctl.Dynamics::new(
  f=fn (x, u, dt) {
    ignore(u)
    @mctl.Vec::of([x.get(0) + dt])
  },
  h=fn (x) { @mctl.Vec::of([x.get(0)]) },
)
let filter = @mctl.ExtendedKalman::new(
  model=plant,
  process=@mctl.Mat::of([[0.01]]),
  measurement=@mctl.Mat::of([[0.1]]),
  state=@mctl.Vec::of([0.0]),
  covariance=@mctl.Mat::of([[1.0]]),
)
filter.predict(@mctl.Vec::of([0.0]), 1.0)
filter.correct(@mctl.Vec::of([2.0]))
assert_eq(filter.state().len(), 1)

#Packages

#linear

#contract

#control

import {
  "moonbitstack/moonmcl" @mctl,
}

// Saturation, rate limiting and a cascade all take any controller.
let raw = @mctl.Pid::new(config=@mctl.pid_gains(kp=10.0))
let limited = @mctl.Saturating::new(inner=raw, output={ low: -1.0, high: 1.0 })
let command = limited.update(@mctl.Vec::of([1.0]), @mctl.Vec::of([0.0]), 0.1)
assert_eq(command.get(0), 1.0)

#estimation

#trajectory

import {
  "moonbitstack/moonmcl" @mctl,
}

let fast = @mctl.trapezoid(distance=1.0, v_max=1.0, a_max=1.0)
let far = @mctl.trapezoid(distance=4.0, v_max=1.0, a_max=1.0)
let set = @mctl.Synced::new(axes=[fast, far])
let ends = set.sample_all(set.duration())
assert_eq((ends[0].position - 1.0).abs() < 1.0e-9, true)

#planning

import {
  "moonbitstack/moonmcl" @mctl,
}

let car = @mctl.ReedsShepp::new(turning_radius=1.0)
let start = @mctl.Vec::of([0.0, 0.0, 0.0])
let goal = @mctl.Vec::of([-1.0, 0.4, 0.0])
let path = car.path(start~, goal~)
let end = path.at(path.length)
assert_eq((end.get(0) - -1.0).abs() < 1.0e-9, true)
assert_eq((end.get(1) - 0.4).abs() < 1.0e-9, true)
assert_eq(path.length < @mctl.Dubins::new(turning_radius=1.0).path(start~, goal~).length, true)

#kinematics

#Units and conventions

#Consistency

• The jerk-limited profile matches the reference durations to nine digits on every replayed move, with the start and end accelerations folded into the first and last ramp. A move that would have to reverse mid-way — from 3 m/s to 1 m/s over half a metre, say — is refused: this profile is written for motion along one direction, and the reference solves the reversing case with a different structure. The refusal is part of the contract and is tested as such. A boundary acceleration that points against the move is carried into the first ramp, so a profile can begin with a short stretch of negative velocity — a few centimetres per second across the swept domain — before the move itself starts; that is the state it was handed rather than a reversal of the move.
• Reeds-Shepp matches the reference length, the number of cusps and the pose along the path. Two words of exactly equal length meet at the same place, so which of the two is reported can differ from the reference; both are shortest. A second, separately written implementation agrees on the length to 1e-9 over twenty-four poses of its own.
• Dubins matches the reference length and the pose along the path for all six words, and a second implementation agrees on the length to 1e-9 over twenty-four poses of its own.
• The unscented filter is compared at 1e-7 rather than 1e-9: the reference takes the matrix square root by Cholesky and this library takes it by symmetric eigen decomposition, so the sigma points differ in the last digits. The linear and extended filters agree to 1e-9. A second implementation, which spreads its sigma points at a different scale as well, is compared at 5e-3: at a spread of 1e-3 the second-order term that separates the two vanishes, and at a spread of 1 it does not, so the loose tolerance is the one that compares the recursion rather than the tuning. A wrong weight is off by far more than that, which the mutation tests show. The linear filter and the backward pass are checked against the same second implementation at 1e-9, where nothing separates the two.
• The Riccati solution is compared at 1e-8: the reference solves the equation with a Schur decomposition and this library iterates the recursion to a fixed point.
• Sampling plans are compared by cost and feasibility, not point by point: two planners drawing from different generators find different routes of the same order of magnitude, and a route is judged by whether the map allows it.
• Arm kinematics are compared on the forward map exactly; the inverse is judged by where the solved angles put the tip, because a chain with more than two links has more than one set of angles for the same tip.
• Dense linear algebra matches the reference solve, inverse, determinant, Cholesky factor, symmetric eigenvalues and matrix exponential to 1e-9. A QR factorisation is unique only up to the sign of each column, so both sides orient the columns the same way before comparing, at 1e-10.
• The default numerical Jacobians are compared at 1e-8. The reference differentiates by complex step, which is accurate to machine precision; the contracts use a central difference with a step of 1e-6, so the difference is the truncation and cancellation error of that step.
• Discretisation matches the reference for all three rules to 1e-12 — the zero-order hold, the trapezoidal rule and forward Euler — on plants from a double integrator to a three-state chain and random matrices whose spectrum sits in the left half plane. The hold is the one that matters downstream, and it is the matrix exponential of the augmented plant, so the comparison covers both the augmentation and expm.
• The backward smoothing pass is replayed against the reference on its own forward record: the filtered means, covariances and transitions are the ones the reference held, and only the smoothed output is compared, at 1e-9. That separates a smoother that disagrees from a filter that does. A second implementation, given the same record, agrees at 1e-9 as well.

#Licence