src.kinematics package

Submodules

src.kinematics.fk module

class src.kinematics.fk.JointFrame(joint_name, joint, T_world)

Bases: object

T_world: ndarray

joint: JointInfo

joint_name: str

class src.kinematics.fk.VisualPose(link_name, visual, T_world)

Bases: object

T_world: ndarray

link_name: str

visual: LinkVisual

src.kinematics.fk.forward_kinematics(robot, theta=None, link_name=None)

Return the world-frame pose of a single link (generic URDF tree FK).

Internally calls link_transforms() and returns the transform for the requested link directly.

Parameters:

• robot (RobotTreeLike) – Robot object implementing the RobotTreeLike interface.

• theta (ndarray | None) – (dof,) active joint vector. If None, robot.get_theta() is used.

• link_name (str | None) – Name of the link whose pose is requested. Defaults to robot.tool_link.

Raises:

KeyError – If link_name is not present in the computed transform tree.

Return type:

ndarray

Returns:

4×4 world-frame homogeneous transform of the requested link.

src.kinematics.fk.joint_frames(robot, theta=None, base_transform=None)

Compute world-frame transforms of all active joint frames of a robot.

This function evaluates forward kinematics for all links and composes each parent-link transform with the joint origin transform to obtain the pose of each active joint frame in the world frame.

Inputs

robotRobot-like object

Must implement:

• active_joint_names

• get_joint(joint_name)

• get_theta()

• expand_theta()

• get_child_joints(link_name)

• link_names

theta(dof,) array-like, optional

Active joint vector. If None, uses robot.get_theta().

base_transform(4,4) ndarray, optional

Homogeneous transform from the robot base/world reference used by the caller to the rendered world frame. If None, identity is used.

returns:

A list of JointFrame objects, each containing:

• joint_name : name of the joint

• joint : JointInfo object

• T_world(4,4) homogeneous transform of the joint frame

  in the world frame

rtype:

list[JointFrame]

Notes

• The returned transform corresponds to the joint frame located at the URDF joint origin.

• This function performs only geometric pose computation and does NOT perform any rendering.

src.kinematics.fk.joint_transform(joint_type, axis, q)

Return the joint-generated homogeneous transform for a single URDF joint.

• Revolute joint with axis \(\hat{a}\):

  \[T = e^{[\hat{a},\, 0]\, q}\]

• Prismatic joint with axis \(\hat{a}\):

  \[T = e^{[0,\, \hat{a}]\, q}\]

• Fixed joint: returns \(I_4\).

Parameters:

• joint_type (str) – One of 'revolute', 'prismatic', or 'fixed'.

• axis (ndarray) – (3,) joint axis vector (expressed in the joint local frame).

• q (float) – Scalar joint displacement (radians for revolute, metres for prismatic).

Raises:

ValueError – If joint_type is not one of the supported types.

Return type:

ndarray

Returns:

4×4 homogeneous transform \(T \in SE(3)\).

src.kinematics.fk.link_transforms(robot, theta=None)

Compute world-frame transforms for every link via a depth-first tree traversal.

Starting from the world link at \(T = I_4\), the function propagates transforms through the kinematic tree:

\[T_{\text{child}} = T_{\text{parent}} \cdot T_{\text{joint origin}} \cdot T_{\text{joint}}(q)\]

where \(T_{\text{joint origin}}\) comes from the URDF <origin> tag and \(T_{\text{joint}}(q)\) is the joint-generated transform from joint_transform().

Parameters:

• robot (RobotTreeLike) – Robot object implementing the RobotTreeLike interface.

• theta (ndarray | None) – (dof,) active joint vector. If None, robot.get_theta() is used.

Return type:

dict[str, ndarray]

Returns:

Dictionary mapping link_name → 4×4 world-frame transform.

src.kinematics.fk.visual_transforms(robot, theta=None, base_transform=None)

Compute world-frame transforms of all visual geometries of a robot.

This function evaluates forward kinematics for all links and composes each link transform with its associated visual origin to obtain the final pose of each mesh in the world frame.

Inputs

robotRobot-like object

Must implement:

• link_names

• get_link_visuals(link_name)

• expand_theta()

• get_theta()

• get_child_joints()

• get_joint()

theta(dof,) array-like, optional

Active joint vector. If None, uses robot.get_theta().

base_transform(4,4) ndarray, optional

Homogeneous transform from the robot base frame to the world frame. If None, identity is used.

returns:

A list of VisualPose objects, each containing:

• link_name : name of the parent link

• visual : LinkVisual object (includes mesh_path and local origin)

• T_world : (4,4) homogeneous transform of the visual in world frame

rtype:

list[VisualPose]

src.kinematics.ik module

src.kinematics.ik.finite_difference_grad(w_func, theta, eps=1e-06)

Approximate the gradient of a scalar objective using central finite differences.

\[\frac{\partial w}{\partial \theta_i} \approx \frac{w(\theta + \varepsilon e_i) - w(\theta - \varepsilon e_i)}{2\varepsilon}\]

where \(e_i\) is the i-th standard basis vector.

Note

This function was generated with AI assistance.

Parameters:

• w_func – Scalar-valued objective \(w(\theta)\) that accepts an (n,) array and returns a float.

• theta – (n,) array of current joint coordinates.

• eps – Finite-difference step size \(\varepsilon\) (default 1e-6).

Returns:

(n,) gradient vector \(\nabla_\theta w\).

