Maps between wheel motion and body motion for a differential drive, pose integration on SE(2), and
robots built from joints. Fixed-size, no allocation, no panics, and generic over the Numeric
scalar.
The body motion is deliberately 2-DOF, not 3. A differential drive has exactly two degrees of
freedom (v, ω) and exactly two wheels, so the map between them is a bijection and both round trips
are exact identities. There is no lateral term to silently drop.
DifferentialDrive: the geometry, a wheel radius and a track width. Constructing it is the only fallible operation in the module; with the geometry checked once, every map below is total.WheelVelocities/BodyTwist: motion per second, related byforwardandinverse. ABodyTwistis the se(2) twist a differential drive can realise, with the lateral term dropped.WheelRotations/BodyArc: motion over one tick, related byforward_arcandinverse_arc.WheelRotationsis what an encoder reports; aBodyArcis arc length and heading change, the exponential coordinates of the relative pose.integrate: advances anSE2pose along the exact constant-twist arc.Unicycle: the same plant as an ODE right-hand side, forRk4/Rk45.OdometryStep: the process model as aVectorFn, for autodiff Jacobians.
use multicalc::kinematics::integrate;
use multicalc::{BodyTwist, DifferentialDrive, WheelVelocities};
use multicalc::Dual;
use multicalc::SE2;
let wheel_radius = 0.036_f64; // 36 mm
let track_width = 0.235; // 235 mm between the wheels
let drive = DifferentialDrive::new(wheel_radius, track_width).unwrap();
// Wheel velocities to a body twist, and back exactly.
let wheel_speeds = WheelVelocities::new(10.0, 10.0); // rad/s on each wheel
let twist = drive.forward(wheel_speeds); // v = 0.36 m/s, ω = 0
let body_motion = BodyTwist::new(0.36, 0.0); // m/s forward, rad/s turn
let wheels = drive.inverse(body_motion); // back to (10, 10)
// The encoder path: distance travelled -> wheel rotation -> body arc -> pose.
let left_travel = 0.01; // metres rolled by each wheel
let right_travel = 0.012;
let rotations = drive.wheel_rotations_from_travel(left_travel, right_travel);
let start = SE2::identity();
let pose = integrate(start, drive.forward_arc(rotations));
// Autodiff straight through an odometry step: d(pose)/d(arc length).
let arc_length = Dual::variable(0.4); // the quantity being differentiated
let turn_rate = Dual::constant(0.3);
let duration = Dual::constant(1.0);
let step = integrate(
SE2::<Dual<f64>>::identity(),
BodyTwist::new(arc_length, turn_rate).integrate_over(duration),
);
let dx_ds = step.translation()[0].deriv;Because integrate is built on SE2::exp, a straight line (ω = 0) is handled by the same code path
as an arc, with no 1/ω to blow up: the value and its derivative stay finite at exactly zero
curvature. The arc is exact for a constant twist at any step size, so the modelling error is the
zero-order hold on the wheel velocities rather than integration error.
Full demo: kinematics.rs.
A KinematicTree is a jointed robot model: joints in topological order, each attached to the world
or to an earlier joint. Storage is a fixed array of MAX_JOINTS joints plus a runtime length, so the
model is Copy and needs no heap.
Joint: one single-degree-of-freedom joint.Joint::revolute,Joint::prismaticandJoint::fixedbuild one; thewith_*methods set the anchor it rotates about, the reference configuration, the travel limits, and the armature, damping and friction loss a later dynamics pass reads;Joint::continuousbuilds one with no travel limit, wrapping past ±π instead of stopping.JointParent: what a joint is attached to, eitherWorldor an earlier joint by index.KinematicTree: the model.try_from_jointsandpushare the only fallible calls; with the model validated once, every query afterwards is total.KinematicTreeState: the world pose of every joint frame for one configuration, returned byforward_kinematicsand indexed by joint.KinematicJacobian: how each joint's rate moves a chosen frame — six rows, three of straight-line motion then three of turning, one column per joint.geometric_jacobian_atbuilds one from a configuration;geometric_jacobianreuses poses you already solved for.InverseKinematics: the other direction — the joint readings that put a chosen frame at a pose you name. Steps toward the target, damping the step near a pose where the arm loses a direction of motion, keeping every reading inside its travel.SecondaryObjective: what an arm with joints to spare should do with the freedom the task leaves it — hold a comfortable posture, or stay off its limits.
Every joint takes at least one configuration slot, welds included — joint index and configuration index agree for every joint except a floating one, which takes seven.
use multicalc::kinematics::{Joint, JointParent, KinematicTree};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::{SE3, SO3};
// A planar two-link arm: both joints rotate about z, each link reaches one unit along x, and a
// weld carries the tool frame at the far end.
let about_z = Vector::new([0.0, 0.0, 1.0]);
let along_x = SE3::from_parts(SO3::identity(), Vector::new([1.0, 0.0, 0.0]));
let tree = KinematicTree::<3, 3, f64>::try_from_joints(
&[
Joint::revolute(about_z, SE3::identity()),
Joint::revolute(about_z, along_x),
Joint::fixed(along_x),
],
&[
JointParent::World,
JointParent::Joint(0),
JointParent::Joint(1),
],
)
.unwrap();
// Shoulder at +90 degrees, elbow straight.
let configuration = Vector::new([core::f64::consts::FRAC_PI_2, 0.0, 0.0]);
let state = tree.forward_kinematics(&configuration).unwrap();
let tool = state.pose(2).unwrap().translation();
assert!(tool[0].abs() < 1e-12);
assert!((tool[1] - 2.0).abs() < 1e-12);Readings are counted from each joint's reference configuration (with_zero_offset, MuJoCo's ref),
so an encoder zero that differs from the model zero is handled by the model rather than by a constant
buried in whatever reads it.
The whole path is generic over the scalar, so pushing a Dual through it gives the exact derivative
of any frame's pose with respect to any joint reading, with nothing hand-derived.
Full demo: forward_kinematics.rs.
A revolute joint with no stated limit is merely never clamped. A continuous joint states the
unboundedness: it can carry no limit at all, and its reading is treated as periodic wherever
configuration distance is measured — shortest arc about ±π rather than the raw difference. Wheels,
turrets and spinning wrist rolls; multicalc-robot-model maps an MJCF hinge with no resolved
range to one, and URDF names a continuous joint outright.
use multicalc::kinematics::{Joint, JointParent, KinematicTree};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::SE3;
let about_z = Vector::new([0.0, 0.0, 1.0]);
let tree = KinematicTree::<1, 1, f64>::try_from_joints(
&[Joint::continuous(about_z, SE3::identity())],
&[JointParent::World],
)
.unwrap();
// Readings are not folded back: 7 rad is 7 rad.
let state = tree.forward_kinematics(&Vector::new([7.0])).unwrap();
assert!((state.pose(0).unwrap().rotation().log()[2] - (7.0 - 2.0 * core::f64::consts::PI)).abs() < 1e-12);
// 3 rad to -3 rad is 2*pi - 6 the short way, not 6.
let distance = tree.configuration_distance(&Vector::new([3.0]), &Vector::new([-3.0]));
assert!((distance - (2.0 * core::f64::consts::PI - 6.0)).abs() < 1e-12);configuration_distance applies that per joint kind across the whole model: plain difference at a
revolute or prismatic joint, shortest arc at a continuous one, translation plus rotation log at a
floating one, nothing at a weld. SecondaryObjective::PreferredPosture uses the same shortest-arc
error, so posture bias at a continuous joint drives the short way round.
A body free to move in every direction — MJCF's <freejoint>, URDF's floating — is a joint too,
just a wider one: Joint::floating takes no axis or anchor, because none applies. Its reading is
seven numbers, a position and a unit quaternion, rather than the usual one, and its rate is six
rather than one — see FreeJointState. A floating joint may only be a tree's first joint, since it
describes a body's connection to the world rather than to another joint.
KinematicTree carries a second capacity for this: KinematicTree<MAX_JOINTS, MAX_CONFIG, T>,
where MAX_CONFIG bounds the configuration vector rather than the joint count. A tree with no
floating joint sets both to the same number; one with a floating base sets MAX_CONFIG six higher.
use multicalc::kinematics::{Joint, JointParent, KinematicTree};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::SE3;
// One joint, one slot, seven configuration numbers.
let tree = KinematicTree::<1, 7, f64>::try_from_joints(
&[Joint::floating(SE3::identity())],
&[JointParent::World],
)
.unwrap();
// Position first, then a scalar-first quaternion: at rest, facing the world axes.
let state = tree
.forward_kinematics(&Vector::new([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
.unwrap();
assert!(state.pose(0).unwrap().translation().norm() < 1e-12);Building a model joint by joint is fine for a two-link arm. For a real robot, read it out of the
file the manufacturer's model ships as. multicalc-robot-model reads both MuJoCo MJCF — the format
the MuJoCo Menagerie models use — and URDF, the one most ROS robots ship, into the same types.
load_path picks the reader from the file's extension.
use multicalc_robot_model::load_path;
let model = load_path(std::path::Path::new("third_party/menagerie/franka_emika_panda/panda.xml"))?;
// Eleven bodies: seven turning joints, two sliding fingers, and two welds.
assert_eq!(model.body_count(), 11);
// The arm alone: the chain running from the world down to the hand.
let arm = model.kinematic_tree_to::<9, 9>("hand")?;
let state = arm.forward_kinematics(&Vector::zeros())?;The same robot as a URDF reads into the same types, with two differences worth knowing. URDF states
a link's mass outright or not at all, so inertia() gives None for a link that carries none — the
MoveIt Panda states none anywhere, being a kinematics-only description. And URDF can couple one
joint to another, which a tree of independent joints cannot express, so a model with a coupled
gripper gives its arm through kinematic_tree_to rather than the whole tree.
let model = load_path(std::path::Path::new("third_party/moveit_resources_panda/panda.urdf"))?;
// Twelve bodies here: the URDF carries a bare frame between the last arm link and the hand.
assert_eq!(model.body_count(), 12);
assert!(model.bodies().iter().all(|body| body.inertia().is_none()));
// The second finger follows the first, so take the chain to the hand.
let arm = model.kinematic_tree_to::<10, 10>("panda_hand")?;(multicalc-robot-model is not a dependency of multicalc itself — see its own
README
for a demo that actually compiles.)
The travel limits, the reference reading, and the armature, damping and friction figures come across
with the model, so a solver holds each joint inside the travel the file states and a later dynamics
pass has the numbers it needs. What the reader passed over is listed in model.ignored(), and
anything that could change a mass is refused rather than ignored — see the
crate README
for the part of each format it reads.
CollisionQuery measures sphere and capsule clearance: self-collision between primitives attached
to the robot's own frames, and environment collision against world-fixed ones. Distances are
surface-to-surface, negative on overlap. Every pair is checked — no broad phase — so capacities are
sized for a primitive or two per link rather than mesh geometry, and adjacent-link pairs that
always touch are dropped with exclude_pair.
use multicalc::kinematics::{CollisionQuery, Joint, JointParent, KinematicTree, Primitive};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::{SE3, SO3};
let about_z = Vector::new([0.0, 0.0, 1.0]);
let tree = KinematicTree::<1, 1, f64>::try_from_joints(
&[Joint::revolute(about_z, SE3::identity())],
&[JointParent::World],
)
.unwrap();
// A 0.1 m ball carried 1 m out on the link, and an obstacle 1 m out along y.
let mut query = CollisionQuery::<1, 1, 0, f64>::new();
let out_along_x = SE3::from_parts(SO3::identity(), Vector::new([1.0, 0.0, 0.0]));
let obstacle_pose = SE3::from_parts(SO3::identity(), Vector::new([0.0, 1.0, 0.0]));
query
.push_self_primitive(0, Primitive::Sphere { radius: 0.1 }, out_along_x)
.unwrap();
query
.push_environment_primitive(Primitive::Sphere { radius: 0.1 }, obstacle_pose)
.unwrap();
// A quarter turn apart: sqrt(2) m between centres, less both radii.
let state = tree.forward_kinematics(&Vector::zeros()).unwrap();
let report = query.check(&state).unwrap();
assert!((report.minimum_clearance - (2.0_f64.sqrt() - 0.2)).abs() < 1e-12);
assert!(report.is_clear(0.05));
// Turned onto the obstacle: the two coincide, so the clearance goes negative.
let turned = tree
.forward_kinematics(&Vector::new([core::f64::consts::FRAC_PI_2]))
.unwrap();
assert!(!query.check(&turned).unwrap().is_clear(0.0));Primitive is posed by an SE3 alongside it; a capsule's segment runs along its pose's local z,
matching MJCF's convention. sphere_sphere_distance, capsule_capsule_distance and
sphere_capsule_distance are public if you want the closed forms without the query around them.
Used as an IK solution filter, this composes rather than hooking into the solver: walk a
MultiStartReport's solutions(), run forward_kinematics on each, and keep the first whose
check comes back is_clear.
use multicalc::kinematics::{
InverseKinematics, InverseKinematicsTermination, Joint, JointParent, KinematicTree,
};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::{SE3, SO3};
// The same planar two-link arm, with each hinge limited to a half turn either way.
let about_z = Vector::new([0.0, 0.0, 1.0]);
let along_x = SE3::from_parts(SO3::identity(), Vector::new([1.0, 0.0, 0.0]));
let hinge = |origin| Joint::revolute(about_z, origin).with_limits(-3.14, 3.14);
let tree = KinematicTree::<3, 3, f64>::try_from_joints(
&[hinge(SE3::identity()), hinge(along_x), Joint::fixed(along_x)],
&[
JointParent::World,
JointParent::Joint(0),
JointParent::Joint(1),
],
)
.unwrap();
// Put the tool at (1, 1): reachable, with the elbow at a right angle.
let target = SE3::from_parts(SO3::identity(), Vector::new([1.0, 1.0, 0.0]));
let seed = Vector::new([0.3, 0.3, 0.0]);
let solver = InverseKinematics::<3, f64>::new();
let report = solver.solve(&tree, 2, target, &seed).unwrap();
assert_eq!(report.termination, InverseKinematicsTermination::Converged);
assert!(report.position_error < 1e-6);
// The answer is not unique — which of the two elbow poses you land on depends on the guess you
// started from. Check the pose it reached, not the readings it chose.
let reached = tree.forward_kinematics(&report.joint_positions).unwrap();
let tool = reached.pose(2).unwrap().translation();
assert!((tool[0] - 1.0).abs() < 1e-6 && (tool[1] - 1.0).abs() < 1e-6);An arm with more joints than the task needs can move without disturbing the frame it is holding.
with_secondary_objective says what to do with that freedom — PreferredPosture drifts toward a
set of readings you like, JointLimitMargin drifts toward the middle of each joint's travel — and
the task is unaffected either way.
Full demo: 3d_arm_ik.rs.
One DLS solve converges to whichever branch its seed falls into. MultiStartInverseKinematics
runs up to MAX_STARTS solves and keeps the distinct converged configurations, deduplicated by
configuration_distance against a threshold you set. Seeds come from an array you supply, or from
solve_seeded, which runs the base seed unperturbed and then jitters draws from any
RandomSource — uniform across a joint's range where it has limits, base ± jitter_span where it
does not.
use multicalc::kinematics::{Joint, JointParent, KinematicTree, MultiStartInverseKinematics};
use multicalc::linear_algebra::Vector;
use multicalc::random::Pcg32;
use multicalc::spatial::{SE3, SO3};
// Six hinges alternating x/y on 0.25 m links, tool welded 0.25 m past the last: 6 DOF against a
// 6-DOF task, so its solutions are discrete branches.
let about_x = Vector::new([1.0, 0.0, 0.0]);
let about_y = Vector::new([0.0, 1.0, 0.0]);
let link = SE3::from_parts(SO3::identity(), Vector::new([0.0, 0.0, 0.25]));
let mut tree = KinematicTree::<7, 7, f64>::new();
for index in 0..6 {
let axis = if index % 2 == 0 { about_x } else { about_y };
let origin = if index == 0 { SE3::identity() } else { link };
let parent = if index == 0 {
JointParent::World
} else {
JointParent::Joint(index - 1)
};
tree.push(Joint::revolute(axis, origin), parent).unwrap();
}
tree.push(Joint::fixed(link), JointParent::Joint(5)).unwrap();
let posture = Vector::new([0.3, 0.6, -0.4, 0.9, 0.2, -0.5, 0.0]);
let target = tree.forward_kinematics(&posture).unwrap().pose(6).unwrap();
let solver = MultiStartInverseKinematics::<8, 7, f64>::new();
let mut source = Pcg32::<f64>::new(11);
let report = solver
.solve_seeded(&tree, 6, target, &posture, &mut source, 6)
.unwrap();
assert_eq!(report.attempts(), 6);
assert!(report.len() >= 2); // several configurations reach the same pose
// Branch continuity: hold to whichever solution is nearest the configuration already commanded.
let nearest = report.closest_to(&tree, &posture).unwrap();
assert!(tree.configuration_distance(&nearest.joint_positions, &posture) < 1e-6);Nothing here is exhaustive. Without a closed-form solver there is no enumeration of the full
solution set — multi-start finds some branches, probabilistically from jittered seeds or
deterministically from chosen ones. Note also that dedup runs through configuration_distance, so
readings 2π apart are two branches on a Revolute joint and one on a Continuous one.
smallest_singular_value gives σ_min — the distance to a rank deficiency, and what the solver
ramps its damping against. classify_singularity names the degenerate direction instead:
Positional where the lost twist direction is predominantly translational, Rotational where it
is predominantly angular, Mixed where neither half carries two thirds, None above the
threshold. Not wrist/elbow/shoulder — the same classifier runs on planar chains, legs and
floating-base trees.
use multicalc::kinematics::{
JacobianFrame, Joint, JointParent, KinematicTree, SingularityKind,
};
use multicalc::linear_algebra::Vector;
use multicalc::spatial::SE3;
let about_x = Vector::new([1.0, 0.0, 0.0]);
let about_y = Vector::new([0.0, 1.0, 0.0]);
let about_z = Vector::new([0.0, 0.0, 1.0]);
// A gantry: three slides plus hinges about x and y. Rank 5 against a 6-DOF task, degenerate in
// omega_z.
let tree = KinematicTree::<6, 6, f64>::try_from_joints(
&[
Joint::prismatic(about_x, SE3::identity()),
Joint::prismatic(about_y, SE3::identity()),
Joint::prismatic(about_z, SE3::identity()),
Joint::revolute(about_x, SE3::identity()),
Joint::revolute(about_y, SE3::identity()),
Joint::fixed(SE3::identity()),
],
&[
JointParent::World,
JointParent::Joint(0),
JointParent::Joint(1),
JointParent::Joint(2),
JointParent::Joint(3),
JointParent::Joint(4),
],
)
.unwrap();
let jacobian = tree
.geometric_jacobian_at(&Vector::zeros(), 5, JacobianFrame::World)
.unwrap();
assert_eq!(
jacobian.classify_singularity(1e-2).unwrap(),
SingularityKind::Rotational
);The threshold is the solver's own damping threshold: at or above it, the chain reads as full rank. Only the direction belonging to σ_min is classified, so under a nullity above one — any chain with fewer than six actuated DOF — the reported direction is an arbitrary member of the degenerate subspace.