Dynamics User Guide — ManipulaPy 1.1.0 Documentation
Description: This comprehensive guide covers robot dynamics computations in ManipulaPy, optimized for Python 3.10.12. Introduction to Robot Dynamics: Robot dynamics deals with the relationship between forces/to...

# 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 core equation of motion for an n-DOF serial manipulator is the Newton–Euler (or Lagrange) form:

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

where:

- \(\boldsymbol\tau\in\mathbb R^{n}\) is the vector of joint torques

- \(\boldsymbol\theta,\dot{\boldsymbol\theta},\ddot{\boldsymbol\theta}\in\mathbb R^{n}\) are joint positions, velocities, and accelerations

- \(\mathbf F_{\mathrm{ext}}\in\mathbb R^{6}\) is the spatial external wrench (force/torque) at the end‐effector

Mass Matrix

The symmetric, positive‐definite inertia matrix

\[M(\boldsymbol\theta) = \sum_{i=1}^{n} \bigl(\mathrm{Ad}_{T_{0}^{\,i-1}}^{T}\bigr)\;G_{i}\;\bigl(\mathrm{Ad}_{T_{0}^{\,i-1}}\bigr)\]

where for each link i, \(G_{i}\) is its 6×6 spatial inertia, and \(\mathrm{Ad}_{T}\) denotes the SE(3) adjoint of the transform from the base to link i.

Coriolis & Centrifugal

Combined velocity‐dependent forces:

\[\begin{split}C(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) \, \dot{\boldsymbol{\theta}} = \begin{bmatrix} \sum\limits_{j,k=1}^{n} \Gamma_{1jk}(\boldsymbol{\theta}) \, \dot{\theta}_{j} \, \dot{\theta}_{k} \\[6pt] \vdots \\[3pt] \sum\limits_{j,k=1}^{n} \Gamma_{njk}(\boldsymbol{\theta}) \, \dot{\theta}_{j} \, \dot{\theta}_{k} \end{bmatrix}, \quad \Gamma_{ijk} = \frac{1}{2} \left( \frac{\partial M_{ij}}{\partial \theta_k} + \frac{\partial M_{ik}}{\partial \theta_j} - \frac{\partial M_{jk}}{\partial \theta_i} \right)\end{split}\]

Gravity

Derived from the potential energy

\[U(\boldsymbol\theta) = \sum_{i=1}^{n} m_{i}\;g^{T}\,p_{i}(\boldsymbol\theta),\]

the gravity torque vector is

\[\begin{split}G(\boldsymbol\theta) = \frac{\partial U}{\partial\boldsymbol\theta} = \begin{bmatrix} \tfrac{\partial U}{\partial\theta_{1}}\\[3pt] \vdots\\[3pt] \tfrac{\partial U}{\partial\theta_{n}} \end{bmatrix}.\end{split}\]

Here, \(p_{i}(\boldsymbol\theta)\) is the world‐frame position of link i’s center of mass.

External Wrench Mapping

An end‐effector wrench \(\mathbf F_{\mathrm{ext}}\in\mathbb R^{6}\) is pulled back to joint torques via the Jacobian transpose:

\[\tau_{\mathrm{ext}} = J(\boldsymbol\theta)^{T}\,\mathbf F_{\mathrm{ext}}.\]

Putting it all together:

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

This formulation underlies both:

- Inverse Dynamics: compute \(\boldsymbol\tau\) from given \((\boldsymbol\theta,\dot{\boldsymbol\theta},\ddot{\boldsymbol\theta})\)

- Forward Dynamics: compute \(\ddot{\boldsymbol\theta}\) via \(\ddot{\boldsymbol\theta} = M(\theta)^{-1}\bigl(\boldsymbol\tau - C\,\dot{\boldsymbol\theta} - G - J^{T}\mathbf F_{\mathrm{ext}}\bigr)\)

### Key Concepts

Forward Dynamics

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

Inverse Dynamics

  Given desired motion, compute required torques: \(\boldsymbol\tau = f(\boldsymbol\theta, \dot{\boldsymbol\theta}, \ddot{\boldsymbol\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