src.kinematics.ik.ik_jacobian_transpose_position(M_ee, B_list, theta_init, p_des, max_iters=100, tol_converge=1e-06, q_min=None, q_max=None, K=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), print_iterations=True)

Numerical position-only IK using the Jacobian-transpose method.

Unlike the pseudoinverse, this method uses the transpose of the linear Jacobian weighted by a gain matrix \(K\):

\[\delta\theta = J_v^\top K\, e, \qquad e = p_{\text{des}} - p_{\text{ee}}\]

The Jacobian transpose method avoids the matrix inversion and is unconditionally stable, but typically converges more slowly and does not exploit the null space.

Parameters:

• M_ee (ndarray) – 4×4 home configuration of the end-effector.

• B_list (list) – List of n 6-vector body-frame screw axes.

• theta_init (ndarray) – (n,) initial joint vector \(\theta_0\).

• p_des (ndarray) – (3,) desired end-effector position.

• max_iters (int) – Maximum number of iterations.

• tol_converge (float) – Convergence threshold on \(\|e\|\).

• q_min (ndarray | None) – (n,) lower joint limits (None = unconstrained).

• q_max (ndarray | None) – (n,) upper joint limits (None = unconstrained).

• K (ndarray) – 3×3 positive-definite gain matrix (default: identity).

• print_iterations (bool) – If True, print per-iteration diagnostics.

Return type:

tuple[ndarray, ndarray]

Returns:

(theta, theta_history) where theta is the (n,) solution and theta_history is an (iters+1, n) array of intermediate joint vectors.

src.kinematics.ik.joint_limit_objective(theta, q_min, q_max)

Compute a joint-limit avoidance objective for use in null-space control.

Returns a negative value (to be maximised) that penalises configurations far from the joint-range midpoints:

\[w(\theta) = -\frac{1}{2n} \sum_{i=1}^{n} \left(\frac{\theta_i - \bar{q}_i}{q_{\max,i} - q_{\min,i}}\right)^2, \qquad \bar{q}_i = \frac{q_{\min,i} + q_{\max,i}}{2}.\]

The gradient \(\nabla_\theta w\) is used by numerical_inverse_kinematics_position() / numerical_inverse_kinematics_pose() via finite_difference_grad().

Parameters:

• theta (ndarray) – (n,) current joint coordinates.

• q_min (ndarray) – (n,) lower joint limits.

• q_max (ndarray) – (n,) upper joint limits.

Return type:

float

Returns:

Scalar objective value \(w \le 0\); closer to 0 means closer to the midpoint of each joint range.

src.kinematics.ik.manipulability_objective(theta, B_list)

Compute Yoshikawa’s manipulability as a secondary IK objective.

\[w(\theta) = \sqrt{\det(J_b(\theta)\, J_b(\theta)^\top)}\]

Maximising w in the null-space of the primary task steers the robot away from kinematic singularities.

Parameters:

• theta (ndarray) – (n,) current joint coordinates.

• B_list (list) – List of n 6-vector body-frame screw axes.

Return type:

float

Returns:

Scalar manipulability value \(w \ge 0\).

src.kinematics.ik.numerical_inverse_kinematics_pose(M_ee, B_list, theta_init, T_sd, max_iters=100, tol_w=1e-06, tol_v=1e-06, tol_manipulability=0.001, q_min=None, q_max=None, objective_func=None, objective_args=None, k_null=0.1, k_damping=0.01, print_iterations=True)

Numerical full-pose IK via the body Jacobian and matrix logarithm error.

Error twist (from current body frame to desired frame):

\[\mathcal{V}_b = \left[\log\!\left(T_{bs}\, T_{sd}\right)\right]^\vee\]

where \(T_{bs} = T_{sb}^{-1}\) and \(T_{sb}\) is the current end-effector pose from body-PoE FK.

Primary step:

\[\delta\theta = J_b^\dagger\,\mathcal{V}_b\]

Null-space projection and DLS fallback work identically to numerical_inverse_kinematics_position().

Parameters:

• M_ee (ndarray) – 4×4 home configuration of the end-effector.

• B_list (list) – List of n 6-vector body-frame screw axes.

• theta_init (ndarray) – (n,) initial joint vector \(\theta_0\).

• T_sd (ndarray) – 4×4 desired end-effector pose in the space frame.

• max_iters (int) – Maximum number of Newton iterations.

• tol_w (float) – Convergence threshold on \(\|\omega_b\|\).

• tol_v (float) – Convergence threshold on \(\|v_b\|\).

• tol_manipulability (float) – Manipulability threshold for DLS fallback.

• q_min (ndarray | None) – (n,) lower joint limits (None = unconstrained).

• q_max (ndarray | None) – (n,) upper joint limits (None = unconstrained).

• objective_func (Optional[Callable]) – Optional scalar secondary objective.

• objective_args (Optional[Mapping[str, Any]]) – Keyword arguments forwarded to objective_func.

• k_null (float) – Gain for the null-space secondary objective.

• k_damping (float) – Damping factor \(k\) for the DLS inverse.

• print_iterations (bool) – If True, print per-iteration diagnostics.

Return type:

tuple[ndarray, ndarray]

Returns:

(theta, theta_history) where theta is the (n,) solution and theta_history is an (iters+1, n) array of intermediate joint vectors.

src.kinematics.ik.numerical_inverse_kinematics_position(M_ee, B_list, theta_init, p_des, max_iters=100, tol_converge=1e-06, tol_manipulability=0.001, q_min=None, q_max=None, objective_func=None, objective_args=None, k_null=0.1, k_damping=0.01, print_iterations=True)

