# Dynamics User Guide

This comprehensive guide covers robot dynamics computations in ManipulaPy, optimized for Python 3.10.12.

Note

This guide is written for Python 3.10.12 users and includes version-specific optimizations and performance improvements.

Introduction to Robot Dynamics

Robot dynamics deals with the relationship between forces/torques and motion in robotic systems. Unlike kinematics, which only considers geometric relationships, dynamics incorporates:

  • Mass properties of robot links

  • Inertial forces due to acceleration

  • Gravitational forces acting on the robot

  • External forces applied to the robot

  • Joint torques required for desired motion

Mathematical Background

The fundamental equation of robot dynamics is the Newton-Euler equation:

\[\tau = M(\theta)\ddot{\theta} + C(\theta,\dot{\theta})\dot{\theta} + G(\theta) + J^T(\theta)F_{ext}\]
Where:

  • \(\tau\) = joint torques

  • \(M(\theta)\) = mass/inertia matrix

  • \(C(\theta,\dot{\theta})\) = Coriolis and centrifugal forces

  • \(G(\theta)\) = gravitational forces

  • \(J^T(\theta)F_{ext}\) = external forces mapped to joint space

Key Concepts

Forward Dynamics

Given joint torques, compute joint accelerations: \(\ddot{\theta} = f(\tau, \theta, \dot{\theta})\)

Inverse Dynamics

Given desired motion, compute required torques: \(\tau = f(\theta, \dot{\theta}, \ddot{\theta})\)

Mass Matrix

Represents the robot’s inertial properties and coupling between joints

Velocity-Dependent Forces

Coriolis and centrifugal forces that arise from robot motion

Setting Up Robot Dynamics

Basic Setup from URDF

import numpy as np
from ManipulaPy.urdf_processor import URDFToSerialManipulator
from ManipulaPy.dynamics import ManipulatorDynamics

# Load robot from URDF (automatically extracts inertial properties)
urdf_processor = URDFToSerialManipulator("robot.urdf")
robot = urdf_processor.serial_manipulator
dynamics = urdf_processor.dynamics

print(f"Robot has {len(dynamics.Glist)} links with inertial properties")

Manual Setup

For custom robots or when URDF is not available:

from ManipulaPy.dynamics import ManipulatorDynamics
import numpy as np

# Define robot parameters
M_list = np.eye(4)  # Home configuration
M_list[:3, 3] = [0.5, 0, 0.3]  # End-effector position

# Screw axes in space frame
S_list = np.array([
    [0, 0, 1, 0, 0, 0],      # Joint 1: rotation about z-axis
    [0, -1, 0, -0.1, 0, 0],  # Joint 2: rotation about -y-axis
    [0, -1, 0, -0.1, 0, 0.3], # Joint 3: rotation about -y-axis
]).T

# Inertial properties for each link (6x6 spatial inertia matrices)
Glist = []
for i in range(3):  # 3 links
    G = np.zeros((6, 6))

    # Rotational inertia (upper-left 3x3)
    G[:3, :3] = np.diag([0.1, 0.1, 0.05])  # Ixx, Iyy, Izz

    # Mass (lower-right 3x3)
    mass = 2.0 - i * 0.5  # Decreasing mass towards end-effector
    G[3:, 3:] = mass * np.eye(3)

    Glist.append(G)

# Create dynamics object
dynamics = ManipulatorDynamics(
    M_list=M_list,
    omega_list=S_list[:3, :],  # Rotation axes
    r_list=None,  # Will be computed from S_list
    b_list=None,  # Body frame (optional)
    S_list=S_list,
    B_list=None,  # Will be computed
    Glist=Glist
)

Mass Matrix Computation

The mass matrix represents the robot’s inertial properties and varies with configuration.

Computing Mass Matrix

# Define joint configuration
theta = np.array([0.1, 0.3, -0.2])  # Joint angles in radians

# Compute mass matrix
M = dynamics.mass_matrix(theta)

print(f"Mass matrix shape: {M.shape}")
print(f"Mass matrix:\n{M}")

# Check properties
print(f"Matrix is symmetric: {np.allclose(M, M.T)}")
print(f"Matrix is positive definite: {np.all(np.linalg.eigvals(M) > 0)}")

Configuration Dependence

The mass matrix changes with robot configuration:

import matplotlib.pyplot as plt

# Test different configurations
configurations = np.linspace(-np.pi, np.pi, 50)
condition_numbers = []
determinants = []

for angle in configurations:
    theta = np.array([angle, 0.0, 0.0])
    M = dynamics.mass_matrix(theta)

    condition_numbers.append(np.linalg.cond(M))
    determinants.append(np.linalg.det(M))

# Plot results
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 4))

ax1.plot(configurations, condition_numbers)
ax1.set_xlabel('Joint 1 Angle (rad)')
ax1.set_ylabel('Condition Number')
ax1.set_title('Mass Matrix Conditioning')
ax1.grid(True)

ax2.plot(configurations, determinants)
ax2.set_xlabel('Joint 1 Angle (rad)')
ax2.set_ylabel('Determinant')
ax2.set_title('Mass Matrix Determinant')
ax2.grid(True)

plt.tight_layout()
plt.show()

Caching for Performance

For real-time applications, cache mass matrix computations:

class CachedDynamics:
    def __init__(self, dynamics, tolerance=1e-3):
        self.dynamics = dynamics
        self.tolerance = tolerance
        self.cache = {}

    def mass_matrix_cached(self, theta):
        # Create cache key (rounded configuration)
        key = tuple(np.round(theta / self.tolerance) * self.tolerance)

        if key not in self.cache:
            self.cache[key] = self.dynamics.mass_matrix(theta)

        return self.cache[key]

    def clear_cache(self):
        self.cache.clear()

# Usage
cached_dynamics = CachedDynamics(dynamics)
M = cached_dynamics.mass_matrix_cached(theta)

Velocity-Dependent Forces

Coriolis and centrifugal forces arise from robot motion and joint coupling.

Computing Velocity Forces

# Define joint state
theta = np.array([0.1, 0.3, -0.2])      # Joint positions
theta_dot = np.array([0.5, -0.3, 0.8])  # Joint velocities

# Compute velocity-dependent forces
c = dynamics.velocity_quadratic_forces(theta, theta_dot)

print(f"Velocity forces: {c}")
print(f"Force magnitude: {np.linalg.norm(c)}")

Analyzing Velocity Effects

def analyze_velocity_effects(dynamics, theta, max_velocity=2.0):
    """Analyze how joint velocities affect Coriolis forces."""

    velocities = np.linspace(0, max_velocity, 20)
    force_magnitudes = []

    for vel in velocities:
        # Apply same velocity to all joints
        theta_dot = np.ones(len(theta)) * vel
        c = dynamics.velocity_quadratic_forces(theta, theta_dot)
        force_magnitudes.append(np.linalg.norm(c))

    # Plot results
    plt.figure(figsize=(8, 6))
    plt.plot(velocities, force_magnitudes, 'b-', linewidth=2)
    plt.xlabel('Joint Velocity (rad/s)')
    plt.ylabel('Coriolis Force Magnitude (Nβ‹…m)')
    plt.title('Velocity-Dependent Forces')
    plt.grid(True)
    plt.show()

    return velocities, force_magnitudes

# Analyze for current configuration
analyze_velocity_effects(dynamics, theta)

Centrifugal vs Coriolis

Separate centrifugal (velocityΒ²) and Coriolis (cross-coupling) effects:

def decompose_velocity_forces(dynamics, theta, theta_dot):
    """Decompose velocity forces into centrifugal and Coriolis components."""

    n = len(theta)
    centrifugal = np.zeros(n)
    coriolis = np.zeros(n)

    # Centrifugal forces (diagonal terms)
    for i in range(n):
        theta_dot_i = np.zeros(n)
        theta_dot_i[i] = theta_dot[i]
        c_i = dynamics.velocity_quadratic_forces(theta, theta_dot_i)
        centrifugal += c_i

    # Total velocity forces
    c_total = dynamics.velocity_quadratic_forces(theta, theta_dot)

    # Coriolis forces (off-diagonal coupling)
    coriolis = c_total - centrifugal

    return centrifugal, coriolis

# Example usage
centrifugal, coriolis = decompose_velocity_forces(dynamics, theta, theta_dot)

