use super::*;
struct Chain {
axes: Vec<V3>,
links: Vec<V3>,
tool_at: V3,
tool_dir: V3,
}
type M3 = [[f64; 3]; 3];
fn rot(axis: V3, deg: f64) -> M3 {
let a = unit(axis);
let (s, c) = deg.to_radians().sin_cos();
let t = 1.0 - c;
[
[
t * a[0] * a[0] + c,
t * a[0] * a[1] - s * a[2],
t * a[0] * a[2] + s * a[1],
],
[
t * a[0] * a[1] + s * a[2],
t * a[1] * a[1] + c,
t * a[1] * a[2] - s * a[0],
],
[
t * a[0] * a[2] - s * a[1],
t * a[1] * a[2] + s * a[0],
t * a[2] * a[2] + c,
],
]
}
fn mul(a: M3, b: M3) -> M3 {
let mut o = [[0.0; 3]; 3];
for (r, row) in o.iter_mut().enumerate() {
for (c, v) in row.iter_mut().enumerate() {
*v = (0..3).map(|k| a[r][k] * b[k][c]).sum();
}
}
o
}
fn apply(m: M3, v: V3) -> V3 {
[
m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2],
m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2],
m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2],
]
}
impl Chain {
fn tool(&self, q: &[f64]) -> (V3, V3) {
let mut r: M3 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let mut p: V3 = [0.0; 3];
for ((axis, &angle), link) in self.axes.iter().zip(q.iter()).zip(self.links.iter()) {
r = mul(r, rot(*axis, angle));
let d = apply(r, *link);
p = [p[0] + d[0], p[1] + d[1], p[2] + d[2]];
}
let t = apply(r, self.tool_at);
(
[p[0] + t[0], p[1] + t[1], p[2] + t[2]],
apply(r, self.tool_dir),
)
}
fn planar() -> Self {
Chain {
axes: vec![[0.0, 1.0, 0.0]; 3],
links: vec![[0.0, 0.0, 400.0], [0.0, 0.0, 300.0], [0.0, 0.0, 200.0]],
tool_at: [0.0; 3],
tool_dir: [0.0, 0.0, 1.0],
}
}
fn redundant() -> Self {
Chain {
axes: (0..7)
.map(|i| {
if i % 2 == 0 {
[0.0, 0.0, 1.0]
} else {
[0.0, 1.0, 0.0]
}
})
.collect(),
links: vec![[0.0, 0.0, 300.0]; 7],
tool_at: [0.0, 0.0, 150.0],
tool_dir: [0.0, 0.0, 1.0],
}
}
fn gimbal() -> Self {
Chain {
axes: vec![[0.0, 1.0, 0.0], [1.0, 0.0, 0.0]],
links: vec![[0.0; 3], [0.0; 3]],
tool_at: [0.0; 3],
tool_dir: [0.0, 0.0, 1.0],
}
}
}
const DOWN: V3 = [0.0, 0.0, -1.0];
#[test]
fn reaches_a_point_it_can_reach() {
let c = Chain::planar();
let s = Ik::default().solve(
&[10.0, 20.0, 30.0],
&Goal::Point([500.0, 0.0, 300.0]),
|q| c.tool(q),
);
assert_eq!(s.stop, Stop::Converged, "{s:?}");
assert!(s.position_err < 1e-3, "{s:?}");
}
#[test]
fn holds_the_tool_square_while_reaching() {
let c = Chain::planar();
let g = Goal::PointAlong {
at: [400.0, 0.0, 200.0],
along: DOWN,
};
let s = Ik::default().solve(&[10.0, 20.0, 30.0], &g, |q| c.tool(q));
assert_eq!(s.stop, Stop::Converged, "{s:?}");
assert!(s.within(1e-3, 1e-3), "{s:?}");
}
#[test]
fn an_infeasible_approach_reports_a_miss() {
let c = Chain::planar();
let at = [690.0, 0.0, 100.0];
let seed = [10.0, 20.0, 30.0];
let free = Ik::default().solve(&seed, &Goal::Point(at), |q| c.tool(q));
assert!(free.position_err < 1e-3, "reachable when free: {free:?}");
let lean = Ik::default().solve(&seed, &Goal::PointAlong { at, along: DOWN }, |q| c.tool(q));
let square = Ik {
rot_weight: 40_000.0,
..Default::default()
}
.solve(&seed, &Goal::PointAlong { at, along: DOWN }, |q| c.tool(q));
assert!(
lean.position_err > 10.0,
"must not claim it arrived: {lean:?}"
);
assert!(
square.axis_err < lean.axis_err,
"weight must tighten the axis"
);
assert!(square.axis_err < 0.01, "{square:?}");
assert!(
(square.position_err - 52.4).abs() < 0.5,
"expected the 52.4 shortfall, got {square:?}"
);
}
#[test]
fn escapes_an_axis_pointing_exactly_backwards() {
let g = Chain::gimbal();
let (_, a) = g.tool(&[0.0, 0.0]);
assert!(
dot(a, DOWN) < -0.999,
"the seed must be anti-parallel to be a test"
);
let cross = [
a[1] * DOWN[2] - a[2] * DOWN[1],
a[2] * DOWN[0] - a[0] * DOWN[2],
a[0] * DOWN[1] - a[1] * DOWN[0],
];
assert_eq!(norm(cross), 0.0);
assert!((norm([a[0] - DOWN[0], a[1] - DOWN[1], a[2] - DOWN[2]]) - 2.0).abs() < 1e-12);
let s = Ik::default().solve(
&[0.0, 0.0],
&Goal::PointAlong {
at: [0.0; 3],
along: DOWN,
},
|q| g.tool(q),
);
assert_eq!(s.stop, Stop::Converged, "{s:?}");
assert!(s.axis_err < 1e-3, "{s:?}");
}
#[test]
fn consecutive_solves_stay_on_one_branch() {
let c = Chain::redundant();
let ik = Ik::default();
let station = |x: f64| Goal::PointAlong {
at: [x, 0.0, 300.0],
along: DOWN,
};
let mut q = ik
.solve(
&[20.0, -40.0, 15.0, 50.0, -10.0, 30.0, 5.0],
&station(300.0),
|q| c.tool(q),
)
.joints;
let (mut worst_step, mut worst_pos, mut worst_axis) = (0.0f64, 0.0f64, 0.0f64);
for i in 1..=40 {
let s = ik.solve(&q, &station(300.0 + 500.0 * i as f64 / 40.0), |q| c.tool(q));
for (a, b) in q.iter().zip(&s.joints) {
worst_step = worst_step.max((a - b).abs());
}
worst_pos = worst_pos.max(s.position_err);
worst_axis = worst_axis.max(s.axis_err);
q = s.joints;
}
assert!(worst_pos < 1e-3, "worst position error {worst_pos}");
assert!(worst_axis < 1e-3, "worst axis error {worst_axis}");
assert!(
worst_step < 5.0,
"joint jumped {worst_step:.1} deg between stations"
);
}
#[test]
fn never_leaves_the_joint_limits() {
let c = Chain::redundant();
let ik = Ik {
limits: (-45.0, 45.0),
..Default::default()
};
let g = Goal::PointAlong {
at: [1800.0, 400.0, 200.0],
along: DOWN,
};
let s = ik.solve(&[0.0; 7], &g, |q| c.tool(q));
for (i, v) in s.joints.iter().enumerate() {
assert!(
(-45.0..=45.0).contains(v),
"joint {i} at {v} left its limits"
);
}
}
#[test]
fn admits_when_a_target_is_out_of_reach() {
let c = Chain::planar();
let s = Ik::default().solve(&[10.0, 20.0, 30.0], &Goal::Point([5000.0, 0.0, 0.0]), |q| {
c.tool(q)
});
assert_ne!(s.stop, Stop::Converged, "{s:?}");
assert!((s.position_err - 4100.0).abs() < 0.1, "{s:?}");
assert!((s.joints[0] - 90.0).abs() < 0.5, "{s:?}");
assert!(s.joints[1].abs() < 0.5 && s.joints[2].abs() < 0.5, "{s:?}");
}
#[test]
fn a_singular_seed_degrades_without_lying() {
let c = Chain::redundant();
let g = Goal::PointAlong {
at: [600.0, 0.0, 300.0],
along: DOWN,
};
let s = Ik {
max_iters: 200,
..Default::default()
}
.solve(&[0.0; 7], &g, |q| c.tool(q));
assert_ne!(s.stop, Stop::Converged, "{s:?}");
assert!(s.axis_err < 1.0, "should still turn the tool over: {s:?}");
assert!(s.position_err < 25.0, "{s:?}");
}
#[test]
fn a_point_goal_leaves_the_axis_unreported() {
let c = Chain::planar();
let s = Ik::default().solve(
&[10.0, 20.0, 30.0],
&Goal::Point([500.0, 0.0, 300.0]),
|q| c.tool(q),
);
assert_eq!(
s.axis_err, 0.0,
"an unconstrained axis has no error to report"
);
assert!(s.within(1e-3, 0.0));
}
#[test]
fn an_unnormalised_goal_axis_is_fine() {
let c = Chain::planar();
let seed = [10.0, 20.0, 30.0];
let at = [400.0, 0.0, 200.0];
let a = Ik::default().solve(
&seed,
&Goal::PointAlong {
at,
along: [0.0, 0.0, -37.5],
},
|q| c.tool(q),
);
let b = Ik::default().solve(&seed, &Goal::PointAlong { at, along: DOWN }, |q| c.tool(q));
for (x, y) in a.joints.iter().zip(&b.joints) {
assert!((x - y).abs() < 1e-9, "{a:?} vs {b:?}");
}
}