Numerical position-only IK via the body Jacobian pseudoinverse.

Primary step (minimise end-effector position error \(e = p_{\text{des}} - p_{\text{ee}}\)):

\[\delta\theta = J_v^\dagger\, e\]

where \(J_v\) is the 3×n linear-velocity block of the body Jacobian.

Null-space projection (optional, secondary objective):

\[\delta\theta \mathrel{+}= \underbrace{(I - J_v^\dagger J_v)}_{\text{null-space projector}} k_{\text{null}}\, \nabla_\theta w(\theta)\]

Near singularities the DLS inverse \(J_v^* = J_v^\top (J_v J_v^\top + k^2 I)^{-1}\) is substituted.

Parameters:

• M_ee (ndarray) – 4×4 home configuration of the end-effector.

• B_list (list) – List of n 6-vector body-frame screw axes.

• theta_init (ndarray) – (n,) initial joint vector \(\theta_0\).

• p_des (ndarray) – (3,) desired end-effector position.

• max_iters (int) – Maximum number of Newton iterations.

• tol_converge (float) – Convergence threshold on \(\|e\|\).

• tol_manipulability (float) – If manipulability() of \(J_v\) falls below this value, the DLS inverse is used instead of the pseudoinverse.

• q_min (ndarray | None) – (n,) lower joint limits (None = unconstrained).

• q_max (ndarray | None) – (n,) upper joint limits (None = unconstrained).

• objective_func (Optional[Callable]) – Optional scalar secondary objective \(w(\theta, **\text{args})\).

• objective_args (Optional[Mapping[str, Any]]) – Keyword arguments forwarded to objective_func.

• k_null (float) – Gain for the null-space secondary objective.

• k_damping (float) – Damping factor \(k\) for the DLS inverse.

• print_iterations (bool) – If True, print per-iteration diagnostics.

Return type:

tuple[ndarray, ndarray]

Returns:

(theta, theta_history) where theta is the (n,) solution and theta_history is an (iters+1, n) array of intermediate joint vectors.

src.kinematics.jacobian module

src.kinematics.jacobian.body_jacobian(B_list, theta)

Compute the 6×n body Jacobian using the PoE backward recursion.

Column i (0-indexed) is the i-th body screw axis transformed by the inverse adjoint of all following joint exponentials:

\[J_b^{(i)} = [\text{Ad}_{e^{-[\mathcal{B}_{i+1}]\theta_{i+1}} \cdots e^{-[\mathcal{B}_n]\theta_n}}]\,\mathcal{B}_{i}, \quad J_b^{(n-1)} = \mathcal{B}_n.\]

Parameters:

• B_list (list) – List of n 6-vector screw axes in the body (end-effector) frame.

• theta (ndarray) – Array of length n containing joint displacements.

Return type:

ndarray

Returns:

6×n body Jacobian \(J_b\).

src.kinematics.jacobian.check_singularity(J)

Return True if the Jacobian is rank-deficient (singular).

A Jacobian is singular when it does not have full row rank (for redundant/square manipulators) or full column rank (for non-redundant):

\[\text{singular} \iff \operatorname{rank}(J) < \min(m, n)\]

Parameters:

J (ndarray) – m*×*n Jacobian matrix.

Return type:

bool

Returns:

True if J is rank-deficient, False otherwise.

src.kinematics.jacobian.damped_least_square_inverse(J, k=0.01)

Compute the Damped Least-Squares (DLS) pseudoinverse of a Jacobian.

The DLS inverse avoids numerical blow-up near singularities by adding a regularisation term \(k^2 I\) to the Gram matrix:

\[J^* = J^\top (J J^\top + k^2 I)^{-1}\]

As \(k \to 0\) this converges to the Moore-Penrose pseudoinverse; larger \(k\) trades accuracy for stability near singularities.

Parameters:

• J – m*×*n Jacobian matrix.

• k – Damping factor (default 0.01).

Returns:

n*×*m DLS pseudoinverse \(J^*\).

src.kinematics.jacobian.manipulability(J)

Compute Yoshikawa’s manipulability measure.

\[\mu = \sqrt{\det(J J^\top)}\]

The measure is zero at a singularity and larger values indicate greater dexterity. For a square, full-rank Jacobian this equals \(|\det(J)|\).

Parameters:

J (ndarray) – m*×*n Jacobian matrix (can be the full 6×n body/space Jacobian or a submatrix such as the 3×n linear-velocity block).

Return type:

float

Returns:

Non-negative scalar manipulability \(\mu\).

src.kinematics.jacobian.manipulability_ellipsoid(J)

Compute the manipulability ellipsoid data for the angular and linear subspaces.

For each of the angular (\(J_\omega\), rows 0–2) and linear (\(J_v\), rows 3–5) blocks the following are computed:

\[A_* = J_* J_*^\top\]

The eigendecomposition \(A_* = V \Lambda V^\top\) yields the principal axes and semi-axis lengths. Three scalar measures are also returned:

\[\begin{split}\mu_1 &= \sqrt{\lambda_{\max} / \lambda_{\min}} &&\text{(isotropy)} \\ \mu_2 &= \lambda_{\max} / \lambda_{\min} &&\text{(condition number)} \\ \mu_3 &= \sqrt{\det(A_*)} &&\text{(ellipsoid volume)}\end{split}\]

Parameters:

J (ndarray) – 6×n Jacobian matrix (full body or space Jacobian).

