ManipulaPy.kinematics.SerialManipulator
- 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]
- Parameters:
- __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
Methods
__init__(M_list, omega_list[, r_list, ...])Initialize the class with the given parameters.
end_effector_pose(thetalist)Computes the end-effector's position and orientation given joint angles.
end_effector_velocity(thetalist, dthetalist)Calculate the end effector velocity given the joint angles and joint velocities.
forward_kinematics(thetalist[, frame])Compute the forward kinematics of a robotic arm using the product of exponentials method.
iterative_inverse_kinematics(T_desired, ...)Damped-least-squares iterative IK with joint-limit projection and residual plot saved to file (no interactive window).
jacobian(thetalist[, frame])Calculate the Jacobian matrix for the given joint angles.
joint_velocity(thetalist, V_ee[, frame])Calculates the joint velocity given the joint positions, end-effector velocity, and frame type.
update_state(joint_positions[, joint_velocities])Updates the internal state of the manipulator.
- 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:
- end_effector_velocity(thetalist, dthetalist, frame='space')[source]
Calculate the end effector velocity given the joint angles and joint velocities.
- Parameters:
- Returns:
The end effector velocity.
- Return type:
- jacobian(thetalist, frame='space')[source]
Calculate the Jacobian matrix for the given joint angles.
- Parameters:
- Returns:
The Jacobian matrix of shape (6, len(thetalist)).
- Return type:
- 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
- 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:
- 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:
- joint_velocity(thetalist, V_ee, frame='space')[source]
Calculates the joint velocity given the joint positions, end-effector velocity, and frame type.
- 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:
- 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:
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 ... )