Kinematics API Reference

This page documents ManipulaPy.kinematics, the module for manipulator kinematics computations.

Tip

For conceptual explanations, see Kinematics User Guide.

SerialManipulator Class

class ManipulaPy.kinematics.SerialManipulator(M_list, omega_list, r_list=None, b_list=None, S_list=None, B_list=None, G_list=None, joint_limits=None)[source]

Bases: object

Main class for serial manipulator kinematics using Product of Exponentials (PoE) formulation.

Constructor

Parameters:

M_list (ndarray[Any, dtype[float64]]) –
omega_list (ndarray[Any, dtype[float64]] | List[float]) –
r_list (ndarray[Any, dtype[float64]] | List[float] | None) –
b_list (ndarray[Any, dtype[float64]] | List[float] | None) –
S_list (ndarray[Any, dtype[float64]] | None) –
B_list (ndarray[Any, dtype[float64]] | None) –
G_list (ndarray[Any, dtype[float64]] | List[ndarray[Any, dtype[float64]]] | None) –
joint_limits (List[Tuple[float | None, float | None]] | None) –

__init__(M_list, omega_list, r_list=None, b_list=None, S_list=None, B_list=None, G_list=None, joint_limits=None)[source]

Initialize the class with the given parameters.

Parameters:

M_list (list) – A list of M values.
omega_list (list) – A list of omega values.
r_list (list, optional) – A list of r values. Defaults to None.
b_list (list, optional) – A list of b values. Defaults to None.
S_list (list, optional) – A list of S values. Defaults to None.
B_list (list, optional) – A list of B values. Defaults to None.
G_list (list, optional) – A list of G values. Defaults to None.
joint_limits (list, optional) – A list of joint limits. Defaults to None.

Return type:

None

Parameters:

M_list (numpy.ndarray) – Home configuration matrix (4×4)

omega_list (numpy.ndarray) – Joint rotation axes (3×n)

r_list (numpy.ndarray, optional) – Joint position vectors (3×n)

b_list (numpy.ndarray, optional) – Body frame joint positions (3×n)

S_list (numpy.ndarray, optional) – Space frame screw axes (6×n)

B_list (numpy.ndarray, optional) – Body frame screw axes (6×n)

G_list (numpy.ndarray, optional) – Spatial inertia matrices (6×6×n)

joint_limits (list, optional) – Joint limits [(min, max), …] for each joint

Auto-computation: Missing S_list or B_list computed from omega_list and r_list/b_list.

State Management

update_state(joint_positions, joint_velocities=None)[source]

Updates the internal state of the manipulator.

Parameters:

joint_positions (np.ndarray) – Current joint positions.
joint_velocities (np.ndarray, optional) – Current joint velocities. Default is None.

Return type:

None

Parameters:

joint_positions (array_like) – Current joint positions in radians

joint_velocities (array_like, optional) – Current joint velocities in rad/s

Purpose: Updates internal state for stateful operations.

Forward Kinematics

forward_kinematics(thetalist, frame='space')[source]

Compute the forward kinematics of a robotic arm using the product of exponentials method.

Parameters:

thetalist (numpy.ndarray) – A 1D array of joint angles in radians.
frame (str, optional) – The frame in which to compute the forward kinematics. Either ‘space’ or ‘body’.

Returns:

The 4x4 transformation matrix representing the end-effector’s pose.

Return type:

numpy.ndarray

Parameters:

thetalist (array_like) – Joint angles in radians

frame (str) – Reference frame: “space” or “body” (default: “space”)

Returns:

T (numpy.ndarray) – 4×4 transformation matrix

Formula (Space): T(θ) = e^{S₁θ₁} e^{S₂θ₂} … e^{Sₙθₙ} M

Formula (Body): T(θ) = M e^{B₁θ₁} e^{B₂θ₂} … e^{Bₙθₙ}

Example:

>>> T = robot.forward_kinematics([0.1, 0.2, 0.3], frame="space")
>>> position = T[:3, 3]  # Extract position
>>> rotation = T[:3, :3]  # Extract rotation matrix

end_effector_pose(thetalist)[source]

Computes the end-effector’s position and orientation given joint angles.