Return type:

list

Returns:

``[[A_w, eigvals_w, eigvecs_w, mu1_w, mu2_w, mu3_w],

[A_v, eigvals_v, eigvecs_v, mu1_v, mu2_v, mu3_v]]``

where *_w is the angular subspace and *_v is the linear subspace. \(\mu_1\) and \(\mu_2\) are inf when \(\lambda_{\min} \approx 0\).

src.kinematics.jacobian.pseudoinverse_jacobian(J)

Compute the Moore-Penrose pseudoinverse of a Jacobian matrix.

Two cases:

• Fat Jacobian (\(n > m\), full row rank): right pseudoinverse minimising \(\|\delta\theta\|\):

  \[J^\dagger = J^\top (J J^\top)^{-1}\]

• Tall Jacobian (\(n \le m\), full column rank): left pseudoinverse minimising the least-squares residual:

  \[J^\dagger = (J^\top J)^{-1} J^\top\]

Parameters:

J (ndarray) – m*×*n Jacobian matrix (must have full row or column rank).

Raises:

ValueError – If J does not satisfy the required rank condition.

Return type:

ndarray

Returns:

n*×*m pseudoinverse \(J^\dagger\).

src.kinematics.jacobian.space_jacobian(S_list, theta)

Compute the 6×n space Jacobian using the PoE forward recursion.

Column i (0-indexed) is the i-th screw axis transformed into the space frame by the product of all preceding joint exponentials:

\[J_s^{(i)} = [\text{Ad}_{e^{[\mathcal{S}_1]\theta_1} \cdots e^{[\mathcal{S}_i]\theta_i}}]\,\mathcal{S}_{i+1}, \quad J_s^{(0)} = \mathcal{S}_1.\]

Parameters:

• S_list (list) – List of n 6-vector screw axes in the space frame.

• theta (ndarray) – Array of length n containing joint displacements.

Return type:

ndarray

Returns:

6×n space Jacobian \(J_s\).

src.kinematics.kinematic_model module

class src.kinematics.kinematic_model.KinematicModel(M_ee, S_list, B_list, q_min, q_max)

Bases: object

Pre-computed PoE quantities for Jacobian and IK solvers.

M_ee

4×4 home configuration of the end-effector at \(\theta = 0\).

S_list

List of n 6-vector screw axes in the space frame.

B_list

List of n 6-vector screw axes in the body (end-effector) frame.

q_min

(n,) lower joint limits (may contain -inf).

q_max

(n,) upper joint limits (may contain +inf).

B_list: List[ndarray]

M_ee: ndarray

S_list: List[ndarray]

q_max: ndarray

q_min: ndarray

src.kinematics.kinematic_model.build_kinematic_model(robot)

Build all PoE quantities needed for Jacobian and IK computation.

Convenience wrapper that calls compute_home_ee_pose(), compute_screw_axes(), and compute_joint_limit_arrays() in sequence.

Parameters:

robot – Any robot object satisfying the interface expected by the three sub-functions.

Return type:

KinematicModel

Returns:

KinematicModel populated with M_ee, S_list, B_list, q_min, and q_max.

src.kinematics.kinematic_model.compute_home_ee_pose(robot)

Return the home end-effector pose \(M_{\text{ee}}\) at \(\theta = 0\).

The pose is obtained by running tree FK with all joints set to zero and is expressed in the world/space frame used by the robot’s FK.

Parameters:

robot – Robot object with attributes dof and tool_link and implementing forward_kinematics().

Return type:

ndarray

Returns:

4×4 homogeneous transform \(M_{\text{ee}} \in SE(3)\).

src.kinematics.kinematic_model.compute_joint_limit_arrays(robot)

Build (n,) joint-limit arrays ordered by robot.active_joint_names.

For joints without a <limit> element in the URDF the bounds are set to \((-\infty, +\infty)\).

Parameters:

robot – Any robot object that exposes active_joint_names and get_joint(name).

Return type:

tuple[ndarray, ndarray]

Returns:

(q_min, q_max) — each a float64 array of length dof.

src.kinematics.kinematic_model.compute_screw_axes(robot, M_ee=None)

Compute space-frame and body-frame screw axes for all active joints.

For each active joint the URDF joint axis (expressed in the local joint frame) is rotated into the space frame using the home joint-frame rotation \(R_{\text{joint}}\):

\[\hat{a}_{\text{space}} = R_{\text{joint}}\, \hat{a}_{\text{local}}\]

The space screw axis is then constructed as:

• Revolute: \(\mathcal{S} = [\hat{a}_{\text{space}}; -\hat{a}_{\text{space}} \times q]\)

• Prismatic: \(\mathcal{S} = [0; \hat{a}_{\text{space}}]\)

The body screw axis is converted via the inverse adjoint of \(M_{\text{ee}}\):

\[\mathcal{B} = [\text{Ad}_{M_{\text{ee}}^{-1}}]\, \mathcal{S}\]

Parameters:

• robot – Robot object exposing active_joint_names, get_joint, dof, and tool_link.

• M_ee (ndarray | None) – Pre-computed home end-effector pose. If None, computed via compute_home_ee_pose().

Raises:

• KeyError – If a joint frame is missing or a mimic-source joint is not found.

• ValueError – If an active joint has a near-zero axis or an unsupported type.

Return type:

tuple[List[ndarray], List[ndarray]]

Returns:

(S_list, B_list) — each a list of dof 6-vector screw axes.

