Dynamics API Reference#

This page documents ``ManipulaPy.dynamics``, the module that adds full rigid-body dynamics to SerialManipulator.

Tip

Looking for a conceptual tour? See Dynamics User Guide.

Quick Navigation#

ManipulatorDynamics Class#

class ManipulaPy.dynamics.ManipulatorDynamics(M_list, omega_list, r_list, b_list, S_list, B_list, Glist, Mlist_per_link=None)[source]#

Bases: SerialManipulator

Serial manipulator model with mass, Coriolis, gravity, and dynamics APIs.

Inheritance: derives from SerialManipulator and therefore exposes all forward-kinematics & Jacobian helpers.

Constructor

Parameters:
__init__(M_list, omega_list, r_list, b_list, S_list, B_list, Glist, Mlist_per_link=None)[source]#

Initialize manipulator dynamics data and finite-difference caches.

Parameters:
Return type:

None

Arguments

M_list

4 Γ— 4 home-configuration matrix \(\mathbf{M}\)

omega_list

3 Γ— n joint axes \(\boldsymbol{\omega}_i\)

r_list

3 Γ— n position vectors in the space frame

b_list

3 Γ— n position vectors in the body frame

S_list

6 Γ— n screw axes in the space frame

B_list

6 Γ— n screw axes in the body frame

Glist

list of 6 Γ— 6 spatial inertia matrices

Mass matrix \(\mathbf{M}(\boldsymbol{\theta})\)

mass_matrix(thetalist)[source]#

Compute mass matrix using per-link body Jacobians.

M(ΞΈ) = Ξ£_k J_k^T G_k J_k

Where J_k is the body Jacobian of link k’s CoM (6Γ—n), and G_k is link k’s spatial inertia in body frame. For joints i > k, J_k[:, i] = 0 (link k doesn’t depend on those joints).

Reference: Modern Robotics Β§8.3 / Murray, Li, Sastry Β§4.3.

If Mlist_per_link is None (legacy path), falls back to the previous EE-Jacobian approximation with a deprecation warning.

Caching: results are memoised per joint configuration.

Parameters:

thetalist (ndarray[tuple[Any, ...], dtype[float64]] | List[float])

Return type:

ndarray[tuple[Any, …], dtype[float64]]

Partial-derivative helper

partial_derivative(i, j, k, thetalist)[source]#

Keep public API but serve results from the cached tensor so a single derivative never re-triggers mass matrix evaluation.

Parameters:
Return type:

float

Force components

velocity_quadratic_forces(thetalist, dthetalist)[source]#

Compute Coriolis and centripetal generalized forces.

Parameters:
Returns:

Joint-space velocity quadratic force vector.

Return type:

ndarray[tuple[Any, …], dtype[float64]]

gravity_forces(thetalist, g=None)[source]#

Compute joint torques needed to compensate gravity.

Parameters:
Returns:

Joint-space gravity compensation torques.

Return type:

ndarray[tuple[Any, …], dtype[float64]]

Inverse Dynamics

inverse_dynamics(thetalist, dthetalist, ddthetalist, g, Ftip)[source]#

Compute joint torques for the requested motion and end-effector wrench.

Parameters:
Returns:

Required joint torques.

Return type:

ndarray[tuple[Any, …], dtype[float64]]

Forward Dynamics

forward_dynamics(thetalist, dthetalist, taulist, g, Ftip)[source]#

Solve joint accelerations from applied torques and external loads.

Parameters:
Returns:

Joint accelerations.

Return type:

ndarray[tuple[Any, …], dtype[float64]]

Full Equations#

Inverse dynamics

\[\boldsymbol{\tau}\;=\; \mathbf{M}\!\bigl(\boldsymbol{\theta}\bigr)\, \ddot{\boldsymbol{\theta}} \;+\; \mathbf{C}\!\bigl(\boldsymbol{\theta},\dot{\boldsymbol{\theta}}\bigr)\, \dot{\boldsymbol{\theta}} \;+\; \mathbf{G}\!\bigl(\boldsymbol{\theta}\bigr) \;+\; \mathbf{J}^{\mathsf{T}}\!\bigl(\boldsymbol{\theta}\bigr)\, \mathbf{F}_{\text{ext}}\]

Forward dynamics

\[\ddot{\boldsymbol{\theta}} \;=\; \mathbf{M}^{-1}\!\bigl(\boldsymbol{\theta}\bigr) \Bigl( \boldsymbol{\tau} \;-\; \mathbf{C}\!\bigl(\boldsymbol{\theta},\dot{\boldsymbol{\theta}}\bigr)\, \dot{\boldsymbol{\theta}} \;-\; \mathbf{G}\!\bigl(\boldsymbol{\theta}\bigr) \;-\; \mathbf{J}^{\mathsf{T}}\!\bigl(\boldsymbol{\theta}\bigr)\, \mathbf{F}_{\text{ext}} \Bigr)\]

Usage Examples#

Basic set-up

import numpy as np
from ManipulaPy.urdf_processor import URDFToSerialManipulator

# Load a URDF and grab its pre-built dynamics model
processor = URDFToSerialManipulator("robot.urdf")
dynamics  = processor.dynamics        # instance of ManipulatorDynamics

Mass matrix

M = dynamics.mass_matrix([0.1, 0.2, 0.3, 0, 0, 0])

Individual force terms

q   = np.array([0.1, 0.2, 0.3, 0, 0, 0])
dq  = np.array([0.05, 0.1, 0, 0, 0, 0])

C   = dynamics.velocity_quadratic_forces(q, dq)
G   = dynamics.gravity_forces(q)

Inverse dynamics

ddq = np.array([1.0, 0.5, 0, 0, 0, 0])
tau = dynamics.inverse_dynamics(q, dq, ddq,
                                g=[0, 0, -9.81],
                                Ftip=[0, 0, -10, 0, 0, 0])

Forward dynamics

ddq_sim = dynamics.forward_dynamics(
    q, dq, tau,
    g=[0, 0, -9.81],
    Ftip=[0, 0, 0, 0, 0, 0]
)

Key Features#

  • Automatic caching of expensive mass-matrix evaluations

  • Complete force model (Coriolis, centrifugal, gravity, externals)

  • Gravity override for moon/Mars/zero-g simulations

  • Joint-limit aware helpers inherited from kinematics

  • Designed for GPU – the same formulas power the CUDA kernels

Troubleshooting#

Large torques? Check your gravity vector and link inertias. Singularities? Watch the Jacobian condition number (see Kinematics guide). Slow first call? Numba JIT compiles on demand; subsequent calls are much faster.

See Also#