print(f"Centrifugal forces: {centrifugal}")
print(f"Coriolis forces: {coriolis}")
print(f"Total: {centrifugal + coriolis}")

Gravity Compensation

Gravity forces must be overcome for the robot to maintain its position.

Computing Gravity Forces

# Standard Earth gravity
g = [0, 0, -9.81]  # m/sΒ²

# Compute gravitational forces
gravity_forces = dynamics.gravity_forces(theta, g)

print(f"Gravity forces: {gravity_forces}")
print(f"Total gravity torque: {np.linalg.norm(gravity_forces)} Nβ‹…m")

Configuration Dependence

Gravity forces vary significantly with robot pose:

def gravity_analysis(dynamics, g=[0, 0, -9.81]):
    """Analyze gravity forces across workspace."""

    # Test range of configurations
    angles = np.linspace(-np.pi/2, np.pi/2, 30)
    gravity_magnitudes = []
    configurations = []

    for angle1 in angles[::5]:  # Subsample for speed
        for angle2 in angles[::5]:
            theta = np.array([angle1, angle2, 0.0])
            g_forces = dynamics.gravity_forces(theta, g)

            gravity_magnitudes.append(np.linalg.norm(g_forces))
            configurations.append(theta.copy())

    configurations = np.array(configurations)
    gravity_magnitudes = np.array(gravity_magnitudes)

    # Find maximum gravity configuration
    max_idx = np.argmax(gravity_magnitudes)
    max_config = configurations[max_idx]
    max_gravity = gravity_magnitudes[max_idx]

    print(f"Maximum gravity torque: {max_gravity:.2f} Nβ‹…m")
    print(f"At configuration: {np.degrees(max_config)} degrees")

    return configurations, gravity_magnitudes

# Analyze gravity effects
configs, g_mags = gravity_analysis(dynamics)

Gravity Compensation Control

def gravity_compensation_demo():
    """Demonstrate gravity compensation for robot control."""

    # Simulation parameters
    dt = 0.01  # Time step
    duration = 5.0  # Simulation time
    time_steps = np.arange(0, duration, dt)

    # Initial conditions
    theta = np.array([0.2, 0.5, -0.3])
    theta_dot = np.zeros(3)

    # Storage for results
    positions = []
    velocities = []
    torques = []

    for t in time_steps:
        # Compute gravity compensation torque
        tau_gravity = dynamics.gravity_forces(theta, [0, 0, -9.81])

        # Apply gravity compensation (torque = gravity compensation)
        tau_applied = tau_gravity

        # Simulate dynamics (simplified)
        M = dynamics.mass_matrix(theta)
        c = dynamics.velocity_quadratic_forces(theta, theta_dot)

        # Forward dynamics: M*theta_ddot = tau - c - g
        tau_net = tau_applied - c - tau_gravity  # Should be ~0 for perfect compensation
        theta_ddot = np.linalg.solve(M, tau_net)

        # Integrate
        theta_dot += theta_ddot * dt
        theta += theta_dot * dt

        # Store results
        positions.append(theta.copy())
        velocities.append(theta_dot.copy())
        torques.append(tau_applied.copy())

    # Plot results
    positions = np.array(positions)
    velocities = np.array(velocities)
    torques = np.array(torques)

    fig, axes = plt.subplots(3, 1, figsize=(10, 8))

    # Positions
    for i in range(3):
        axes[0].plot(time_steps, np.degrees(positions[:, i]), label=f'Joint {i+1}')
    axes[0].set_ylabel('Position (degrees)')
    axes[0].set_title('Joint Positions with Gravity Compensation')
    axes[0].legend()
    axes[0].grid(True)

    # Velocities
    for i in range(3):
        axes[1].plot(time_steps, velocities[:, i], label=f'Joint {i+1}')
    axes[1].set_ylabel('Velocity (rad/s)')
    axes[1].set_title('Joint Velocities')
    axes[1].legend()
    axes[1].grid(True)

    # Torques
    for i in range(3):
        axes[2].plot(time_steps, torques[:, i], label=f'Joint {i+1}')
    axes[2].set_ylabel('Torque (Nβ‹…m)')
    axes[2].set_xlabel('Time (s)')
    axes[2].set_title('Gravity Compensation Torques')
    axes[2].legend()
    axes[2].grid(True)

    plt.tight_layout()
    plt.show()

# Run gravity compensation demo
gravity_compensation_demo()

Forward and Inverse Dynamics

Forward Dynamics

Given torques, compute resulting accelerations:

# Define robot state and inputs
theta = np.array([0.1, 0.3, -0.2])
theta_dot = np.array([0.5, -0.3, 0.8])
tau = np.array([10.0, 5.0, 2.0])  # Applied torques
g = [0, 0, -9.81]  # Gravity
F_ext = np.zeros(6)  # No external forces

# Compute forward dynamics
theta_ddot = dynamics.forward_dynamics(theta, theta_dot, tau, g, F_ext)

print(f"Applied torques: {tau}")
print(f"Resulting accelerations: {theta_ddot}")

Inverse Dynamics

Given desired motion, compute required torques:

# Define desired motion
theta = np.array([0.1, 0.3, -0.2])
theta_dot = np.array([0.5, -0.3, 0.8])
theta_ddot_desired = np.array([1.0, -0.5, 0.3])  # Desired accelerations

# Compute required torques
tau_required = dynamics.inverse_dynamics(
    theta, theta_dot, theta_ddot_desired, g, F_ext
)

print(f"Desired accelerations: {theta_ddot_desired}")
print(f"Required torques: {tau_required}")

# Verify with forward dynamics
theta_ddot_check = dynamics.forward_dynamics(
    theta, theta_dot, tau_required, g, F_ext
)

error = np.linalg.norm(theta_ddot_check - theta_ddot_desired)
print(f"Verification error: {error:.6f}")

Trajectory Dynamics

Compute dynamics along a trajectory:

def trajectory_dynamics_analysis():
    """Analyze dynamics along a planned trajectory."""

    # Generate simple trajectory (sinusoidal motion)
    t_final = 5.0
    dt = 0.01
    time_steps = np.arange(0, t_final, dt)

    # Trajectory parameters
    amplitude = np.array([0.5, 0.3, 0.2])
    frequency = np.array([0.5, 0.8, 1.2])

    # Generate trajectory
    trajectory = []
    velocities = []
    accelerations = []

    for t in time_steps:
        # Position (sinusoidal)
        pos = amplitude * np.sin(2 * np.pi * frequency * t)

        # Velocity (derivative)
        vel = amplitude * 2 * np.pi * frequency * np.cos(2 * np.pi * frequency * t)

        # Acceleration (second derivative)
        acc = -amplitude * (2 * np.pi * frequency)**2 * np.sin(2 * np.pi * frequency * t)

        trajectory.append(pos)
        velocities.append(vel)
        accelerations.append(acc)

    trajectory = np.array(trajectory)
    velocities = np.array(velocities)
    accelerations = np.array(accelerations)

    # Compute required torques along trajectory
    torques = []
    for i, t in enumerate(time_steps):
        tau = dynamics.inverse_dynamics(
            trajectory[i], velocities[i], accelerations[i],
            [0, 0, -9.81], np.zeros(6)
        )
        torques.append(tau)

    torques = np.array(torques)

    # Analyze results
    fig, axes = plt.subplots(2, 2, figsize=(12, 8))

    # Trajectory
    for j in range(3):
        axes[0, 0].plot(time_steps, np.degrees(trajectory[:, j]), label=f'Joint {j+1}')
    axes[0, 0].set_title('Joint Trajectories')
    axes[0, 0].set_ylabel('Position (degrees)')
    axes[0, 0].legend()
    axes[0, 0].grid(True)

    # Velocities
    for j in range(3):
        axes[0, 1].plot(time_steps, velocities[:, j], label=f'Joint {j+1}')
    axes[0, 1].set_title('Joint Velocities')
    axes[0, 1].set_ylabel('Velocity (rad/s)')
    axes[0, 1].legend()
    axes[0, 1].grid(True)

    # Accelerations
    for j in range(3):
        axes[1, 0].plot(time_steps, accelerations[:, j], label=f'Joint {j+1}')
    axes[1, 0].set_title('Joint Accelerations')
    axes[1, 0].set_ylabel('Acceleration (rad/sΒ²)')
    axes[1, 0].set_xlabel('Time (s)')
    axes[1, 0].legend()
    axes[1, 0].grid(True)

    # Required torques
    for j in range(3):
        axes[1, 1].plot(time_steps, torques[:, j], label=f'Joint {j+1}')
    axes[1, 1].set_title('Required Torques')
    axes[1, 1].set_ylabel('Torque (Nβ‹…m)')
    axes[1, 1].set_xlabel('Time (s)')
    axes[1, 1].legend()
    axes[1, 1].grid(True)

    plt.tight_layout()
    plt.show()

    # Print statistics
    max_torques = np.max(np.abs(torques), axis=0)
    print(f"Maximum torques required: {max_torques}")
    print(f"Peak total torque: {np.max(np.linalg.norm(torques, axis=1)):.2f} Nβ‹…m")