src.kinematics.kinematic_model.validate_kinematic_model(robot, model, atol=1e-08)

Assert self-consistency of a KinematicModel against a robot.

Checks performed:

1. len(S_list) == len(B_list) == robot.dof

2. q_min.shape == q_max.shape == (robot.dof,)

3. Tree FK home pose agrees with body-PoE home pose:

   \[T_{\text{FK}}(0) \approx M_{\text{ee}}\, e^{[B_1] \cdot 0} \cdots e^{[B_n] \cdot 0} = M_{\text{ee}}\]

Parameters:

• robot – Robot object used to build model.

• model (KinematicModel) – The KinematicModel to validate.

• atol (float) – Absolute tolerance passed to numpy.allclose().

Raises:

ValueError – If any of the above checks fail.

Return type:

None

src.kinematics.pinocchio_ik module

class src.kinematics.pinocchio_ik.PinocchioIK(robot, urdf_path, ee_frame_name)

Bases: object

Pinocchio-backed IK solver for URDF robots with mimic joints.

This class wraps a Pinocchio model built from the robot’s URDF and exposes position-only and full-pose IK solvers that work entirely in the active-joint space (i.e. the subspace spanned by robot.active_joint_names). Mimic joints are handled via an affine projection:

\[q_{\text{full}} = b + A\, q_{\text{act}}\]

where \(A\) and \(b\) are computed from the <mimic> tags in the URDF.

Parameters:

• robot – Robot object exposing active_joint_names, joints, and get_joint.

• urdf_path (str | Path) – Path to the URDF file used to build the Pinocchio model.

• ee_frame_name (str) – Name of the end-effector frame as it appears in the URDF.

Raises:

• ValueError – If ee_frame_name is not found in the Pinocchio model, or if the Pinocchio model has undefined nq/nv.

• KeyError – If the active/mimic joint mapping is inconsistent between the custom robot model and the Pinocchio model.

active_to_full(q_act)

Map the active-joint vector to the full Pinocchio configuration vector.

\[q_{\text{full}} = b + A\, q_{\text{act}}\]

Parameters:

q_act (ndarray) – (dof,) active-joint vector.

Return type:

ndarray

Returns:

(nq,) full Pinocchio configuration vector.

clamp_active(q_act)

Clamp an active-joint vector to the robot’s joint limits.

Parameters:

q_act (ndarray) – (dof,) active-joint vector.

Return type:

ndarray

Returns:

(dof,) clamped active-joint vector within [q_min, q_max].

forward_pose(q_act)

Return the current end-effector pose from Pinocchio FK.

Runs pinocchio.forwardKinematics() and pinocchio.updateFramePlacements() with the full configuration obtained via active_to_full(), then reads off the end-effector placement.

Parameters:

q_act (ndarray) – (dof,) active-joint vector.

Return type:

ndarray

Returns:

4×4 homogeneous end-effector pose in the robot local/base frame.

solve_pose(T_des, q_init, max_iters=50, tol_rot=0.001, tol_pos=0.0001, damping=0.001, step_size=0.5, weight_rot=1.0, weight_pos=1.0)

Full-pose IK in active-joint space using Pinocchio’s SE(3) error.

The pose error is computed in the local (end-effector) frame via Pinocchio’s pin.log6():

\[e = \log\!\left(T_{\text{cur}}^{-1}\, T_{\text{des}}\right)^\vee \in \mathbb{R}^6\]

A weighted DLS step is applied to the 6×dof Jacobian:

\[\delta q_{\text{act}} = (W J_{\text{act}})^\top \left((W J_{\text{act}})(W J_{\text{act}})^\top + k^2 I\right)^{-1} (W e)\]

where \(W = \operatorname{diag}(w_\omega I_3,\, w_v I_3)\).

Parameters:

• T_des (ndarray) – 4×4 desired end-effector pose in the robot local/base frame.

• q_init (ndarray) – (dof,) initial active-joint vector.

• max_iters (int) – Maximum number of iterations.

• tol_rot (float) – Convergence threshold on the rotational component \(\|e_{[:3]}\|\).

• tol_pos (float) – Convergence threshold on the positional component \(\|e_{[3:]}\|\).

• damping (float) – DLS damping factor \(k\).

• step_size (float) – Step-size scaling \(\alpha \in (0, 1]\).

• weight_rot (float) – Weight \(w_\omega\) on the rotational error.

• weight_pos (float) – Weight \(w_v\) on the positional error.

Return type:

tuple[ndarray, bool]

Returns:

(q_act, converged) where q_act is the (dof,) solution and converged is True if both tolerances were met.

solve_position(p_des, q_init, max_iters=50, tol=0.0001, damping=0.001, step_size=0.5)

Position-only IK in active-joint space (Damped Least-Squares).

Iterates the DLS update rule on the 3×dof linear-velocity Jacobian:

\[\delta q_{\text{act}} = J_{\text{act}}^\top \left(J_{\text{act}} J_{\text{act}}^\top + k^2 I\right)^{-1} e, \qquad e = p_{\text{des}} - p_{\text{ee}}\]

where \(J_{\text{act}} = J_{\text{full}} A\) maps from active joints to the Cartesian position error.

Parameters:

• p_des (ndarray) – (3,) desired end-effector position expressed in the robot local/base frame.

• q_init (ndarray) – (dof,) initial active-joint vector.

• max_iters (int) – Maximum number of iterations.

• tol (float) – Convergence threshold on \(\|e\|\).

