Kinematics API Reference๏ƒ

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

Tip

For conceptual explanations, see Kinematics User Guide.

Quick Navigation๏ƒ

SerialManipulator Class๏ƒ

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")

Hybrid Neural Network IK:

import torch
from sklearn.preprocessing import StandardScaler

# Load trained model and scalers
model = torch.load("ik_model.pth")
scaler_X = StandardScaler()  # Load fitted scaler
scaler_y = StandardScaler()  # Load fitted scaler
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

# Solve with neural network initialization
solution, success, iterations = robot.hybrid_inverse_kinematics(
    T_desired=T_target,
    neural_network=model,
    scaler_X=scaler_X,
    scaler_y=scaler_y,
    device=device
)

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

  • Neural network integration for improved IK initialization

  • 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)

See Also๏ƒ