# Run trajectory analysis
trajectory_dynamics_analysis()

Performance Optimization

GPU Acceleration

For large-scale simulations, use GPU acceleration:

try:
    import cupy as cp

    def gpu_dynamics_demo():
        """Demonstrate GPU-accelerated dynamics computations."""

        # Generate batch of configurations
        n_configs = 1000
        configs = cp.random.uniform(-cp.pi, cp.pi, (n_configs, 3))

        # Measure performance
        import time

        # CPU computation
        start_time = time.time()
        cpu_results = []
        for i in range(100):  # Smaller sample for CPU
            config = cp.asnumpy(configs[i])
            M = dynamics.mass_matrix(config)
            cpu_results.append(M)
        cpu_time = time.time() - start_time

        print(f"CPU time for 100 configurations: {cpu_time:.3f} seconds")
        print(f"CPU rate: {100/cpu_time:.1f} computations/second")

        # For full GPU implementation, you would need CUDA kernels
        # This is a simplified example showing the concept

except ImportError:
    print("CuPy not available - GPU acceleration not supported")

Parallel Processing

Use multiprocessing for CPU parallelization:

from multiprocessing import Pool
import functools

def parallel_dynamics_computation(dynamics, configurations):
    """Compute dynamics for multiple configurations in parallel."""

    def compute_single_config(config):
        M = dynamics.mass_matrix(config)
        g_forces = dynamics.gravity_forces(config, [0, 0, -9.81])
        return {
            'config': config,
            'mass_matrix': M,
            'gravity_forces': g_forces,
            'condition_number': np.linalg.cond(M)
        }

    # Create partial function with dynamics object
    compute_func = functools.partial(compute_single_config)

    # Use multiprocessing
    with Pool() as pool:
        results = pool.map(compute_func, configurations)

    return results

# Example usage
test_configs = [
    np.array([0.1, 0.3, -0.2]),
    np.array([0.5, -0.2, 0.4]),
    np.array([-0.3, 0.6, 0.1]),
    np.array([0.8, -0.1, -0.3])
]

# Parallel computation
import time
start_time = time.time()
parallel_results = parallel_dynamics_computation(dynamics, test_configs)
parallel_time = time.time() - start_time

print(f"Parallel computation time: {parallel_time:.3f} seconds")
print(f"Number of configurations processed: {len(parallel_results)}")

Advanced Topics

Energy Analysis

Analyze kinetic and potential energy:

def energy_analysis(dynamics, theta, theta_dot, g=[0, 0, -9.81]):
    """Compute kinetic and potential energy of the robot."""

    # Kinetic energy: T = 0.5 * theta_dot^T * M * theta_dot
    M = dynamics.mass_matrix(theta)
    kinetic_energy = 0.5 * theta_dot.T @ M @ theta_dot

    # Potential energy (gravitational)
    # This is a simplified calculation - full implementation would
    # require link positions and masses
    potential_energy = 0.0  # Placeholder

    total_energy = kinetic_energy + potential_energy

    return {
        'kinetic': kinetic_energy,
        'potential': potential_energy,
        'total': total_energy
    }

# Example usage
energy = energy_analysis(dynamics, theta, theta_dot)
print(f"Kinetic energy: {energy['kinetic']:.3f} J")
print(f"Total energy: {energy['total']:.3f} J")

Linearization

Linearize dynamics about an operating point:

def linearize_dynamics(dynamics, theta_0, theta_dot_0, epsilon=1e-6):
    """Linearize robot dynamics about operating point."""

    n = len(theta_0)

    # Compute nominal values
    M_0 = dynamics.mass_matrix(theta_0)
    c_0 = dynamics.velocity_quadratic_forces(theta_0, theta_dot_0)
    g_0 = dynamics.gravity_forces(theta_0, [0, 0, -9.81])

    # Compute Jacobians numerically
    dM_dtheta = np.zeros((n, n, n))
    dc_dtheta = np.zeros((n, n))
    dg_dtheta = np.zeros((n, n))

    for i in range(n):
        # Perturb theta
        theta_plus = theta_0.copy()
        theta_plus[i] += epsilon
        theta_minus = theta_0.copy()
        theta_minus[i] -= epsilon

        # Compute derivatives
        M_plus = dynamics.mass_matrix(theta_plus)
        M_minus = dynamics.mass_matrix(theta_minus)
        dM_dtheta[:, :, i] = (M_plus - M_minus) / (2 * epsilon)

        c_plus = dynamics.velocity_quadratic_forces(theta_plus, theta_dot_0)
        c_minus = dynamics.velocity_quadratic_forces(theta_minus, theta_dot_0)
        dc_dtheta[:, i] = (c_plus - c_minus) / (2 * epsilon)

        g_plus = dynamics.gravity_forces(theta_plus, [0, 0, -9.81])
        g_minus = dynamics.gravity_forces(theta_minus, [0, 0, -9.81])
        dg_dtheta[:, i] = (g_plus - g_minus) / (2 * epsilon)

    return {
        'M_0': M_0, 'c_0': c_0, 'g_0': g_0,
        'dM_dtheta': dM_dtheta, 'dc_dtheta': dc_dtheta, 'dg_dtheta': dg_dtheta
    }

# Example usage
operating_point = np.array([0.0, 0.0, 0.0])  # Home position
operating_velocity = np.zeros(3)

linearization = linearize_dynamics(dynamics, operating_point, operating_velocity)
print(f"Mass matrix at operating point:\n{linearization['M_0']}")

Model Identification

Estimate robot parameters from experimental data:

def parameter_identification_demo():
    """Demonstrate robot parameter identification."""

    # Simulate noisy measurements
    np.random.seed(42)
    n_samples = 100

    # Generate test trajectories
    test_data = []
    for i in range(n_samples):
        theta = np.random.uniform(-0.5, 0.5, 3)
        theta_dot = np.random.uniform(-1.0, 1.0, 3)
        theta_ddot = np.random.uniform(-2.0, 2.0, 3)

        # Compute "measured" torques with noise
        tau_true = dynamics.inverse_dynamics(
            theta, theta_dot, theta_ddot, [0, 0, -9.81], np.zeros(6)
        )

        # Add measurement noise
        noise = np.random.normal(0, 0.1, 3)
        tau_measured = tau_true + noise

        test_data.append({
            'theta': theta,
            'theta_dot': theta_dot,
            'theta_ddot': theta_ddot,
            'tau_measured': tau_measured,
            'tau_true': tau_true
        })

    # Simple parameter identification (mass scaling factors)
    def objective_function(params):
        """Objective function for parameter identification."""
        mass_scale_factors = params

        total_error = 0.0
        for data in test_data:
            # Scale the inertia matrices
            scaled_Glist = []
            for i, G in enumerate(dynamics.Glist):
                G_scaled = G.copy()
                G_scaled[3:, 3:] *= mass_scale_factors[i]  # Scale mass components
                scaled_Glist.append(G_scaled)

            # Create temporary dynamics with scaled parameters
            temp_dynamics = ManipulatorDynamics(
                dynamics.M_list, dynamics.omega_list, dynamics.r_list,
                dynamics.b_list, dynamics.S_list, dynamics.B_list,
                scaled_Glist
            )

            # Compute predicted torques
            tau_predicted = temp_dynamics.inverse_dynamics(
                data['theta'], data['theta_dot'], data['theta_ddot'],
                [0, 0, -9.81], np.zeros(6)
            )

            # Accumulate error
            error = np.linalg.norm(tau_predicted - data['tau_measured'])
            total_error += error

        return total_error

    # Optimize parameters (simplified example)
    from scipy.optimize import minimize

    initial_guess = np.ones(len(dynamics.Glist))
    bounds = [(0.1, 10.0)] * len(dynamics.Glist)  # Mass can vary 10x

    result = minimize(objective_function, initial_guess, bounds=bounds)

    print(f"Identified mass scale factors: {result.x}")
    print(f"Optimization success: {result.success}")
    print(f"Final error: {result.fun:.3f}")