• damping (float) – DLS damping factor \(k\) (default 1e-3).

• step_size (float) – Step-size scaling \(\alpha \in (0, 1]\) (default 0.5).

Return type:

tuple[ndarray, bool]

Returns:

(q_act, converged) where q_act is the (dof,) solution and converged is True if the tolerance was met.

src.kinematics.poe module

src.kinematics.poe.body_product_of_exponentials(M, B_list, theta)

Forward kinematics via the body-frame Product of Exponentials formula.

\[T(\theta) = M\, e^{[\mathcal{B}_1]\theta_1} \cdots e^{[\mathcal{B}_n]\theta_n}\]

Parameters:

• M (ndarray) – 4×4 home configuration of the end-effector (at \(\theta = 0\)).

• B_list (list) – List of 6-vector screw axes expressed in the body (end-effector) frame.

• theta (ndarray) – Array of length n containing joint displacements.

Return type:

ndarray

Returns:

4×4 end-effector pose in the space frame.

src.kinematics.poe.body_product_of_transforms(M, T_list)

Chain a list of joint transforms in the body frame, starting from the home config.

\[T_{\text{ee}} = M\, T_1\, T_2 \cdots T_n\]

Parameters:

• M (ndarray) – 4×4 home configuration of the end-effector.

• T_list (list) – List of 4×4 joint-generated transforms, ordered from joint 1 to joint n.

Return type:

ndarray

Returns:

4×4 end-effector pose in the space frame.

src.kinematics.poe.space_link_poses(M_list, S_list, theta)

Compute world-frame poses of all link frames for visualization.

For each link i (0-indexed), the pose is accumulated as:

\[T_i = e^{[\mathcal{S}_1]\theta_1} \cdots e^{[\mathcal{S}_i]\theta_i}\, M_i\]

The zeroth frame (base) is returned as \(M_0\) (no joint transform applied).

Parameters:

• M_list (list) – List of 4×4 home configurations, one per link frame including the base (length n + 1).

• S_list (list) – List of n 6-vector screw axes in the space frame.

• theta (ndarray) – Array of length n containing joint displacements.

Return type:

list

Returns:

List of n + 1 4×4 homogeneous poses, one per link frame in the space frame.

src.kinematics.poe.space_product_of_exponentials(M, S_list, theta)

Forward kinematics via the space-frame Product of Exponentials formula.

\[T(\theta) = e^{[\mathcal{S}_1]\theta_1} \cdots e^{[\mathcal{S}_n]\theta_n}\, M\]

Parameters:

• M (ndarray) – 4×4 home configuration of the end-effector (at \(\theta = 0\)).

• S_list (list) – List of 6-vector screw axes expressed in the space frame.

• theta (ndarray) – Array of length n containing joint displacements.

Return type:

ndarray

Returns:

4×4 end-effector pose in the space frame.

src.kinematics.poe.space_product_of_transforms(M, T_list)

Chain a list of joint transforms in the space frame and apply the home config.

\[T_{\text{ee}} = T_1\, T_2 \cdots T_n\, M\]

Parameters:

• M (ndarray) – 4×4 home configuration of the end-effector in the space frame.

• T_list (list) – List of 4×4 joint-generated transforms, ordered from joint 1 to joint n.

Return type:

ndarray

Returns:

4×4 end-effector pose in the space frame.

src.kinematics.se3 module

src.kinematics.se3.adjoint(T)

Compute the 6×6 adjoint representation \([\text{Ad}_T]\) of an SE(3) element.

\[\begin{split}[\text{Ad}_T] = \begin{bmatrix} R & 0 \\ [p]R & R \end{bmatrix}\end{split}\]

where \([p]\) is the skew-symmetric matrix of the translation \(p\).

Parameters:

T (ndarray) – 4×4 homogeneous transformation \(T \in SE(3)\).

Return type:

ndarray

Returns:

6×6 adjoint matrix \([\text{Ad}_T]\).

src.kinematics.se3.adjoint_inverse(T)

Compute the 6×6 inverse adjoint \([\text{Ad}_{T^{-1}}]\).

\[\begin{split}[\text{Ad}_{T^{-1}}] = \begin{bmatrix} R^\top & 0 \\ -R^\top[p] & R^\top \end{bmatrix}\end{split}\]

This is equivalent to \([\text{Ad}_T]^{-1}\) but computed without a matrix inversion.

Parameters:

T (ndarray) – 4×4 homogeneous transformation \(T \in SE(3)\).

Return type:

ndarray

Returns:

6×6 inverse adjoint matrix \([\text{Ad}_{T^{-1}}]\).

src.kinematics.se3.adjoint_transform(T, X, to_space=True)

Transform a twist between body and space frames using the adjoint map.

• Body → Space (to_space=True):

  \[\mathcal{V}_s = [\text{Ad}_{T_{sb}}]\, \mathcal{V}_b\]

• Space → Body (to_space=False):

  \[\mathcal{V}_b = [\text{Ad}_{T_{sb}^{-1}}]\, \mathcal{V}_s\]

Parameters:

• T (ndarray) – 4×4 pose of the body frame in the space frame (\(T_{sb}\)).

• X (ndarray) – 6-vector twist to transform.

• to_space (bool) – If True, transforms from body to space frame; if False, from space to body frame.

Return type:

ndarray

Returns:

Transformed 6-vector twist.

src.kinematics.se3.adjoint_transform_list(T, X_list, to_space=True)

Apply adjoint_transform() to a list of twists.

