- Closed-Loop Multi-Body DAEs
- 29×29 KKT Inverse Dynamics Solve
- Baumgarte Constraint Stabilization
- Closed-Loop PID Joint Regulation
- 12-Chapter Theoretical Monograph
DYNAMICS SIMULATION / CLOSED-LOOP MECHANISMS
Closed-Loop Multi-Body Dynamics, Inverse Control, and Numerical Stabilization
Closed kinematic chains arise frequently in parallel robotics, industrial linkages, and biomechanical exoskeletons. Because loop-closure equations introduce algebraic constraints alongside differential equations of motion, simulating a 5-link closed-loop mechanism requires specialized numerical integration to eliminate constraint drift, model inverse actuator efforts, and ensure stable closed-loop trajectory tracking.
Google Colab Interactive Research Notebook (68 Executed Cells)
A complete, self-contained multi-body dynamics research pipeline in Python. Features symbolic Lagrangian derivation, Baumgarte stabilization, a 29×29 KKT augmented inverse dynamics block solver, and closed-loop PID trajectory regulation.
Holonomic Kinematic Manifold & SymPy Derivations
Defines Cartesian generalized coordinates q ∈ ℝ¹⁵ for 5 rigid links subjected to 13 holonomic pin and ground constraints Φ(q)=0 (yielding 2 independent DOFs). Evaluates the 13×15 constraint Jacobian J=∂Φ/∂q, quadratic acceleration bias γ(q,q̇)=J̇q̇, and Baumgarte feedback stabilization (ζ_B=1.0, ω_B=20.0 rad/s) to prevent numerical drift.
Forward DAE Time Integration & Hamiltonian Energy Audits
Integrates the unactuated multi-body system under gravity using SciPy's adaptive solve_ivp (RK45, rtol=1e-9, atol=1e-11). Continuously audits total mechanical energy E(t) = T(t) + V(t) to guarantee Hamiltonian consistency across 10 seconds of motion, producing high-resolution Matplotlib animation frames.
29×29 KKT Inverse Dynamics & Feedforward Torque Replay
Formulates active actuation at revolute joint P34 (link 3 relative to grounded base link 4) with actuator mapping B₃₄ = J₃₄ᵀ ∈ ℝ¹⁵ˣ¹. Constructs and solves an augmented 29×29 KKT block linear system for both the continuous feedforward motor torque τ₃₄(t) and all 13 pin reaction multipliers λ(t). Verifies trajectory execution through forward simulation replay with zero kinematic divergence.
Closed-Loop PID Setpoint Regulation & Disturbance Rejection
Implements a fixed-setpoint joint PID controller with anti-windup (Kp=200 N·m/rad, Ki=10 N·m/(rad·s), Kd=60 N·m·s/rad, saturation limit ±100 N·m). Injects an initial rate perturbation of 0.15 rad/s projected onto the constraint tangent space, verifying asymptotic disturbance rejection within 1.5 seconds while maintaining sub-millimeter constraint tolerances.
constrained_five_link_dynamics.ipynbTarget Cell #388ed077# Generalized constrained Euler-Lagrange DAE:
# M(q) * q_ddot + J(q)^T * lambda = Q_g(q) + B(q)*u
# Baumgarte constraint acceleration stabilization:
# gamma_stab = gamma(q, q_dot) + 2*alpha_B*Phi_dot + (beta_B^2)*Phi
A_kkt = np.block([
[M_matrix(q), J_jacobian(q).T],
[J_jacobian(q), np.zeros((13, 13))]
]) # Augmented Block Matrix: (29 x 29)
rhs = np.concatenate([
Q_gravity(q) + B_actuator(q) @ u_tau,
-gamma_accel(q, q_dot) - 2 * alpha_B * Phi_dot - (beta_B**2) * Phi_pos
])
# Direct factorization for accelerations & reaction forces
sol = np.linalg.solve(A_kkt, rhs)
q_ddot, reaction_lambdas = sol[:15], sol[15:]Holonomic constraints & Jacobian (q ∈ ℝ¹⁵, J ∈ ℝ¹³ˣ¹⁵)
Models Cartesian generalized coordinates ordered by body (x₁, y₁, θ₁, ..., x₅, y₅, θ₅) with holonomic loop-closure and ground constraints Φ(q)=0. The constraint Jacobian J=∂Φ/∂q (13×15) projects the system onto two regular local degrees of freedom.
DAE formulation & reaction multipliers (λ ∈ ℝ¹³)
Solves the coupled constrained acceleration system M q̈ + Jᵀλ = Q_g(q) + Q_ext + B(q)u with differentiated acceleration constraints J q̈ + γ(q, q̇) = 0, computing both generalized accelerations and ideal internal pin reaction forces λ ∈ ℝ¹³.
29×29 KKT Inverse dynamics & closed-loop PID control
Extends from natural unactuated dynamics to active inverse control: solves the force balance B(q_d)u - Jᵀλ = M q̈_d - Q_g - Q_ext to synthesize feedforward joint torques, paired with closed-loop PID feedback (Kp=200, Ki=10, Kd=60) to reject perturbations and guarantee trajectory convergence.
Baumgarte stabilization & Hamiltonian energy conservation
Standard numerical integration causes geometric constraints to drift over time. Baumgarte feedback stabilization (ζ_B=1.0, ω_B=20.0 rad/s) guarantees pin-closure and ground-pose consistency, while Hamiltonian energy audits verify conservative dynamics in continuous time.
Complete Mathematical Monograph (12 Chapters)
Comprehensive scientific monograph written in LaTeX (MiKTeX), detailing the complete Lagrangian equations of motion, symbolic DAE formulation, Baumgarte numerical stabilization, adaptive RK45 integration, and double-pendulum benchmark validation.
Loading document viewer...
System Specifications & Theoretical Framework
- Kinematic Chain
- Planar 5-bar closed kinematic chain with dual grounded base anchors (2 DOFs)
- Coordinate System
- Cartesian body coordinates q ∈ ℝ¹⁵ subjected to 13 holonomic constraints Φ(q)=0
- Dynamics Formulation
- Constrained Euler-Lagrange DAEs with Lagrange reaction multipliers λ ∈ ℝ¹³
- Inverse Dynamics
- 29×29 augmented KKT block system for joint P34 actuator torque and reaction forces
- Control Schemes
- Gravity-driven free response, feedforward inverse dynamics, and closed-loop joint PID
- Numerical Solvers
- SciPy solve_ivp (RK45) with Baumgarte constraint stabilization (ζ_B=1.0, ω_B=20.0 rad/s)
- Notebook Architecture
- 68 executed cells with SymPy symbolic derivations and Matplotlib animation renderers
- Documentation
- 12-chapter comprehensive monograph in LaTeX (MiKTeX), validation against double pendulum benchmark
Significance for Robotics & Advanced Mechanical Systems
Demonstrates end-to-end multi-body engineering: from symbolic Lagrangian mechanics and constraint manifolds to numerical simulation, inverse dynamics control, and academic monograph publication.