Pure Nx implementation of FABRIK, constrained to a robot's real joint axes.
solve_constrained/3 is the entry point. backward_pass/3 is the classic
FABRIK reach it builds on, kept separate because it is the one half of the
algorithm that needs no knowledge of how the joints are allowed to move.
Summary
Functions
FABRIK backward reaching pass.
Place a point at desired_distance from anchor, along the direction from
anchor toward point_to_move. The per-joint reaching step shared by both
passes; a defn so it composes into them and is reusable on its own.
Run FABRIK against a chain whose joints each turn about one fixed axis.
Functions
@spec backward_pass(Nx.Tensor.t(), Nx.Tensor.t(), Nx.Tensor.t()) :: Nx.Tensor.t()
FABRIK backward reaching pass.
Pins the end effector to target, then walks toward the root placing each
joint at its segment length from the next. points is {n, 3}, lengths
{n - 1}. Returns the updated {n, 3} points.
Place a point at desired_distance from anchor, along the direction from
anchor toward point_to_move. The per-joint reaching step shared by both
passes; a defn so it composes into them and is reusable on its own.
@spec solve_constrained(map(), map(), map()) :: {Nx.Tensor.t(), Nx.Tensor.t(), Nx.Tensor.t(), Nx.Tensor.t()}
Run FABRIK against a chain whose joints each turn about one fixed axis.
Classic FABRIK moves points freely, as though every joint were a ball joint. A robot's are not, so the point configuration it converges on generally has no counterpart in any pose the robot can hold, and reading joint values back out of it is a fit that starts centimetres wrong.
This keeps the backward pass — the reach that pins the end effector to the target and distributes the correction back along the chain — but treats its answer as a set of desired directions rather than positions. The forward pass then walks base to tip choosing, for each joint, the rotation about its real world axis that carries its links closest to those directions, clamped to its limits, and regenerates the positions beyond it by forward kinematics. Every pose considered is therefore one the robot can hold, and the joint values are the solver's output rather than something recovered afterwards.
Arguments
chain- the description fromBB.IK.FABRIK.Chain.kinematics/1target-%{position: {3}, rotation: {3, 3}, enforce: scalar}, whereenforceabove0.5asks for the orientation as well as the positionopts-%{max_iterations:, tolerance:, orientation_tolerance:, lever:}
All geometry is in the chain root's frame, target included. Returns
{positions, iterations, residual, orientation_residual} as tensors, and
vectorises over a leading batch axis so a fleet of identical chains — the legs
of a gait, say — solves in one call with a lane per chain.