Applies the same adjoint transformation elementwise:

\[\mathcal{V}'_i = [\text{Ad}_{T_{sb}}^{\pm 1}]\, \mathcal{V}_i \quad \forall\, i.\]

Parameters:

• T (ndarray) – 4×4 pose of the body frame in the space frame (\(T_{sb}\)).

• X_list (list) – List of 6-vector twists to transform.

• to_space (bool) – If True, transforms from body to space frame; if False, from space to body frame.

Return type:

list

Returns:

List of transformed 6-vector twists in the same order as X_list.

src.kinematics.se3.exp_screw_axis(S, theta)

Compute the matrix exponential \(e^{[\mathcal{S}]\theta} \in SE(3)\).

Convenience wrapper around exp_screw_hat() that accepts the 6-vector form of the screw axis directly.

\[T = e^{[\mathcal{S}]\theta}, \quad \mathcal{S} \in \mathbb{R}^6.\]

Parameters:

• S (ndarray) – 6-vector \([\omega^\top, v^\top]^\top\) screw axis.

• theta (float) – Joint displacement (radians for revolute, metres for prismatic).

Return type:

ndarray

Returns:

4×4 homogeneous transformation \(T \in SE(3)\).

src.kinematics.se3.exp_screw_hat(hat_S, theta)

Compute the matrix exponential \(e^{[\mathcal{S}]\theta} \in SE(3)\).

For a revolute-type screw axis (\(\|\omega\| > 0\)):

\[\begin{split}T = e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[\omega]\theta} & G(\theta)\, v \\ 0 & 1 \end{bmatrix}\end{split}\]

where \(e^{[\omega]\theta}\) uses Rodrigues’ formula and

\[G(\theta) = I\theta + (1-\cos\theta)[\omega] + (\theta - \sin\theta)[\omega]^2.\]

For a pure-translation axis (\(\omega = 0\)):

\[\begin{split}T = \begin{bmatrix} I & v\,\theta \\ 0 & 1 \end{bmatrix}.\end{split}\]

Parameters:

• hat_S (ndarray) – 4×4 \(\mathfrak{se}(3)\) matrix \([\mathcal{S}]\).

• theta (float) – Joint displacement (radians for revolute, metres for prismatic).

Return type:

ndarray

Returns:

4×4 homogeneous transformation \(T \in SE(3)\).

src.kinematics.se3.inv_SE3(T)

Compute the closed-form inverse of a homogeneous transformation.

Exploits the structure of \(SE(3)\) to avoid a generic matrix inversion:

\[\begin{split}T^{-1} = \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

T (ndarray) – 4×4 homogeneous transformation \(T \in SE(3)\).

Return type:

ndarray

Returns:

4×4 inverse \(T^{-1} \in SE(3)\).

src.kinematics.se3.log_screw_axis(T)

Compute the matrix logarithm of an SE(3) element (inverse of exp_screw_axis()).

Returns the screw axis \(\mathcal{S}\) and displacement \(\theta\) such that \(T = e^{[\mathcal{S}]\theta}\).

For the revolute case (\(\|\omega\| > 0\)):

\[\theta = \arccos\!\left(\frac{\operatorname{tr}(R)-1}{2}\right), \qquad v = G^{-1}(\theta)\, p\]

where

\[G^{-1}(\theta) = \frac{I}{\theta} - \frac{[\omega]}{2} + \left(\frac{1}{\theta} - \frac{1}{2}\cot\frac{\theta}{2}\right)[\omega]^2.\]

For the pure-translation case (\(\omega = 0\)):

\[\mathcal{S} = [0,\, \hat{v}]^\top, \quad \theta = \|p\|.\]

Parameters:

T (ndarray) – 4×4 homogeneous transformation \(T \in SE(3)\).

Return type:

tuple[ndarray, float]

Returns:

(S, theta) where S is the 6-vector screw axis and theta is the scalar displacement.

src.kinematics.se3.origin_transform(xyz, rpy)

Build a 4×4 homogeneous transform from a URDF <origin> tag.

\[\begin{split}T = \begin{bmatrix} R(\text{rpy}) & \text{xyz} \\ 0 & 1 \end{bmatrix}\end{split}\]

where \(R(\text{rpy}) = R_z(y)\,R_y(p)\,R_x(r)\) (ZYX convention).

Parameters:

• xyz (ndarray) – 3-vector translation \([x, y, z]^\top\) (metres).

• rpy (ndarray) – 3-vector \([\text{roll}, \text{pitch}, \text{yaw}]^\top\) (radians).

Return type:

ndarray

Returns:

4×4 homogeneous transform \(T \in SE(3)\).

src.kinematics.se3.screw_axis_from_w_q(w, q)

Build a revolute screw axis from a rotation axis and a point on the axis.

A revolute screw axis has zero pitch, so the linear component is determined entirely by the cross-product rule:

\[\begin{split}\mathcal{S} = \begin{bmatrix} \hat{\omega} \\ v \end{bmatrix}, \quad v = -\hat{\omega} \times q = -[\hat{\omega}]\, q\end{split}\]

Parameters:

• w (ndarray) – Unit rotation axis \(\hat{\omega} \in \mathbb{R}^3\).

• q (ndarray) – Any point on the rotation axis \(q \in \mathbb{R}^3\).

Return type:

ndarray

Returns:

6-vector \(\mathcal{S} = [\hat{\omega}^\top,\, v^\top]^\top\).

src.kinematics.se3.vec_to_se3(V)

Lift a 6-vector twist to its 4×4 \(\mathfrak{se}(3)\) matrix (hat map).

\[\begin{split}\mathcal{V} = \begin{bmatrix} \omega \\ v \end{bmatrix} \;\longmapsto\; [\mathcal{V}] = \begin{bmatrix} [\omega] & v \\ 0 & 0 \end{bmatrix} \in \mathfrak{se}(3)\end{split}\]

Parameters:

V (ndarray) – 6-vector \([\omega^\top, v^\top]^\top\) of twist coordinates.

Return type:

ndarray

Returns:

4×4 \(\mathfrak{se}(3)\) matrix \([\mathcal{V}]\).

src.kinematics.so3 module

src.kinematics.so3.AxisAngleToR(w, theta)

Convert a rotation axis–angle pair to a 3×3 rotation matrix (Rodrigues’ formula).

\[R = e^{[\omega]\theta} = I + \sin\theta\,[\omega] + (1 - \cos\theta)\,[\omega]^2\]

where \([\omega]\) is the skew()-symmetric (hat) matrix of the unit vector \(\hat{\omega}\).

Parameters:

• w (ndarray) – Rotation axis as a 3-vector (need not be a unit vector; normalised internally).

• theta (float) – Rotation angle in radians.

Return type:

ndarray

Returns:

3×3 rotation matrix \(R \in SO(3)\). Returns \(I\) when \(\|w\| < 10^{-12}\).

src.kinematics.so3.RToAxisAngle(R, eps=1e-06)

Extract the rotation axis and angle from a rotation matrix (matrix logarithm).

Three cases are handled:

• Identity (\(\|R - I\| < \varepsilon\)): axis is undefined; returns \(\hat{z}\) and \(\theta = 0\).

• :math:`theta = pi` (\(\operatorname{tr}(R) = -1\)): extracts the axis from the column with the largest diagonal entry.

• Generic case: uses

  \[\theta = \arccos\!\left(\frac{\operatorname{tr}(R)-1}{2}\right), \qquad [\hat{\omega}] = \frac{R - R^\top}{2\sin\theta}\]

Parameters:

• R (ndarray) – 3×3 rotation matrix.

• eps (float) – Numerical tolerance for detecting special cases.

Return type:

tuple[ndarray, float]

Returns:

(w_hat, theta) where w_hat is the unit rotation axis (shape (3,)) and theta is the rotation angle in radians \(\in [0, \pi]\).

src.kinematics.so3.Rx(a)

Return the 3×3 rotation matrix for a rotation about the X axis.

\[\begin{split}R_x(a) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos a & -\sin a \\ 0 & \sin a & \cos a \end{bmatrix}\end{split}\]

Parameters:

a (float) – Rotation angle in radians.

Return type:

ndarray

Returns:

3×3 rotation matrix \(R_x(a) \in SO(3)\).

src.kinematics.so3.Ry(a)

Return the 3×3 rotation matrix for a rotation about the Y axis.

\[\begin{split}R_y(a) = \begin{bmatrix} \cos a & 0 & \sin a \\ 0 & 1 & 0 \\ -\sin a & 0 & \cos a \end{bmatrix}\end{split}\]

Parameters:

a (float) – Rotation angle in radians.

Return type:

ndarray

Returns:

3×3 rotation matrix \(R_y(a) \in SO(3)\).

src.kinematics.so3.Rz(a)

Return the 3×3 rotation matrix for a rotation about the Z axis.

\[\begin{split}R_z(a) = \begin{bmatrix} \cos a & -\sin a & 0 \\ \sin a & \cos a & 0 \\ 0 & 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

a (float) – Rotation angle in radians.

Return type:

ndarray

Returns:

3×3 rotation matrix \(R_z(a) \in SO(3)\).

src.kinematics.so3.rpy_to_R(rpy)

Convert roll-pitch-yaw angles to a 3×3 rotation matrix (ZYX convention).

The rotation is applied in the order roll → pitch → yaw:

\[R = R_z(y)\, R_y(p)\, R_x(r)\]

Parameters:

rpy (ndarray) – Array of shape (3,) containing [roll, pitch, yaw] in radians.

Return type:

ndarray

Returns:

3×3 rotation matrix \(R \in SO(3)\).

src.kinematics.so3.skew(w)

Build the 3×3 skew-symmetric (hat) matrix of a 3-vector.

The hat map \((\cdot)^\wedge : \mathbb{R}^3 \to \mathfrak{so}(3)\) is defined as:

\[\begin{split}[\omega]^{} = \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \end{bmatrix}\end{split}\]

so that \([\omega] v = \omega \times v\) for any \(v \in \mathbb{R}^3\).

Parameters:

w (ndarray) – 3-vector \(\omega = [w_1, w_2, w_3]^\top\).

Return type:

ndarray

Returns:

3×3 skew-symmetric matrix \([\omega] \in \mathfrak{so}(3)\).

src.kinematics.so3.unskew(hat_w)

Extract the 3-vector from a skew-symmetric matrix (vee map).

Inverse of skew():

\[\begin{split}\omega = [\omega]^{\vee} = \begin{bmatrix} [\omega]_{32} \\ [\omega]_{13} \\ [\omega]_{21} \end{bmatrix}\end{split}\]

Parameters:

hat_w (ndarray) – 3×3 skew-symmetric matrix \([\omega] \in \mathfrak{so}(3)\).

Return type:

ndarray

Returns:

3-vector \(\omega\).

Module contents