Parameters:: thetalist (numpy.ndarray) – A 1D array of joint angles in radians.
Returns:: A 6x1 vector representing the position and orientation (Euler angles) of the end-effector.
Return type:: numpy.ndarray

Parameters:

thetalist (array_like) – Joint angles in radians

Returns:

pose (numpy.ndarray) – 6-element vector [x, y, z, roll, pitch, yaw]

Components: Position (meters) + Euler angles (radians)

Velocity Kinematics

jacobian(thetalist, frame='space')[source]

Calculate the Jacobian matrix for the given joint angles.

Parameters:

thetalist (list) – A list of joint angles.
frame (str) – The reference frame for the Jacobian calculation. Valid values are ‘space’ or ‘body’. Defaults to ‘space’.

Returns:

The Jacobian matrix of shape (6, len(thetalist)).

Return type:

numpy.ndarray

Parameters:

thetalist (array_like) – Joint angles in radians

frame (str) – Reference frame: “space” or “body” (default: “space”)

Returns:

J (numpy.ndarray) – 6×n Jacobian matrix

Structure: J = [J_v; J_ω] where J_v is linear, J_ω is angular velocity Jacobian

Formula (Space): J_s = [Ad_{T₁} S₁, Ad_{T₂} S₂, …, Ad_{Tᵢ} Sᵢ, …]

Formula (Body): J_b = [Ad_{T⁻¹} B₁, Ad_{T⁻¹} B₂, …, Ad_{T⁻¹} Bₙ]

end_effector_velocity(thetalist, dthetalist, frame='space')[source]

Calculate the end effector velocity given the joint angles and joint velocities.

Parameters:

thetalist (list) – A list of joint angles.
dthetalist (list) – A list of joint velocities.
frame (str) – The frame in which the Jacobian is calculated. Valid values are ‘space’ and ‘body’.

Returns:

The end effector velocity.

Return type:

numpy.ndarray

Parameters:

thetalist (array_like) – Joint angles in radians

dthetalist (array_like) – Joint velocities in rad/s

frame (str) – Reference frame: “space” or “body” (default: “space”)

Returns:

V (numpy.ndarray) – 6-element twist vector [vx, vy, vz, ωx, ωy, ωz]

Formula: V = J(θ) θ̇

joint_velocity(thetalist, V_ee, frame='space')[source]

Calculates the joint velocity given the joint positions, end-effector velocity, and frame type.

Parameters:

thetalist (list) – A list of joint positions.
V_ee (array-like) – The end-effector velocity.
frame (str, optional) – The frame type. Defaults to ‘space’.

Returns:

The joint velocity.

Return type:

array-like

Parameters:

thetalist (array_like) – Joint angles in radians

V_ee (array_like) – Desired end-effector velocity twist

frame (str) – Reference frame: “space” or “body” (default: “space”)

Returns:

dthetalist (numpy.ndarray) – Required joint velocities in rad/s

Formula: θ̇ = J⁺(θ) V where J⁺ is Moore-Penrose pseudoinverse

Inverse Kinematics

iterative_inverse_kinematics(T_desired, thetalist0, eomg=1e-06, ev=1e-06, max_iterations=10000, plot_residuals=False, damping=0.02, step_cap=0.3, png_name='ik_residuals.png', weight_orientation=1.0, weight_position=1.0, adaptive_tuning=False, backtracking=False)[source]

Damped-least-squares iterative IK with joint-limit projection and residual plot saved to file (no interactive window).

Features: - Levenberg-Marquardt style adaptive damping - SVD-robust Jacobian solve for near-singular configs - Stagnation detection with perturbation recovery - Improved line search with multiple scales - Best solution tracking

Parameters:

T_desired (numpy.ndarray) – Desired 4×4 transformation matrix

thetalist0 (array_like) – Initial guess for joint angles

eomg (float) – Rotational error tolerance (default: 1e-9)

ev (float) – Translational error tolerance (default: 1e-9)

max_iterations (int) – Maximum iterations (default: 5000)

plot_residuals (bool) – Plot convergence (default: False)

Returns:

thetalist (numpy.ndarray) – Solution joint angles

success (bool) – Convergence status