# Run parameter identification
parameter_identification_demo()

Manipulator Inertia Ellipsoid

Visualize the manipulator’s inertial characteristics:

def plot_inertia_ellipsoid(dynamics, theta):
    """Plot the manipulator inertia ellipsoid."""

    # Compute mass matrix
    M = dynamics.mass_matrix(theta)

    # Eigenvalue decomposition
    eigenvals, eigenvecs = np.linalg.eigh(M)

    # Create ellipsoid points
    u = np.linspace(0, 2 * np.pi, 50)
    v = np.linspace(0, np.pi, 25)

    # Unit sphere
    x_sphere = np.outer(np.cos(u), np.sin(v))
    y_sphere = np.outer(np.sin(u), np.sin(v))
    z_sphere = np.outer(np.ones(np.size(u)), np.cos(v))

    # Transform to ellipsoid
    points = np.stack([x_sphere.flatten(), y_sphere.flatten(), z_sphere.flatten()])

    # Apply eigenvalue scaling and rotation
    if len(eigenvals) >= 3:
        scaling = np.diag(1.0 / np.sqrt(eigenvals[:3]))
        rotation = eigenvecs[:3, :3]
        transform = rotation @ scaling

        transformed_points = transform @ points

        x_ellipsoid = transformed_points[0].reshape(x_sphere.shape)
        y_ellipsoid = transformed_points[1].reshape(y_sphere.shape)
        z_ellipsoid = transformed_points[2].reshape(z_sphere.shape)

        # Plot
        fig = plt.figure(figsize=(10, 8))
        ax = fig.add_subplot(111, projection='3d')

        ax.plot_surface(x_ellipsoid, y_ellipsoid, z_ellipsoid,
                       alpha=0.7, cmap='viridis')

        ax.set_xlabel('Inertia Direction 1')
        ax.set_ylabel('Inertia Direction 2')
        ax.set_zlabel('Inertia Direction 3')
        ax.set_title(f'Manipulator Inertia Ellipsoid at ΞΈ = {np.degrees(theta):.1f}Β°')

        plt.show()

        # Print eigenvalues
        print(f"Inertia eigenvalues: {eigenvals[:3]}")
        print(f"Condition number: {np.max(eigenvals[:3]) / np.min(eigenvals[:3]):.2f}")

# Example usage
plot_inertia_ellipsoid(dynamics, theta)

Dynamic Manipulability

Analyze the robot’s dynamic manipulability:

def dynamic_manipulability_analysis(dynamics, configurations):
    """Analyze dynamic manipulability across configurations."""

    manipulabilities = []
    condition_numbers = []

    for config in configurations:
        # Compute mass matrix
        M = dynamics.mass_matrix(config)

        # Compute Jacobian (assuming SerialManipulator interface)
        # J = robot.jacobian(config)  # This would need robot instance

        # For now, use mass matrix properties
        eigenvals = np.linalg.eigvals(M)

        # Dynamic manipulability (simplified)
        manip = np.prod(eigenvals) ** (1.0 / len(eigenvals))
        manipulabilities.append(manip)

        # Condition number
        cond_num = np.max(eigenvals) / np.min(eigenvals)
        condition_numbers.append(cond_num)

    return np.array(manipulabilities), np.array(condition_numbers)

# Test across workspace
test_configs = []
for i in range(20):
    config = np.random.uniform(-np.pi/2, np.pi/2, 3)
    test_configs.append(config)

manip_vals, cond_nums = dynamic_manipulability_analysis(dynamics, test_configs)

# Plot results
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 5))

ax1.plot(manip_vals, 'b-o')
ax1.set_title('Dynamic Manipulability')
ax1.set_ylabel('Manipulability Index')
ax1.set_xlabel('Configuration Index')
ax1.grid(True)

ax2.plot(cond_nums, 'r-o')
ax2.set_title('Mass Matrix Conditioning')
ax2.set_ylabel('Condition Number')
ax2.set_xlabel('Configuration Index')
ax2.grid(True)

plt.tight_layout()
plt.show()

Real-World Applications

Robot Control Integration

Integrate dynamics with control systems:

class DynamicsBasedController:
    """Example controller using robot dynamics."""

    def __init__(self, dynamics):
        self.dynamics = dynamics
        self.gravity = [0, 0, -9.81]
        self.integral_error = None

    def computed_torque_control(self, theta, theta_dot, theta_d, theta_dot_d,
                              theta_ddot_d, Kp, Kd):
        """Computed torque control using exact dynamics."""

        # Tracking errors
        e = theta_d - theta
        e_dot = theta_dot_d - theta_dot

        # Desired acceleration with feedback
        theta_ddot_cmd = theta_ddot_d + Kp @ e + Kd @ e_dot

        # Compute required torques using inverse dynamics
        tau = self.dynamics.inverse_dynamics(
            theta, theta_dot, theta_ddot_cmd, self.gravity, np.zeros(6)
        )

        return tau

    def pid_with_gravity_compensation(self, theta, theta_dot, theta_d,
                                    Kp, Ki, Kd, dt):
        """PID control with gravity compensation."""

        # Initialize integral error if needed
        if self.integral_error is None:
            self.integral_error = np.zeros_like(theta)

        # Tracking errors
        e = theta_d - theta
        e_dot = -theta_dot  # Assuming zero desired velocity

        # Update integral error
        self.integral_error += e * dt

        # PID control law
        tau_pid = Kp @ e + Ki @ self.integral_error + Kd @ e_dot

        # Add gravity compensation
        tau_gravity = self.dynamics.gravity_forces(theta, self.gravity)

        return tau_pid + tau_gravity

    def adaptive_control(self, theta, theta_dot, theta_d, theta_dot_d,
                       theta_ddot_d, Kp, Kd, adaptation_gain=0.1):
        """Adaptive control with parameter uncertainty."""

        # This is a simplified adaptive controller
        # Real implementation would include parameter adaptation

        # Use nominal dynamics for feedforward
        tau_ff = self.dynamics.inverse_dynamics(
            theta_d, theta_dot_d, theta_ddot_d, self.gravity, np.zeros(6)
        )

        # Add feedback component
        e = theta_d - theta
        e_dot = theta_dot_d - theta_dot
        tau_fb = Kp @ e + Kd @ e_dot

        return tau_ff + tau_fb

    def impedance_control(self, theta, theta_dot, theta_d, F_ext,
                        M_des, B_des, K_des):
        """Impedance control for interaction tasks."""

        # Desired impedance dynamics:
        # M_des * ddtheta + B_des * dtheta + K_des * (theta - theta_d) = F_ext

        # Compute desired acceleration
        e = theta - theta_d
        theta_ddot_d = np.linalg.solve(M_des, F_ext - B_des @ theta_dot - K_des @ e)

        # Use computed torque control to achieve desired acceleration
        tau = self.dynamics.inverse_dynamics(
            theta, theta_dot, theta_ddot_d, self.gravity, np.zeros(6)
        )

        return tau

# Example usage
controller = DynamicsBasedController(dynamics)

# Control parameters
Kp = np.diag([100, 80, 60])  # Proportional gains
Kd = np.diag([20, 15, 10])   # Derivative gains
Ki = np.diag([5, 4, 3])      # Integral gains

# Desired trajectory point
theta_desired = np.array([0.5, 0.3, -0.2])
theta_dot_desired = np.array([0.1, -0.05, 0.08])
theta_ddot_desired = np.array([0.0, 0.0, 0.0])

# Current state
theta_current = np.array([0.4, 0.25, -0.15])
theta_dot_current = np.array([0.08, -0.03, 0.06])

# Compute control torques
tau_control = controller.computed_torque_control(
    theta_current, theta_dot_current,
    theta_desired, theta_dot_desired, theta_ddot_desired,
    Kp, Kd
)

