anvil.kinematics

Kinematics of an arm cut from a URDF: poses, forward kinematics, the Jacobian, gravity torque and inverse kinematics.

A Chain is the joints from a URDF's root link to a tip link; Pose is where a frame is; solve_ik finds the joint positions that put a chain's tip at a pose, tuned by IkParams and answered with an IkSolution. Every call here is pure computation: nothing moves an arm or needs a runtime.

Classes:

Name	Description
`Pose`	A rigid-body pose: a position in meters and an orientation as a unit quaternion, x y z w.
`IkParams`	Tuning for `solve_ik`: when to stop, the rest posture it leans toward, and how hard.
`IkSolution`	What `solve_ik` found: the joint positions, whether they reach the target, and how close they got.
`Chain`	The driven joints of one URDF from its root link to a tip link: forward kinematics, Jacobian and gravity torque.

Functions:

Name	Description
`solve_ik`	Returns the joint positions that put `chain`'s tip at `target`, iterating from `seed`.

Attributes:

Name	Type	Description
`MAX_DOF`	`int`	Most driven joints a chain can have, and the length of `IkParams.rest_q`.

MAX_DOF `module-attribute`

MAX_DOF: int = 8

Most driven joints a chain can have, and the length of IkParams.rest_q.

Pose

Pose(
    position: Sequence[float], orientation: Sequence[float]
)

Bases: Struct

A rigid-body pose: a position in meters and an orientation as a unit quaternion, x y z w.

The frame a pose is stated in is named where it is used: a chain's tip pose is in its URDF's root link's frame, the world frame for a robot's chain. A pose holds any values; one with a component that is not finite, or an orientation whose norm is more than 0.001 from 1, is refused by the call it is passed to. An orientation and its negation are the same rotation, and a pose a call returns may carry either sign, so compare orientations with angle_rad rather than ==.

Builds a pose from a position and an orientation.

Parameters:

Name	Type	Description	Default
`position`	`Sequence[float]`	x, y and z, in meters.	required
`orientation`	`Sequence[float]`	The unit quaternion as x, y, z, w, w last, so the identity is `[0, 0, 0, 1]`.	required

Raises:

Type	Description
`TypeError`	A field is a string, or an entry is not a number.
`InvalidArgumentError`	`position` does not have 3 entries or `orientation` does not have 4.

Methods:

Name	Description
`identity`	Returns the pose with no translation and no rotation: a frame stated in itself.
`compose`	Returns `other`, stated relative to this pose, as it is in the frame this pose is stated in.
`inverse`	Returns the pose that undoes this one: where the frame this one is stated in sits relative to it.
`distance_m`	Returns how far apart the two positions are, in meters.
`angle_rad`	Returns the angle of the rotation from one orientation to the other, in radians, from 0 to pi.

Attributes:

Name	Type	Description
`position`	`list[float]`	Position as x, y, z, in meters.
`orientation`	`list[float]`	Orientation as a unit quaternion, x, y, z, w, w last.

position `property` `writable`

position: list[float]

Position as x, y, z, in meters.

Raises:

Type	Description
`TypeError`	On assignment, a string, or an entry that is not a number.
`InvalidArgumentError`	On assignment, a value without exactly 3 entries.

orientation `property` `writable`

orientation: list[float]

Orientation as a unit quaternion, x, y, z, w, w last.

Raises:

Type	Description
`TypeError`	On assignment, a string, or an entry that is not a number.
`InvalidArgumentError`	On assignment, a value without exactly 4 entries.

identity `classmethod`

identity() -> Self

Returns the pose with no translation and no rotation: a frame stated in itself.

compose

compose(other: Pose) -> Pose

Returns other, stated relative to this pose, as it is in the frame this pose is stated in.

pose.compose(other=pose.inverse()) is the identity. The result's orientation is normalized.

Parameters:

Name	Type	Description	Default
`other`	`Pose`	A pose stated in the frame this one describes.	required

Raises:

Type	Description
`InvalidArgumentError`	Either pose has a component that is not finite, or an orientation that is not a unit quaternion.

inverse

inverse() -> Pose

Returns the pose that undoes this one: where the frame this one is stated in sits relative to it.

Raises:

Type	Description
`InvalidArgumentError`	This pose has a component that is not finite, or an orientation that is not a unit quaternion.

distance_m

distance_m(other: Pose) -> float

Returns how far apart the two positions are, in meters.

Parameters:

Name	Type	Description	Default
`other`	`Pose`	The pose to measure to, in the same frame as this one.	required

Raises:

Type	Description
`InvalidArgumentError`	Either pose has a component that is not finite, or an orientation that is not a unit quaternion, though only the positions are compared.

angle_rad

angle_rad(other: Pose) -> float

Returns the angle of the rotation from one orientation to the other, in radians, from 0 to pi.