num_iterations (int) – Iterations used

Algorithm: Newton-Raphson with Damped Least Squares

Update: θₖ₊₁ = θₖ + α J⁺(θₖ) V_error

Convergence: ||V_trans|| < ev AND ||V_rot|| < eomg

Parameters:

T_desired (ndarray[Any, dtype[float64]]) –
thetalist0 (ndarray[Any, dtype[float64]] | List[float]) –
eomg (float) –
ev (float) –
max_iterations (int) –
plot_residuals (bool) –
damping (float) –
step_cap (float) –
png_name (str) –
weight_orientation (float) –
weight_position (float) –
adaptive_tuning (bool) –
backtracking (bool) –

Return type:

Tuple[ndarray[Any, dtype[float64]], bool, int]

update_state(joint_positions, joint_velocities=None)[source]

Updates the internal state of the manipulator.

Parameters:

joint_positions (np.ndarray) – Current joint positions.
joint_velocities (np.ndarray, optional) – Current joint velocities. Default is None.

Return type:

None

forward_kinematics(thetalist, frame='space')[source]

Compute the forward kinematics of a robotic arm using the product of exponentials method.

Parameters:

thetalist (numpy.ndarray) – A 1D array of joint angles in radians.
frame (str, optional) – The frame in which to compute the forward kinematics. Either ‘space’ or ‘body’.

Returns:

The 4x4 transformation matrix representing the end-effector’s pose.

Return type:

numpy.ndarray

end_effector_velocity(thetalist, dthetalist, frame='space')[source]

Calculate the end effector velocity given the joint angles and joint velocities.

Parameters:

thetalist (list) – A list of joint angles.
dthetalist (list) – A list of joint velocities.
frame (str) – The frame in which the Jacobian is calculated. Valid values are ‘space’ and ‘body’.

Returns:

The end effector velocity.

Return type:

numpy.ndarray

jacobian(thetalist, frame='space')[source]

Calculate the Jacobian matrix for the given joint angles.

Parameters:

thetalist (list) – A list of joint angles.
frame (str) – The reference frame for the Jacobian calculation. Valid values are ‘space’ or ‘body’. Defaults to ‘space’.

Returns:

The Jacobian matrix of shape (6, len(thetalist)).

Return type:

numpy.ndarray

iterative_inverse_kinematics(T_desired, thetalist0, eomg=1e-06, ev=1e-06, max_iterations=10000, plot_residuals=False, damping=0.02, step_cap=0.3, png_name='ik_residuals.png', weight_orientation=1.0, weight_position=1.0, adaptive_tuning=False, backtracking=False)[source]

Damped-least-squares iterative IK with joint-limit projection and residual plot saved to file (no interactive window).

Features: - Levenberg-Marquardt style adaptive damping - SVD-robust Jacobian solve for near-singular configs - Stagnation detection with perturbation recovery - Improved line search with multiple scales - Best solution tracking

Parameters:

T_desired (ndarray[Any, dtype[float64]]) –
thetalist0 (ndarray[Any, dtype[float64]] | List[float]) –
eomg (float) –
ev (float) –
max_iterations (int) –
plot_residuals (bool) –
damping (float) –
step_cap (float) –
png_name (str) –
weight_orientation (float) –
weight_position (float) –
adaptive_tuning (bool) –
backtracking (bool) –

Return type:

Tuple[ndarray[Any, dtype[float64]], bool, int]

smart_inverse_kinematics(T_desired, strategy='workspace_heuristic', theta_current=None, T_current=None, cache=None, eomg=1e-06, ev=1e-06, max_iterations=10000, plot_residuals=False, damping=0.02, step_cap=0.3, png_name='ik_residuals.png', weight_orientation=1.0, weight_position=1.0, adaptive_tuning=True, backtracking=True, auto_fallback=True)[source]

Smart inverse kinematics with intelligent initial guess strategies.

Automatically selects initial guess using various strategies for improved convergence. With auto_fallback=True, tries multiple strategies if first fails.

Parameters:

T_desired (ndarray[Any, dtype[float64]]) – Target 4x4 transformation matrix
strategy (str) – Initial guess strategy to use: - ‘workspace_heuristic’: Geometric approximation (default, recommended) - ‘extrapolate’: Extrapolate from current config (for trajectories) - ‘cached’: Use nearest cached solution (requires cache parameter) - ‘random’: Random within joint limits - ‘midpoint’: Midpoint of joint limits
theta_current (ndarray[Any, dtype[float64]] | List[float] | None) – Current joint angles (required for ‘extrapolate’)
T_current (ndarray[Any, dtype[float64]] | None) – Current end-effector pose (required for ‘extrapolate’)
cache (Any | None) – IKInitialGuessCache instance (required for ‘cached’)
auto_fallback (bool) – If True, try other strategies if primary fails (default: True)
iterative_inverse_kinematics() (Other args same as) –
eomg (float) –
ev (float) –
max_iterations (int) –
plot_residuals (bool) –
damping (float) –
step_cap (float) –
png_name (str) –
weight_orientation (float) –
weight_position (float) –
adaptive_tuning (bool) –
backtracking (bool) –

Returns:

Tuple of (theta, success, iterations)

Return type:

Tuple[ndarray[Any, dtype[float64]], bool, int]

robust_inverse_kinematics(T_desired, max_attempts=10, eomg=0.002, ev=0.002, max_iterations=5000, verbose=False)[source]

Robust inverse kinematics with adaptive multi-start strategy.

Tries multiple initial guesses and parameter combinations to maximize success rate. Tracks best solution across all attempts.

Parameters:

T_desired (ndarray[Any, dtype[float64]]) – Target 4x4 transformation matrix
max_attempts (int) – Maximum IK attempts (default: 10)
eomg (float) – Orientation tolerance in radians (default: 2e-3 = 2mrad)
ev (float) – Position tolerance in meters (default: 2e-3 = 2mm)
max_iterations (int) – Max iterations per attempt (default: 5000)
verbose (bool) – Print detailed progress (default: False)

Returns:

Tuple of (theta, success, total_iterations, winning_strategy)

Return type:

Tuple[ndarray[Any, dtype[float64]], bool, int, str]

joint_velocity(thetalist, V_ee, frame='space')[source]

Calculates the joint velocity given the joint positions, end-effector velocity, and frame type.

Parameters:

thetalist (list) – A list of joint positions.
V_ee (array-like) – The end-effector velocity.
frame (str, optional) – The frame type. Defaults to ‘space’.

Returns:

The joint velocity.

Return type:

array-like

end_effector_pose(thetalist)[source]

Computes the end-effector’s position and orientation given joint angles.

Parameters:: thetalist (numpy.ndarray) – A 1D array of joint angles in radians.
Returns:: A 6x1 vector representing the position and orientation (Euler angles) of the end-effector.
Return type:: numpy.ndarray

trac_ik(T_desired, theta0=None, timeout=0.2, eomg=0.0001, ev=0.0001, num_restarts=5, use_parallel=False)[source]

TRAC-IK style inverse kinematics solver.

Uses a DLS-first strategy with SQP fallback and diverse initial guesses. Sequential mode (default) avoids Python GIL contention for best results.

Parameters:

T_desired (ndarray[Any, dtype[float64]]) – Target 4x4 transformation matrix
theta0 (ndarray[Any, dtype[float64]] | List[float] | None) – Initial guess (optional, uses heuristic if None)
timeout (float) – Maximum total solve time in seconds (default: 200ms)
eomg (float) – Orientation tolerance in radians (default: 1e-4)
ev (float) – Position tolerance in meters (default: 1e-4)
num_restarts (int) – Number of initial guesses (default: 5)
use_parallel (bool) – Run DLS+SQP in parallel per guess (default: False)

Returns:

Tuple of (theta, success, solve_time) - theta: Joint configuration (best found if not successful) - success: True if solution within tolerances - solve_time: Actual solve time in seconds

Return type:

Tuple[ndarray[Any, dtype[float64]], bool, float]

Example

>>> # Basic usage
>>> theta, success, time = robot.trac_ik(T_target)
>>> print(f"Solved: {success} in {time*1000:.1f}ms")
>>>
>>> # For real-time control (tight timeout)
>>> theta, success, time = robot.trac_ik(
...     T_target,
...     theta0=current_angles,  # Warm start
...     timeout=0.01,           # 10ms for 100Hz control
...     num_restarts=2
... )