print(f"Control torques: {tau_control}")

Simulation Integration

Integrate with physics simulators:

def complete_simulation_example():
    """Complete simulation example with dynamics and control."""

    # Simulation parameters
    dt = 0.001  # 1ms time step for stability
    t_final = 3.0
    time_steps = np.arange(0, t_final, dt)

    # Initial conditions
    theta = np.array([0.1, 0.2, -0.1])
    theta_dot = np.zeros(3)

    # Control parameters
    controller = DynamicsBasedController(dynamics)
    Kp = np.diag([100, 80, 60])
    Ki = np.diag([10, 8, 6])
    Kd = np.diag([20, 15, 10])

    # Target trajectory (figure-8 in joint space)
    def target_trajectory(t):
        amplitude = np.array([0.3, 0.2, 0.15])
        freq1 = 0.5
        freq2 = 1.0

        theta_d = amplitude * np.array([
            np.sin(2 * np.pi * freq1 * t),
            np.sin(2 * np.pi * freq2 * t),
            np.sin(2 * np.pi * freq1 * t) * np.cos(2 * np.pi * freq2 * t)
        ])

        theta_dot_d = amplitude * np.array([
            2 * np.pi * freq1 * np.cos(2 * np.pi * freq1 * t),
            2 * np.pi * freq2 * np.cos(2 * np.pi * freq2 * t),
            2 * np.pi * freq1 * np.cos(2 * np.pi * freq1 * t) * np.cos(2 * np.pi * freq2 * t) -
            2 * np.pi * freq2 * np.sin(2 * np.pi * freq1 * t) * np.sin(2 * np.pi * freq2 * t)
        ])

        return theta_d, theta_dot_d

    # Storage for results
    positions = []
    velocities = []
    accelerations = []
    torques = []
    errors = []
    desired_positions = []

    # Simulation loop
    for i, t in enumerate(time_steps):
        # Get desired trajectory
        theta_d, theta_dot_d = target_trajectory(t)

        # Compute control torques
        tau_control = controller.pid_with_gravity_compensation(
            theta, theta_dot, theta_d, Kp, Ki, Kd, dt
        )

        # Add small disturbances (realistic simulation)
        disturbance = np.random.normal(0, 0.01, 3) if i % 100 == 0 else np.zeros(3)
        tau_total = tau_control + disturbance

        # Apply actuator limits (realistic)
        tau_max = np.array([50, 40, 30])  # Maximum torques
        tau_total = np.clip(tau_total, -tau_max, tau_max)

        # Forward dynamics
        theta_ddot = dynamics.forward_dynamics(
            theta, theta_dot, tau_total, [0, 0, -9.81], np.zeros(6)
        )

        # Numerical integration (4th-order Runge-Kutta for accuracy)
        def dynamics_rhs(state, tau):
            th, th_dot = state[:3], state[3:]
            th_ddot = dynamics.forward_dynamics(th, th_dot, tau, [0, 0, -9.81], np.zeros(6))
            return np.concatenate([th_dot, th_ddot])

        # Current state
        state = np.concatenate([theta, theta_dot])

        # RK4 integration
        k1 = dynamics_rhs(state, tau_total)
        k2 = dynamics_rhs(state + 0.5 * dt * k1, tau_total)
        k3 = dynamics_rhs(state + 0.5 * dt * k2, tau_total)
        k4 = dynamics_rhs(state + dt * k3, tau_total)

        state_new = state + (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4)

        theta = state_new[:3]
        theta_dot = state_new[3:]

        # Apply joint limits
        theta_limits = np.array([[-np.pi, np.pi], [-np.pi/2, np.pi/2], [-np.pi, np.pi]])
        for j in range(3):
            if theta[j] < theta_limits[j, 0]:
                theta[j] = theta_limits[j, 0]
                theta_dot[j] = 0  # Stop at limit
            elif theta[j] > theta_limits[j, 1]:
                theta[j] = theta_limits[j, 1]
                theta_dot[j] = 0  # Stop at limit

        # Store results
        positions.append(theta.copy())
        velocities.append(theta_dot.copy())
        accelerations.append(theta_ddot.copy())
        torques.append(tau_total.copy())
        desired_positions.append(theta_d.copy())

        # Compute tracking error
        error = np.linalg.norm(theta - theta_d)
        errors.append(error)

    # Convert to arrays
    positions = np.array(positions)
    velocities = np.array(velocities)
    accelerations = np.array(accelerations)
    torques = np.array(torques)
    desired_positions = np.array(desired_positions)
    errors = np.array(errors)

    # Plot comprehensive results
    fig, axes = plt.subplots(2, 3, figsize=(15, 10))

    # Joint positions vs desired
    for i in range(3):
        axes[0, 0].plot(time_steps, np.degrees(positions[:, i]),
                       label=f'Joint {i+1} Actual', linewidth=2)
        axes[0, 0].plot(time_steps, np.degrees(desired_positions[:, i]),
                       '--', label=f'Joint {i+1} Desired', alpha=0.7)
    axes[0, 0].set_ylabel('Position (degrees)')
    axes[0, 0].set_title('Joint Positions')
    axes[0, 0].legend()
    axes[0, 0].grid(True)

    # Joint velocities
    for i in range(3):
        axes[0, 1].plot(time_steps, velocities[:, i], label=f'Joint {i+1}')
    axes[0, 1].set_ylabel('Velocity (rad/s)')
    axes[0, 1].set_title('Joint Velocities')
    axes[0, 1].legend()
    axes[0, 1].grid(True)

    # Control torques
    for i in range(3):
        axes[0, 2].plot(time_steps, torques[:, i], label=f'Joint {i+1}')
    axes[0, 2].set_ylabel('Torque (Nβ‹…m)')
    axes[0, 2].set_title('Control Torques')
    axes[0, 2].legend()
    axes[0, 2].grid(True)

    # Tracking error
    axes[1, 0].plot(time_steps, np.degrees(errors), 'r-', linewidth=2)
    axes[1, 0].set_ylabel('Error (degrees)')
    axes[1, 0].set_xlabel('Time (s)')
    axes[1, 0].set_title('Tracking Error')
    axes[1, 0].grid(True)

    # Joint accelerations
    for i in range(3):
        axes[1, 1].plot(time_steps, accelerations[:, i], label=f'Joint {i+1}')
    axes[1, 1].set_ylabel('Acceleration (rad/sΒ²)')
    axes[1, 1].set_xlabel('Time (s)')
    axes[1, 1].set_title('Joint Accelerations')
    axes[1, 1].legend()
    axes[1, 1].grid(True)

    # Phase plot (position vs velocity for first joint)
    axes[1, 2].plot(np.degrees(positions[:, 0]), velocities[:, 0], 'b-', alpha=0.7)
    axes[1, 2].set_xlabel('Joint 1 Position (degrees)')
    axes[1, 2].set_ylabel('Joint 1 Velocity (rad/s)')
    axes[1, 2].set_title('Phase Plot (Joint 1)')
    axes[1, 2].grid(True)

    plt.tight_layout()
    plt.show()

    # Print performance statistics
    final_error = np.degrees(errors[-1])
    max_error = np.degrees(np.max(errors))
    rms_error = np.degrees(np.sqrt(np.mean(errors**2)))

    print(f"\nSimulation Performance:")
    print(f"Final tracking error: {final_error:.2f} degrees")
    print(f"Maximum tracking error: {max_error:.2f} degrees")
    print(f"RMS tracking error: {rms_error:.2f} degrees")

    max_torques = np.max(np.abs(torques), axis=0)
    print(f"Maximum torques used: {max_torques} Nβ‹…m")

    return {
        'time': time_steps,
        'positions': positions,
        'velocities': velocities,
        'torques': torques,
        'errors': errors
    }

# Run complete simulation
simulation_results = complete_simulation_example()

Force and Torque Analysis

Analyze required forces and torques for different tasks:

def force_torque_analysis():
    """Analyze force and torque requirements for different tasks."""

    # Define task scenarios
    scenarios = {
        'hover': {
            'theta': np.array([0.0, np.pi/4, -np.pi/4]),
            'theta_dot': np.zeros(3),
            'theta_ddot': np.zeros(3),
            'description': 'Static hovering against gravity'
        },
        'fast_motion': {
            'theta': np.array([0.2, 0.3, -0.1]),
            'theta_dot': np.array([2.0, -1.5, 1.0]),
            'theta_ddot': np.array([3.0, -2.0, 2.5]),
            'description': 'Fast dynamic motion'
        },
        'external_force': {
            'theta': np.array([0.1, 0.4, -0.2]),
            'theta_dot': np.array([0.5, -0.3, 0.2]),
            'theta_ddot': np.array([0.0, 0.0, 0.0]),
            'F_ext': np.array([10, 5, -8, 0, 0, 2]),
            'description': 'Interacting with environment'
        }
    }

    print("Force and Torque Analysis")
    print("=" * 50)

    for name, scenario in scenarios.items():
        print(f"\nScenario: {scenario['description']}")
        print("-" * 30)

        # Extract parameters
        theta = scenario['theta']
        theta_dot = scenario['theta_dot']
        theta_ddot = scenario['theta_ddot']
        F_ext = scenario.get('F_ext', np.zeros(6))

        # Compute required torques
        tau_total = dynamics.inverse_dynamics(
            theta, theta_dot, theta_ddot, [0, 0, -9.81], F_ext
        )

        # Break down torque components
        tau_inertial = dynamics.mass_matrix(theta) @ theta_ddot
        tau_coriolis = dynamics.velocity_quadratic_forces(theta, theta_dot)
        tau_gravity = dynamics.gravity_forces(theta, [0, 0, -9.81])

        # Compute external force contribution
        if hasattr(dynamics, 'jacobian'):
            # This would need robot instance - simplified here
            tau_external = np.zeros(3)  # Placeholder
        else:
            tau_external = np.zeros(3)

        print(f"Joint angles: {np.degrees(theta):.1f} degrees")
        print(f"Joint velocities: {theta_dot:.2f} rad/s")
        print(f"Joint accelerations: {theta_ddot:.2f} rad/sΒ²")
        print(f"External forces: {F_ext[:3]:.1f} N, {F_ext[3:]:.2f} Nβ‹…m")
        print()
        print("Torque breakdown:")
        print(f"  Inertial:   {tau_inertial:.2f} Nβ‹…m")
        print(f"  Coriolis:   {tau_coriolis:.2f} Nβ‹…m")
        print(f"  Gravity:    {tau_gravity:.2f} Nβ‹…m")
        print(f"  External:   {tau_external:.2f} Nβ‹…m")
        print(f"  Total:      {tau_total:.2f} Nβ‹…m")
        print(f"  Magnitude:  {np.linalg.norm(tau_total):.2f} Nβ‹…m")

        # Check against typical actuator limits
        actuator_limits = np.array([100, 80, 60])  # Typical limits
        safety_factor = tau_total / actuator_limits

        if np.any(np.abs(safety_factor) > 0.8):
            print(f"  ⚠️  High torque demand: {np.max(np.abs(safety_factor)):.1%} of limits")
        else:
            print(f"  βœ… Safe torque levels: {np.max(np.abs(safety_factor)):.1%} of limits")

# Run force/torque analysis
force_torque_analysis()

Troubleshooting

Common Issues

Numerical Instability

def check_numerical_stability(dynamics, theta):
    """Check for potential numerical issues."""

    print("Numerical Stability Check")
    print("=" * 40)

    M = dynamics.mass_matrix(theta)

    # Check condition number
    cond_num = np.linalg.cond(M)
    print(f"Mass matrix condition number: {cond_num:.2e}")

    if cond_num > 1e12:
        print("⚠️ Warning: Poor conditioning - near singularity")
        print("   Recommendations:")
        print("   - Check robot configuration")
        print("   - Verify joint limits")
        print("   - Consider regularization")
    elif cond_num > 1e6:
        print("⚠️ Caution: Moderate conditioning issues")
    else:
        print("βœ… Good conditioning")

    # Check positive definiteness
    eigenvals = np.linalg.eigvals(M)
    min_eigenval = np.min(eigenvals)
    print(f"Minimum eigenvalue: {min_eigenval:.6f}")

    if min_eigenval <= 0:
        print("❌ Error: Mass matrix not positive definite")
        print("   This indicates a fundamental problem with dynamics")
    elif min_eigenval < 1e-6:
        print("⚠️ Warning: Very small eigenvalue - check for singularities")
    else:
        print("βœ… Matrix is positive definite")

    # Check symmetry
    symmetry_error = np.max(np.abs(M - M.T))
    print(f"Symmetry error: {symmetry_error:.2e}")

    if symmetry_error > 1e-10:
        print("⚠️ Warning: Mass matrix not symmetric")
        print("   This may indicate implementation errors")
    else:
        print("βœ… Matrix is symmetric")

    # Check for NaN or infinite values
    if not np.all(np.isfinite(M)):
        print("❌ Error: Mass matrix contains NaN or infinite values")
        print("   Check input angles and robot parameters")
    else:
        print("βœ… All matrix elements are finite")

# Check current configuration
check_numerical_stability(dynamics, theta)

Performance Issues

def performance_diagnostics():
    """Diagnose performance issues in dynamics computations."""

    import time

    # Test configurations
    test_configs = [
        np.array([0.0, 0.0, 0.0]),      # Home position
        np.array([0.5, 0.3, -0.2]),     # Random position
        np.array([1.2, -0.8, 0.9]),     # Near limits
        np.array([-0.7, 1.1, 0.4])      # Another configuration
    ]

    print("Performance Diagnostics")
    print("=" * 40)

    for i, config in enumerate(test_configs):
        print(f"\nConfiguration {i+1}: {np.degrees(config):.1f}Β°")

        # Mass matrix computation
        start = time.time()
        for _ in range(100):
            M = dynamics.mass_matrix(config)
        mass_time = (time.time() - start) / 100

        # Velocity forces computation
        theta_dot = np.array([0.1, -0.2, 0.3])
        start = time.time()
        for _ in range(100):
            c = dynamics.velocity_quadratic_forces(config, theta_dot)
        velocity_time = (time.time() - start) / 100

        # Gravity forces computation
        start = time.time()
        for _ in range(100):
            g = dynamics.gravity_forces(config, [0, 0, -9.81])
        gravity_time = (time.time() - start) / 100

        print(f"  Mass matrix:     {mass_time*1000:.2f} ms")
        print(f"  Velocity forces: {velocity_time*1000:.2f} ms")
        print(f"  Gravity forces:  {gravity_time*1000:.2f} ms")
        print(f"  Total per step:  {(mass_time + velocity_time + gravity_time)*1000:.2f} ms")

        # Check if performance is acceptable
        total_time = mass_time + velocity_time + gravity_time
        if total_time > 0.001:  # 1ms threshold
            print(f"  ⚠️ Slow computation: {total_time*1000:.1f} ms > 1 ms")
        else:
            print(f"  βœ… Good performance: {total_time*1000:.1f} ms")

# Run performance diagnostics
performance_diagnostics()

Memory Issues

def memory_usage_analysis():
    """Analyze memory usage in dynamics computations."""

    import tracemalloc

    print("Memory Usage Analysis")
    print("=" * 40)

    # Start memory tracing
    tracemalloc.start()

    # Baseline memory
    snapshot1 = tracemalloc.take_snapshot()

    # Compute dynamics for many configurations
    configs = [np.random.uniform(-np.pi, np.pi, 3) for _ in range(100)]

    for config in configs:
        M = dynamics.mass_matrix(config)
        c = dynamics.velocity_quadratic_forces(config, np.zeros(3))
        g = dynamics.gravity_forces(config, [0, 0, -9.81])

    # Check memory after computations
    snapshot2 = tracemalloc.take_snapshot()

    # Stop tracing
    tracemalloc.stop()

    # Analyze differences
    top_stats = snapshot2.compare_to(snapshot1, 'lineno')

    print("Top memory allocations:")
    for index, stat in enumerate(top_stats[:3], 1):
        print(f"{index}. {stat}")

    # Check for memory leaks
    total_size = sum(stat.size for stat in snapshot2.statistics('filename'))
    print(f"\nTotal memory used: {total_size / 1024 / 1024:.1f} MB")

    if total_size > 100 * 1024 * 1024:  # 100 MB threshold
        print("⚠️ High memory usage detected")
    else:
        print("βœ… Memory usage is reasonable")