Parameters:

Name	Type	Description	Default
`other`	`Pose`	The pose to measure to, in the same frame as this one.	required

Raises:

Type	Description
`InvalidArgumentError`	Either pose has a component that is not finite, or an orientation that is not a unit quaternion.

IkParams

IkParams(
    max_iterations: int | None = None,
    residual_m: float | None = None,
    residual_rad: float | None = None,
    rest_q: Sequence[float] = (),
    damping_lambda_sq: float | None = None,
    rest_gain: float | None = None,
    limit_gain: float | None = None,
)

Bases: Struct

Tuning for solve_ik: when to stop, the rest posture it leans toward, and how hard.

IkParams() holds the solver's own defaults; anvil.workcell.RobotConfig.ik_params gives each arm of a robot the parameters its description states. Fields are checked when a solve runs, not when they are set: solve_ik refuses a value no solve can run on, and each field says which.

Builds parameters from the solver's defaults, overriding the fields given.

Parameters:

Name	Type	Description	Default
`max_iterations`	`int \| None`	Most steps a solve takes; `None` keeps the default.	`None`
`residual_m`	`float \| None`	Tip position error, in meters, at or under which the position counts as reached; `None` keeps the default.	`None`
`residual_rad`	`float \| None`	Tip orientation error, in radians, at or under which the orientation counts as reached; `None` keeps the default.	`None`
`rest_q`	`Sequence[float]`	The posture to lean toward, chain order, at most `MAX_DOF` entries; zero-padded to `MAX_DOF`.	`()`
`damping_lambda_sq`	`float \| None`	Damping of each step; `None` keeps the default.	`None`
`rest_gain`	`float \| None`	How hard each joint is pulled toward its rest pose; `None` keeps the default.	`None`
`limit_gain`	`float \| None`	How hard a joint past a limit is pushed back inside it; `None` keeps the default.	`None`

Raises:

Type	Description
`TypeError`	`max_iterations` is not an integer; `rest_q` is a string, or an entry is not a number.
`InvalidArgumentError`	`max_iterations` is negative or too large to hold; `rest_q` has more than `MAX_DOF` entries.

Attributes:

Name	Type	Description
`max_iterations`	`int`	Most steps a solve takes; `solve_ik` refuses fewer than 1 or more than 200.
`residual_m`	`float`	Tip position error, in meters, at or under which the position counts as reached.
`residual_rad`	`float`	Tip orientation error, in radians, at or under which the orientation counts as reached.
`rest_q`	`list[float]`	Posture the solve leans toward, `MAX_DOF` positions in chain order, in radians or meters.
`damping_lambda_sq`	`float`	Damping of each step, the square of lambda; `solve_ik` refuses less than 1e-6 or a value that is not finite.
`rest_gain`	`float`	Pull of each joint toward its rest pose, per unit away from it; `solve_ik` refuses one outside 0 to 1e6.
`limit_gain`	`float`	Push of a joint back inside a limit it is past, per span of travel; `solve_ik` refuses one outside 0 to 1e6.

max_iterations `property` `writable`

max_iterations: int = 50

Most steps a solve takes; solve_ik refuses fewer than 1 or more than 200.

Raises:

Type	Description
`TypeError`	On assignment, a value that is not an integer.
`InvalidArgumentError`	On assignment, a negative value, or one too large to hold.

residual_m `property` `writable`

residual_m: float = 0.001

Tip position error, in meters, at or under which the position counts as reached.

solve_ik refuses one that is not finite and positive.

residual_rad `property` `writable`

residual_rad: float = 0.01

Tip orientation error, in radians, at or under which the orientation counts as reached.

solve_ik refuses one that is not finite and positive.

rest_q `property` `writable`

rest_q: list[float] = [
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
    0.0,
]

Posture the solve leans toward, MAX_DOF positions in chain order, in radians or meters.

Only the first Chain.num_joints entries are used, though solve_ik refuses any that is not finite. The list writes through: params.rest_q[1] = 0.1 is what the next solve runs with, and a change to its length is refused.

Raises:

Type	Description
`TypeError`	On assignment, a string, or an entry that is not a number.
`InvalidArgumentError`	On assignment, more than `MAX_DOF` entries, which are zero-padded to `MAX_DOF` otherwise; or a change to the returned list's length.

damping_lambda_sq `property` `writable`

damping_lambda_sq: float = 0.36

Damping of each step, the square of lambda; solve_ik refuses less than 1e-6 or a value that is not finite.

Larger takes shorter, steadier steps near a singular configuration; smaller converges faster away from one.

rest_gain `property` `writable`

rest_gain: float = 0.001

Pull of each joint toward its rest pose, per unit away from it; solve_ik refuses one outside 0 to 1e6.

