Dynamics User Guide#

This comprehensive guide covers robot dynamics computations in ManipulaPy, compatible with Python 3.9 through 3.12.

Note

This guide is compatible with Python 3.9 through 3.12 and includes performance improvements that work across supported versions.

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.array2string(np.degrees(theta), precision=1)} degrees")
        print(f"Joint velocities: {np.array2string(theta_dot, precision=2)} rad/s")
        print(f"Joint accelerations: {np.array2string(theta_ddot, precision=2)} rad/s²")
        print(f"External forces: {np.array2string(F_ext[:3], precision=1)} N, {np.array2string(F_ext[3:], precision=2)} Nâ‹…m")
        print()
        print("Torque breakdown:")
        print(f"  Inertial:   {np.array2string(tau_inertial, precision=2)} Nâ‹…m")
        print(f"  Coriolis:   {np.array2string(tau_coriolis, precision=2)} Nâ‹…m")
        print(f"  Gravity:    {np.array2string(tau_gravity, precision=2)} Nâ‹…m")
        print(f"  External:   {np.array2string(tau_external, precision=2)} Nâ‹…m")
        print(f"  Total:      {np.array2string(tau_total, precision=2)} 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#

class RobotDynamicsManager:
    """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.