Usage Examples

Basic Setup:

from ManipulaPy.kinematics import SerialManipulator
import numpy as np

# Define robot parameters
M = np.array([[1, 0, 0, 0.5],
              [0, 1, 0, 0.0],
              [0, 0, 1, 0.3],
              [0, 0, 0, 1]])

omega_list = np.array([[0, 0, 1],    # Joint 1: Z-axis rotation
                       [0, 1, 0],    # Joint 2: Y-axis rotation
                       [0, 1, 0]])   # Joint 3: Y-axis rotation

robot = SerialManipulator(M_list=M, omega_list=omega_list)

Forward Kinematics:

# Compute end-effector pose
joint_angles = [0.5, -0.3, 0.8]
T = robot.forward_kinematics(joint_angles, frame="space")

print(f"Position: {T[:3, 3]}")
print(f"Orientation:\n{T[:3, :3]}")

# Get pose as [x, y, z, roll, pitch, yaw]
pose = robot.end_effector_pose(joint_angles)

Jacobian and Velocities:

# Compute Jacobian matrix
J = robot.jacobian(joint_angles, frame="space")
print(f"Jacobian shape: {J.shape}")

# Forward velocity kinematics
joint_velocities = [0.1, 0.2, -0.1]
ee_velocity = robot.end_effector_velocity(joint_angles, joint_velocities)

# Inverse velocity kinematics
desired_velocity = [0.05, 0.0, 0.1, 0.0, 0.0, 0.2]
required_joint_vel = robot.joint_velocity(joint_angles, desired_velocity)

Inverse Kinematics:

# Define target pose
T_target = np.array([[0, -1, 0, 0.3],
                     [1,  0, 0, 0.2],
                     [0,  0, 1, 0.4],
                     [0,  0, 0, 1]])

# Solve inverse kinematics
initial_guess = [0, 0, 0]
solution, success, iterations = robot.iterative_inverse_kinematics(
    T_desired=T_target,
    thetalist0=initial_guess,
    eomg=1e-6,
    ev=1e-6,
    plot_residuals=True
)

if success:
    print(f"IK converged in {iterations} iterations")
    print(f"Solution: {solution}")
else:
    print("IK failed to converge")

State Management:

# Update robot state
current_positions = [0.1, 0.2, 0.3]
current_velocities = [0.05, 0.1, 0.0]

robot.update_state(current_positions, current_velocities)

# Access current state
print(f"Current positions: {robot.joint_positions}")
print(f"Current velocities: {robot.joint_velocities}")

Joint Limits:

# Define joint limits
joint_limits = [(-np.pi, np.pi),      # Joint 1: ±180°
                (-np.pi/2, np.pi/2),   # Joint 2: ±90°
                (-np.pi/4, np.pi/4)]   # Joint 3: ±45°

robot = SerialManipulator(
    M_list=M,
    omega_list=omega_list,
    joint_limits=joint_limits
)

# Limits are enforced during IK solving

Key Features

Product of Exponentials formulation for robust kinematics
Dual frame support (space and body frames) for flexibility
Automatic screw axis computation from joint parameters
Damped least squares IK with singularity handling
Joint limit enforcement during inverse kinematics
Comprehensive velocity kinematics with pseudoinverse
Convergence visualization for debugging IK problems

Mathematical Background

Screw Theory Foundation:

Uses 6D twist vectors to represent joint motion

Exponential coordinates for robust orientation handling

Adjoint transformations for frame conversions

Jacobian Computation:

Space Jacobian: J_s = [Ad_{T₁} S₁, …, Ad_{Tᵢ} Sᵢ, …]

Body Jacobian: J_b computed via inverse transformations

Pseudoinverse used for redundant/singular configurations

Inverse Kinematics:

Newton-Raphson iteration in twist coordinates

Separate convergence criteria for translation and rotation

Adaptive step size for stability (α = 0.058)

Kinematics API Reference

Quick Navigation

SerialManipulator Class

Usage Examples

Key Features

Mathematical Background

See Also