# Run memory analysis
memory_usage_analysis()

Best Practices

Code Organization

    """Best practice organization for dynamics computations."""

    def __init__(self, dynamics_instance):
        self.dynamics = dynamics_instance
        self.cache_enabled = True
        self.performance_monitor = True

    def compute_full_dynamics(self, theta, theta_dot, theta_ddot, g, F_ext):
        """Compute all dynamics components efficiently."""

        # Pre-compute commonly used quantities
        M = self.dynamics.mass_matrix(theta)
        c = self.dynamics.velocity_quadratic_forces(theta, theta_dot)
        g_forces = self.dynamics.gravity_forces(theta, g)
        J_T = self.dynamics.jacobian(theta).T

        # External force contribution
        tau_ext = J_T @ F_ext

        # Inverse dynamics
        tau_required = M @ theta_ddot + c + g_forces + tau_ext

        # Forward dynamics (for verification)
        tau_net = tau_required - c - g_forces - tau_ext
        theta_ddot_verify = np.linalg.solve(M, tau_net)

        return {
            'mass_matrix': M,
            'coriolis_forces': c,
            'gravity_forces': g_forces,
            'external_torques': tau_ext,
            'required_torques': tau_required,
            'computed_acceleration': theta_ddot_verify,
            'verification_error': np.linalg.norm(theta_ddot - theta_ddot_verify)
        }

# Example usage
dynamics_manager = RobotDynamicsManager(dynamics)

# Test configuration
theta = np.array([0.1, 0.3, -0.2])
theta_dot = np.array([0.5, -0.3, 0.8])
theta_ddot = np.array([1.0, -0.5, 0.3])
g = [0, 0, -9.81]
F_ext = np.array([2.0, 1.0, -5.0, 0.1, 0.2, 0.0])

results = dynamics_manager.compute_full_dynamics(theta, theta_dot, theta_ddot, g, F_ext)
print(f"Verification error: {results['verification_error']:.6f}")

Error Handling

def robust_dynamics_computation(dynamics, theta, theta_dot=None, theta_ddot=None):
    """Robust dynamics computation with error handling."""

    try:
        # Validate inputs
        theta = np.array(theta)
        if theta_dot is None:
            theta_dot = np.zeros_like(theta)
        if theta_ddot is None:
            theta_ddot = np.zeros_like(theta)

        theta_dot = np.array(theta_dot)
        theta_ddot = np.array(theta_ddot)

        # Check dimensions
        if not (len(theta) == len(theta_dot) == len(theta_ddot)):
            raise ValueError("Inconsistent vector dimensions")

        # Check for valid values
        if not np.all(np.isfinite(theta)):
            raise ValueError("Invalid joint angles (NaN or infinite)")

        # Compute dynamics safely
        M = dynamics.mass_matrix(theta)

        # Check mass matrix properties
        if np.linalg.cond(M) > 1e12:
            print("Warning: Mass matrix is poorly conditioned")

        c = dynamics.velocity_quadratic_forces(theta, theta_dot)
        g_forces = dynamics.gravity_forces(theta, [0, 0, -9.81])

        return {
            'success': True,
            'mass_matrix': M,
            'coriolis_forces': c,
            'gravity_forces': g_forces,
            'condition_number': np.linalg.cond(M)
        }

    except np.linalg.LinAlgError as e:
        return {
            'success': False,
            'error': f"Linear algebra error: {e}",
            'recommendation': "Check robot configuration for singularities"
        }
    except ValueError as e:
        return {
            'success': False,
            'error': f"Input validation error: {e}",
            'recommendation': "Verify input dimensions and values"
        }
    except Exception as e:
        return {
            'success': False,
            'error': f"Unexpected error: {e}",
            'recommendation': "Contact support with robot parameters"
        }

# Example usage with error handling
result = robust_dynamics_computation(dynamics, theta, theta_dot)
if result['success']:
    print("Dynamics computation successful")
    print(f"Condition number: {result['condition_number']:.2e}")
else:
    print(f"Error: {result['error']}")
    print(f"Recommendation: {result['recommendation']}")

Documentation Standards

def example_dynamics_function(dynamics, theta, theta_dot, control_gains):
    """
    Example function demonstrating proper documentation for dynamics operations.

    This function computes robot dynamics and applies control law for trajectory tracking.
    It serves as a template for documenting dynamics-related functions in ManipulaPy.

    Parameters
    ----------
    dynamics : ManipulatorDynamics
        Instance of ManipulatorDynamics class containing robot parameters
    theta : np.ndarray, shape (n,)
        Current joint angles in radians
    theta_dot : np.ndarray, shape (n,)
        Current joint velocities in rad/s
    control_gains : dict
        Dictionary containing control gains:
        - 'Kp': np.ndarray, shape (n,n) - Proportional gain matrix
        - 'Kd': np.ndarray, shape (n,n) - Derivative gain matrix

    Returns
    -------
    dict
        Dictionary containing:
        - 'torques': np.ndarray, shape (n,) - Required joint torques in Nβ‹…m
        - 'energy': float - Total kinetic energy in Joules
        - 'power': float - Instantaneous power in Watts

    Raises
    ------
    ValueError
        If input dimensions are inconsistent
    np.linalg.LinAlgError
        If mass matrix is singular

    Examples
    --------
    >>> # Setup robot dynamics
    >>> dynamics = ManipulatorDynamics(...)
    >>> theta = np.array([0.1, 0.3, -0.2])
    >>> theta_dot = np.array([0.5, -0.3, 0.8])
    >>> gains = {'Kp': np.diag([100, 80, 60]), 'Kd': np.diag([20, 15, 10])}
    >>>
    >>> # Compute dynamics
    >>> result = example_dynamics_function(dynamics, theta, theta_dot, gains)
    >>> print(f"Required torques: {result['torques']}")
    >>> print(f"Kinetic energy: {result['energy']:.3f} J")

    Notes
    -----
    This function assumes:
    - Robot is operating in Earth gravity (9.81 m/sΒ²)
    - No external forces applied to end-effector
    - Joint limits are respected in input configuration

    For high-frequency control applications, consider caching the mass matrix
    computation to improve performance.

    References
    ----------
    .. [1] Murray, R. M., Li, Z., & Sastry, S. S. (1994). A mathematical
           introduction to robotic manipulation. CRC press.
    .. [2] Lynch, K. M., & Park, F. C. (2017). Modern robotics. Cambridge
           University Press.
    """

    # Input validation
    theta = np.asarray(theta)
    theta_dot = np.asarray(theta_dot)

    if theta.shape != theta_dot.shape:
        raise ValueError("theta and theta_dot must have same shape")

    # Extract control gains
    Kp = control_gains['Kp']
    Kd = control_gains['Kd']

    # Compute dynamics
    M = dynamics.mass_matrix(theta)
    c = dynamics.velocity_quadratic_forces(theta, theta_dot)
    g_forces = dynamics.gravity_forces(theta, [0, 0, -9.81])

    # Simple PD control with gravity compensation
    theta_desired = np.zeros_like(theta)  # For this example
    error = theta_desired - theta
    error_dot = -theta_dot  # Assuming zero desired velocity

    tau_control = Kp @ error + Kd @ error_dot
    tau_total = tau_control + g_forces

    # Compute energy and power
    kinetic_energy = 0.5 * theta_dot.T @ M @ theta_dot
    power = tau_total.T @ theta_dot

    return {
        'torques': tau_total,
        'energy': kinetic_energy,
        'power': power
    }

Testing and Validation