The pull acts only in the directions that leave the tip where it is.

limit_gain `property` `writable`

limit_gain: float = 10.0

Push of a joint back inside a limit it is past, per span of travel; solve_ik refuses one outside 0 to 1e6.

Limits only bias the solve, through this push in the directions that leave the tip where it is; they are never enforced, so IkSolution.q can sit outside them.

IkSolution

IkSolution()

Bases: Counted

What solve_ik found: the joint positions, whether they reach the target, and how close they got.

Get one from solve_ik, never by calling the class.

Attributes:

Name	Type	Description
`q`	`list[float]`	Joint positions, one per joint of the chain in chain order, in radians or meters; possibly outside limits.
`converged`	`bool`	Whether the tip is within both `IkParams.residual_m` and `IkParams.residual_rad` of the target at `q`.
`iterations`	`int`	Steps taken: 0 from a seed already at the target, `IkParams.max_iterations` when the solve ran out.
`residual_m`	`float`	Distance from the tip to the target position at `q`, in meters.
`residual_rad`	`float`	Angle from the tip's orientation to the target's at `q`, in radians.

q `property`

q: list[float]

Joint positions, one per joint of the chain in chain order, in radians or meters; possibly outside limits.

converged `property`

converged: bool

Whether the tip is within both IkParams.residual_m and IkParams.residual_rad of the target at q.

iterations `property`

iterations: int

Steps taken: 0 from a seed already at the target, IkParams.max_iterations when the solve ran out.

residual_m `property`

residual_m: float

Distance from the tip to the target position at q, in meters.

residual_rad `property`

residual_rad: float

Angle from the tip's orientation to the target's at q, in radians.

Chain

Chain()

The driven joints of one URDF from its root link to a tip link: forward kinematics, Jacobian and gravity torque.

One comes from from_urdf_file or from_urdf_text, or for each arm of a robot from anvil.workcell.RobotConfig.chains. A configuration q is one position per driven joint in chain order, root first: radians for a revolute joint, meters for a prismatic one. Every pose and position a chain returns is in its URDF's root link's frame, the root link being the one no joint hangs from; for a robot's chain that is the world frame, the composed URDF's world link, which every anvil.workcell.Mount is placed in. A q is not checked against the limits, and one that is not finite gives results that are not finite.

Get one from Chain.from_urdf_file or Chain.from_urdf_text, never by calling the class.

Methods:

Name	Description
`from_urdf_file`	Reads a URDF file and builds the chain of driven joints from its root link to the link `tip`.
`from_urdf_text`	Builds the chain from URDF text in hand, as `from_urdf_file` does from a file.
`fk`	Returns the tip link's pose at configuration `q`.
`joint_origins`	Returns where each driven joint's origin is at configuration `q`: the elbow's position without a chain to it.
`jacobian`	Returns the geometric Jacobian at the tip link's origin at configuration `q`.
`gravity_torque`	Returns the torque at each driven joint that holds the chain still under gravity at configuration `q`.

Attributes:

Name	Type	Description
`num_joints`	`int`	Number of driven joints from the root link to the tip, at most `MAX_DOF`.
`joint_names`	`list[str]`	URDF names of the driven joints, chain order.
`limits`	`tuple[list[float], list[float]]`	Position limits from the URDF as a `(lower, upper)` pair of lists, chain order, in radians or meters.

num_joints `property`

num_joints: int

Number of driven joints from the root link to the tip, at most MAX_DOF.

joint_names `property`

joint_names: list[str]

URDF names of the driven joints, chain order.

limits `property`

limits: tuple[list[float], list[float]]

Position limits from the URDF as a (lower, upper) pair of lists, chain order, in radians or meters.

from_urdf_file `classmethod`

from_urdf_file(path: str | Path, tip: str) -> Self

Reads a URDF file and builds the chain of driven joints from its root link to the link tip.

Revolute and prismatic joints are driven; fixed joints on the path are folded into the links around them.

Parameters:

Name	Type	Description	Default
`path`	`str \| Path`	The URDF file.	required
`tip`	`str`	The link the chain ends at. The root link itself gives a chain of no joints.	required

Raises:

Type	Description
`NotFoundError`	There is no file at `path`.
`InvalidArgumentError`	The file is not UTF-8 text or not a URDF; `tip` is not one of its links; a joint on the path is neither revolute, prismatic nor fixed; the path has more than `MAX_DOF` driven joints; or a driven joint's lower limit is not below its upper one.
`OperationError`	The file could not be read for another reason, such as `path` being a directory.

from_urdf_text `classmethod`

from_urdf_text(urdf: str, tip: str) -> Self

Builds the chain from URDF text in hand, as from_urdf_file does from a file.

Parameters:

Name	Type	Description	Default
`urdf`	`str`	The URDF's text, such as `anvil.workcell.RobotConfig.composed_urdf()` returns.	required
`tip`	`str`	The link the chain ends at. The root link itself gives a chain of no joints.	required

Raises:

Type	Description
`InvalidArgumentError`	`urdf` is not a URDF; `tip` is not one of its links; a joint on the path is neither revolute, prismatic nor fixed; the path has more than `MAX_DOF` driven joints; or a driven joint's lower limit is not below its upper one.

fk

fk(q: Sequence[float]) -> Pose

Returns the tip link's pose at configuration q.

In the URDF's root link's frame, the world frame for a robot's chain.

Parameters:

Name	Type	Description	Default
`q`	`Sequence[float]`	One position per driven joint, chain order.	required

Raises:

Type	Description
`TypeError`	`q` is a string, or an entry is not a number.
`InvalidArgumentError`	`q` does not have `num_joints` entries, or one is NaN or infinite.

joint_origins

joint_origins(q: Sequence[float]) -> list[list[float]]

Returns where each driven joint's origin is at configuration q: the elbow's position without a chain to it.

Joint i's origin is the far end of the links the joints before it carry, so between two configurations these are how far each link's end travels; the tip's own position is fk's.

Parameters:

Name	Type	Description	Default
`q`	`Sequence[float]`	One position per driven joint, chain order.	required

Returns:

Type	Description
`list[list[float]]`	One `[x, y, z]` per driven joint, chain order, in meters in the URDF's root link's frame, the world frame for a robot's chain.

Raises:

Type	Description
`TypeError`	`q` is a string, or an entry is not a number.
`InvalidArgumentError`	`q` does not have `num_joints` entries, or one is NaN or infinite.

jacobian

jacobian(q: Sequence[float]) -> list[list[float]]

Returns the geometric Jacobian at the tip link's origin at configuration q.

In the URDF's root link's frame, the world frame for a robot's chain.

Parameters:

Name	Type	Description	Default
`q`	`Sequence[float]`	One position per driven joint, chain order.	required

Returns:

Type	Description
`list[list[float]]`	6 rows of `num_joints` columns, one column per driven joint in chain order. Rows 0 to 2 are the tip's linear velocity along x, y and z per unit of joint velocity, rows 3 to 5 its angular velocity about them; a prismatic joint's angular rows are zero.

Raises:

Type	Description
`TypeError`	`q` is a string, or an entry is not a number.
`InvalidArgumentError`	`q` does not have `num_joints` entries, or one is NaN or infinite.

gravity_torque

gravity_torque(q: Sequence[float]) -> list[float]

Returns the torque at each driven joint that holds the chain still under gravity at configuration q.

Gravity is 9.81 meters per second squared along the negative z of the URDF's root link's frame, the world frame for a robot's chain, acting on the link masses and centers of mass the URDF states. A URDF with a gripper grafted on, such as anvil.workcell.RobotConfig.composed_urdf() returns, carries the gripper's mass too. Links that branch off the chain, such as a gripper's fingers, load the link they hang from, held at their joints' zero.

Parameters:

Name	Type	Description	Default
`q`	`Sequence[float]`	One position per driven joint, chain order.	required

Returns:

Type	Description
`list[float]`	One entry per driven joint, chain order: newton meters for a revolute joint, newtons for a prismatic one.

Raises:

Type	Description
`TypeError`	`q` is a string, or an entry is not a number.
`InvalidArgumentError`	`q` does not have `num_joints` entries, or one is NaN or infinite.

solve_ik

solve_ik(
    chain: Chain,
    seed: Sequence[float],
    target: Pose,
    params: IkParams | None = None,
) -> IkSolution

Returns the joint positions that put chain's tip at target, iterating from seed.

Each step is a damped least-squares step toward the target pose, leaning toward IkParams.rest_q and back inside the joint limits in the directions that leave the tip where it is, and no joint moves more than pi/4 in one step. It stops when the tip is within both residuals or IkParams.max_iterations steps have been taken, so a solve that runs out returns its last configuration with IkSolution.converged false rather than raising.

Parameters:

Name	Type	Description	Default
`chain`	`Chain`	The chain to solve for.	required
`seed`	`Sequence[float]`	The configuration to start from, one position per driven joint, chain order.	required
`target`	`Pose`	The tip link's pose to reach, in the chain's URDF's root link's frame, the world frame for a robot's chain.	required
`params`	`IkParams \| None`	The solver's tuning; `None` is `IkParams()`.	`None`

Raises:

Type	Description
`TypeError`	`seed` is a string, or an entry is not a number.
`InvalidArgumentError`	`seed` does not have `chain.num_joints` entries or has one that is not finite; `target` has a component that is not finite or an orientation that is not a unit quaternion; or a field of `params` is outside the range that field states.

anvil.kinematics