pub mod actuator;
pub mod chain;
pub mod design;
pub mod materials;
pub mod plant;
pub mod servo;
pub mod sim;
pub mod swivel;
pub mod transmission;
pub mod world;
#[cfg(test)]
pub(crate) mod tests_support;
pub use actuator::{Drive, DriveOutput, HydraulicDrive, TendonDrive};
pub use chain::{JointPose, PlanarArm, RevoluteJoint, SerialChain};
pub use design::{
assemble_joint, parts_flat, ActuatorDesign, DesignPart, GearMechanism, HydraulicMechanism,
PartRole, PartShape, TendonMechanism, PART_STRIDE,
};
pub use materials::{DriveEnv, HydraulicFluid, TendonMaterial};
pub use plant::ArmPlant;
pub use servo::{ServoJoint, ServoMode};
pub use sim::{ideal_envelope, IkServoSim, SimFrame, SimReport, Waypoint};
pub use swivel::{Bearing, Schedule, SchedulePoint, SwivelNozzle, ThrustState};
pub use transmission::{GearTrain, StribeckFriction};
pub use world::{ArmWorld, Contact, JointState, LinkBody, LinkMaterial, WorldObstacles};
pub(crate) fn wrap_deg(e: f64) -> f64 {
let mut x = e % 360.0;
if x > 180.0 {
x -= 360.0;
}
if x < -180.0 {
x += 360.0;
}
x
}
pub type V3 = [f64; 3];
pub(crate) fn dot(a: V3, b: V3) -> f64 {
a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
pub(crate) fn norm(v: V3) -> f64 {
dot(v, v).sqrt()
}
pub(crate) fn unit(v: V3) -> V3 {
let n = norm(v);
if n < 1e-12 {
return [0.0, 0.0, 1.0];
}
[v[0] / n, v[1] / n, v[2] / n]
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum Goal {
Point(V3),
PointAlong {
at: V3,
along: V3,
},
}
impl Goal {
pub fn at(&self) -> V3 {
match *self {
Goal::Point(p) => p,
Goal::PointAlong { at, .. } => at,
}
}
fn rows(&self) -> usize {
match self {
Goal::Point(_) => 3,
Goal::PointAlong { .. } => 6,
}
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Stop {
Converged,
Stalled,
Singular,
MaxIters,
}
#[derive(Debug, Clone)]
pub struct Solution {
pub joints: Vec<f64>,
pub position_err: f64,
pub axis_err: f64,
pub iters: usize,
pub stop: Stop,
}
impl Solution {
pub fn within(&self, pos: f64, axis: f64) -> bool {
self.position_err <= pos && self.axis_err <= axis
}
}
#[derive(Debug, Clone)]
pub struct Ik {
pub lambda: f64,
pub rot_weight: f64,
pub max_step: f64,
pub tol: f64,
pub max_iters: usize,
pub limits: (f64, f64),
pub joint_limits: Vec<(f64, f64)>,
}
impl Default for Ik {
fn default() -> Self {
Self {
lambda: 12.0,
rot_weight: 400.0,
max_step: 15.0,
tol: 1e-3,
max_iters: 60,
limits: (-180.0, 180.0),
joint_limits: Vec::new(),
}
}
}
impl Ik {
fn clamp_joint(&self, i: usize, q: f64) -> f64 {
let (lo, hi) = self
.joint_limits
.get(i)
.copied()
.unwrap_or(self.limits);
q.clamp(lo, hi)
}
pub fn solve_chain(&self, seed: &[f64], goal: &Goal, chain: &chain::SerialChain) -> Solution {
self.solve_inner(seed, goal, |q| chain.tool(q), Some(chain))
}
pub fn solve<F>(&self, seed: &[f64], goal: &Goal, tool: F) -> Solution
where
F: Fn(&[f64]) -> (V3, V3),
{
self.solve_inner(seed, goal, tool, None)
}
fn solve_inner<F>(
&self,
seed: &[f64],
goal: &Goal,
tool: F,
chain: Option<&chain::SerialChain>,
) -> Solution
where
F: Fn(&[f64]) -> (V3, V3),
{
let n = seed.len();
let rows = goal.rows();
let target = goal.at();
let along = match *goal {
Goal::PointAlong { along, .. } => unit(along),
Goal::Point(_) => [0.0, 0.0, 0.0],
};
let resid = |q: &[f64], out: &mut [f64]| {
let (p, a) = tool(q);
out[0] = p[0] - target[0];
out[1] = p[1] - target[1];
out[2] = p[2] - target[2];
if rows == 6 {
let a = unit(a);
for i in 0..3 {
out[3 + i] = self.rot_weight * (a[i] - along[i]);
}
}
};
let len = |e: &[f64]| e.iter().map(|v| v * v).sum::<f64>().sqrt();
let mut cur = seed.to_vec();
let mut e = vec![0.0; rows];
resid(&cur, &mut e);
let mut e2 = vec![0.0; rows];
let mut j = vec![0.0; rows * n]; let mut a = vec![0.0; rows * rows];
let mut y = vec![0.0; rows];
let mut probe = vec![0.0; n];
let mut next = vec![0.0; n];
let mut step = vec![0.0; n];
let mut stop = Stop::MaxIters;
let mut iters = 0;
let mut lambda = self.lambda;
let lambda_max = self.lambda * 1e8;
for it in 0..self.max_iters {
iters = it + 1;
if len(&e) < self.tol {
stop = Stop::Converged;
break;
}
if let Some(ch) = chain {
let (jv, jw) = ch.jacobian(&cur);
let (_, tool_axis) = tool(&cur);
let ua = unit(tool_axis);
for c in 0..n {
for r in 0..3 {
j[r * n + c] = jv[c][r];
}
if rows == 6 {
let d = chain::cross(jw[c], ua);
for k in 0..3 {
j[(3 + k) * n + c] = self.rot_weight * d[k];
}
}
}
} else {
const H: f64 = 1e-4; let h_deg = H.to_degrees();
for c in 0..n {
probe.copy_from_slice(&cur);
probe[c] += h_deg;
resid(&probe, &mut e2);
for r in 0..rows {
j[r * n + c] = (e2[r] - e[r]) / H;
}
}
}
let mut accepted = false;
let mut singular = false;
while lambda <= lambda_max {
for r in 0..rows {
for c in 0..rows {
a[r * rows + c] = (0..n).map(|k| j[r * n + k] * j[c * n + k]).sum();
}
a[r * rows + r] += lambda * lambda;
}
y.copy_from_slice(&e);
if !solve_in_place(&mut a, &mut y, rows) {
lambda *= 10.0;
singular = true;
continue;
}
singular = false;
let mut worst = 0.0f64;
for c in 0..n {
step[c] = -(0..rows)
.map(|r| j[r * n + c] * y[r])
.sum::<f64>()
.to_degrees();
worst = worst.max(step[c].abs());
}
let scale = if worst > self.max_step {
self.max_step / worst
} else {
1.0
};
for c in 0..n {
next[c] = self.clamp_joint(c, cur[c] + step[c] * scale);
}
resid(&next, &mut e2);
if len(&e2) < len(&e) {
accepted = true;
break;
}
lambda *= 10.0;
}
if !accepted {
stop = if singular {
Stop::Singular
} else {
Stop::Stalled
};
break;
}
cur.copy_from_slice(&next);
e.copy_from_slice(&e2);
lambda = (lambda / 3.0).max(self.lambda);
}
let (p, ax) = tool(&cur);
let position_err = norm([p[0] - target[0], p[1] - target[1], p[2] - target[2]]);
let axis_err = if rows == 6 {
dot(unit(ax), along).clamp(-1.0, 1.0).acos().to_degrees()
} else {
0.0
};
Solution {
joints: cur,
position_err,
axis_err,
iters,
stop,
}
}
}
fn solve_in_place(m: &mut [f64], y: &mut [f64], n: usize) -> bool {
for i in 0..n {
let mut piv = i;
for r in i + 1..n {
if m[r * n + i].abs() > m[piv * n + i].abs() {
piv = r;
}
}
if m[piv * n + i].abs() < 1e-12 {
return false;
}
if piv != i {
for c in 0..n {
m.swap(i * n + c, piv * n + c);
}
y.swap(i, piv);
}
let d = m[i * n + i];
for c in i..n {
m[i * n + c] /= d;
}
y[i] /= d;
for r in 0..n {
if r != i && m[r * n + i] != 0.0 {
let f = m[r * n + i];
for c in i..n {
m[r * n + c] -= f * m[i * n + c];
}
y[r] -= f * y[i];
}
}
}
true
}
#[cfg(test)]
mod tests;