def validate_dynamics_implementation(dynamics):
    """
    Comprehensive validation of dynamics implementation.

    This function performs various tests to ensure the dynamics
    implementation is correct and numerically stable.
    """

    print("Dynamics Implementation Validation")
    print("=" * 50)

    # Test configurations
    test_configs = [
        np.zeros(3),                    # Home position
        np.array([0.1, 0.3, -0.2]),   # Random configuration
        np.array([np.pi/4, -np.pi/6, np.pi/3]),  # Larger angles
    ]

    validation_results = {}

    for i, theta in enumerate(test_configs):
        print(f"\nTest Configuration {i+1}: {np.degrees(theta)} degrees")
        print("-" * 30)

        try:
            # Test 1: Mass matrix properties
            M = dynamics.mass_matrix(theta)

            # Symmetry test
            symmetry_error = np.max(np.abs(M - M.T))
            is_symmetric = symmetry_error < 1e-10
            print(f"Mass matrix symmetric: {is_symmetric} (error: {symmetry_error:.2e})")

            # Positive definiteness test
            eigenvals = np.linalg.eigvals(M)
            is_positive_definite = np.all(eigenvals > 0)
            min_eigenval = np.min(eigenvals)
            print(f"Positive definite: {is_positive_definite} (min eigenval: {min_eigenval:.6f})")

            # Condition number test
            cond_num = np.linalg.cond(M)
            is_well_conditioned = cond_num < 1e6
            print(f"Well conditioned: {is_well_conditioned} (cond: {cond_num:.2e})")

            # Test 2: Velocity forces at zero velocity
            c_zero = dynamics.velocity_quadratic_forces(theta, np.zeros_like(theta))
            c_zero_norm = np.linalg.norm(c_zero)
            velocity_test_passed = c_zero_norm < 1e-10
            print(f"Zero velocity forces at rest: {velocity_test_passed} (norm: {c_zero_norm:.2e})")

            # Test 3: Consistency between forward and inverse dynamics
            theta_dot = np.array([0.1, -0.2, 0.15])
            theta_ddot_desired = np.array([0.5, -0.3, 0.2])

            # Compute required torques
            tau = dynamics.inverse_dynamics(theta, theta_dot, theta_ddot_desired,
                                          [0, 0, -9.81], np.zeros(6))

            # Compute resulting accelerations
            theta_ddot_computed = dynamics.forward_dynamics(theta, theta_dot, tau,
                                                          [0, 0, -9.81], np.zeros(6))

            consistency_error = np.linalg.norm(theta_ddot_desired - theta_ddot_computed)
            consistency_test_passed = consistency_error < 1e-6
            print(f"Forward/inverse consistency: {consistency_test_passed} (error: {consistency_error:.2e})")

            # Store results
            validation_results[f"config_{i+1}"] = {
                'symmetric': is_symmetric,
                'positive_definite': is_positive_definite,
                'well_conditioned': is_well_conditioned,
                'zero_velocity_test': velocity_test_passed,
                'consistency_test': consistency_test_passed,
                'condition_number': cond_num,
                'symmetry_error': symmetry_error,
                'consistency_error': consistency_error
            }

        except Exception as e:
            print(f"❌ Error in configuration {i+1}: {e}")
            validation_results[f"config_{i+1}"] = {'error': str(e)}

    # Overall assessment
    print(f"\nOverall Assessment")
    print("=" * 30)

    all_tests_passed = True
    for config_name, results in validation_results.items():
        if 'error' in results:
            all_tests_passed = False
            continue

        config_passed = all([
            results['symmetric'],
            results['positive_definite'],
            results['well_conditioned'],
            results['zero_velocity_test'],
            results['consistency_test']
        ])

        if not config_passed:
            all_tests_passed = False

    if all_tests_passed:
        print("βœ… All validation tests passed!")
        print("   The dynamics implementation appears correct.")
    else:
        print("❌ Some validation tests failed.")
        print("   Please review the implementation and robot parameters.")

    return validation_results

# Run validation
validation_results = validate_dynamics_implementation(dynamics)

ManipulatorDynamics Class Reference

The ManipulatorDynamics Class

The ManipulatorDynamics class is the core component for robot dynamics computations in ManipulaPy. It inherits from SerialManipulator and adds dynamics-specific functionality.

from ManipulaPy.dynamics import ManipulatorDynamics

Class Initialization

The ManipulatorDynamics class is initialized with the robot’s kinematic and dynamic parameters:

def __init__(self, M_list, omega_list, r_list, b_list, S_list, B_list, Glist):
    """
    Initialize ManipulatorDynamics instance.

    Parameters
    ----------
    M_list : np.ndarray, shape (4, 4)
        Home configuration matrix of the end-effector
    omega_list : np.ndarray, shape (3, n)
        Joint rotation axes in space frame
    r_list : np.ndarray, shape (n, 3)
        Points on joint axes in space frame
    b_list : np.ndarray, shape (n, 3)
        Points on joint axes in body frame
    S_list : np.ndarray, shape (6, n)
        Screw axes in space frame
    B_list : np.ndarray, shape (6, n)
        Screw axes in body frame
    Glist : list of np.ndarray, each shape (6, 6)
        Spatial inertia matrices for each link
    """

Key Methods

mass_matrix(thetalist)

Computes the robot’s mass/inertia matrix for a given configuration:

M = dynamics.mass_matrix(theta)
# Returns: np.ndarray, shape (n, n) - Mass matrix

velocity_quadratic_forces(thetalist, dthetalist)

Computes Coriolis and centrifugal forces:

c = dynamics.velocity_quadratic_forces(theta, theta_dot)
# Returns: np.ndarray, shape (n,) - Velocity-dependent forces

gravity_forces(thetalist, g)

Computes gravitational forces acting on the robot:

g_forces = dynamics.gravity_forces(theta, [0, 0, -9.81])
# Returns: np.ndarray, shape (n,) - Gravity torques

inverse_dynamics(thetalist, dthetalist, ddthetalist, g, Ftip)

Computes required joint torques for desired motion:

tau = dynamics.inverse_dynamics(theta, theta_dot, theta_ddot, g, F_ext)
# Returns: np.ndarray, shape (n,) - Required joint torques

forward_dynamics(thetalist, dthetalist, taulist, g, Ftip)

Computes resulting accelerations from applied torques:

theta_ddot = dynamics.forward_dynamics(theta, theta_dot, tau, g, F_ext)
# Returns: np.ndarray, shape (n,) - Joint accelerations

Implementation Details

Caching System

The class implements an intelligent caching system for mass matrix computations:

def mass_matrix(self, thetalist):
    thetalist_key = tuple(thetalist)
    if thetalist_key in self._mass_matrix_cache:
        return self._mass_matrix_cache[thetalist_key]

    # Compute mass matrix...
    M = self._compute_mass_matrix(thetalist)

    self._mass_matrix_cache[thetalist_key] = M
    return M

Numerical Methods

The velocity_quadratic_forces method uses numerical differentiation:

def partial_derivative(self, i, j, k, thetalist):
    epsilon = 1e-6
    thetalist_plus = np.array(thetalist)
    thetalist_plus[k] += epsilon
    M_plus = self.mass_matrix(thetalist_plus)

    thetalist_minus = np.array(thetalist)
    thetalist_minus[k] -= epsilon
    M_minus = self.mass_matrix(thetalist_minus)

    return (M_plus[i, j] - M_minus[i, j]) / (2 * epsilon)

Mathematical Foundation

The implementation is based on the Newton-Euler formulation of robot dynamics:

Equation of Motion

\[\tau = M(\theta)\ddot{\theta} + C(\theta,\dot{\theta})\dot{\theta} + G(\theta) + J^T(\theta)F_{ext}\]

Where: - \(M(\theta)\) is computed using the articulated body algorithm - \(C(\theta,\dot{\theta})\) uses Christoffel symbols - \(G(\theta)\) considers gravitational potential energy - \(J^T(\theta)F_{ext}\) maps external forces to joint space

Mass Matrix Computation

The mass matrix is computed using the articulated body method:

\[M_{ij} = \text{Tr}(Ad_{T_{0j}}^T G_j Ad_{T_{0j}} S_i S_j^T)\]

Where: - \(Ad_{T_{0j}}\) is the adjoint transformation from base to link j - \(G_j\) is the spatial inertia matrix of link j - \(S_i\) is the screw axis of joint i

Summary

The ManipulaPy dynamics module provides a comprehensive implementation of robot dynamics computations. Key features include:

  • Efficient mass matrix computation with caching

  • Accurate Coriolis force calculation using numerical methods

  • Gravity compensation for various orientations

  • Forward and inverse dynamics for control applications

  • Numerical stability checks and error handling

  • Performance optimization for real-time applications

The module is designed for both educational and practical applications, providing clear mathematical foundations while maintaining computational efficiency suitable for real-time control systems.

For advanced users, the module supports GPU acceleration, parallel processing, and custom optimization techniques for high-performance robotics applications.