Robot Dynamics¶
Overview¶
Robot dynamics describes the relationship between forces/torques and robot motion. It answers the question: "What joint torques are needed to produce desired motion?"
Lagrange-Euler Formulation¶
The Lagrangian \(L = K - P\) (kinetic energy minus potential energy) leads to:
\[\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)\]
Where: - \(M(q)\): Mass/Inertia matrix - \(C(q,\dot{q})\): Coriolis and centrifugal forces - \(g(q)\): Gravity vector - \(\tau\): Joint torques
A. Manipulator Equation of Motion¶
To make a robot arm move the way we want, what torques do we need at each joint?
Mathematical Formulation¶
The manipulator equation of motion in Lagrangian form is:
\[\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)\]
Where: - \(\tau \in \mathbb{R}^n\): Joint torque vector - \(M(q) \in \mathbb{R}^{n \times n}\): Mass/inertia matrix - \(C(q,\dot{q}) \in \mathbb{R}^n\): Coriolis and centrifugal terms - \(g(q) \in \mathbb{R}^n\): Gravity vector - \(q, \dot{q}, \ddot{q}\): Joint position, velocity, acceleration
The kinetic energy \(K\) and potential energy \(P\) are:
\[K = \frac{1}{2} \dot{q}^T M(q) \dot{q}\]
\[P = \sum_{i=1}^{n} m_i g h_i(q)\]
Where \(h_i(q)\) is the height of link \(i\)'s center of mass.
Code Implementation¶
import numpy as np
def compute_manipulator_equation(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2, qdd1, qdd2,
g_val=9.81):
"""
Compute the manipulator equation of motion for a 2-DOF planar robot.
Args:
m1, m2: Link masses [kg]
l1, l2: Link lengths [m]
r1, r2: Distance from joint to COM [m]
I1, I2: Moment of inertia about COM [kg·m²]
theta1, theta2: Joint angles [rad]
qd1, qd2: Joint velocities [rad/s]
qdd1, qdd2: Joint accelerations [rad/s²]
g_val: Gravitational acceleration [m/s²]
Returns:
tau: Joint torques [N·m]
"""
# Compute inertia matrix M(q)
M = compute_inertia_matrix(m1, m2, l1, l2, r1, r2, I1, I2, theta1, theta2)
# Compute Coriolis and centrifugal terms C(q, qd)
C = compute_coriolis(m2, l1, r2, theta2, qd1, qd2)
# Compute gravity terms g(q)
g_vec = compute_gravity(m1, m2, l1, r1, r2, g_val, theta1, theta2)
# Manipulator equation: tau = M * qdd + C + g
qdd = np.array([qdd1, qdd2])
tau = M @ qdd + C + g_vec
return tau
def compute_energy(m1, m2, l1, l2, r1, r2, I1, I2, theta1, theta2, qd1, qd2, g_val=9.81):
"""
Compute kinetic and potential energy for a 2-DOF planar robot.
Returns:
K: Kinetic energy [J]
P: Potential energy [J]
"""
# Kinetic energy: K = 0.5 * qd^T * M(q) * qd
M = compute_inertia_matrix(m1, m2, l1, l2, r1, r2, I1, I2, theta1, theta2)
qd = np.array([qd1, qd2])
K = 0.5 * qd @ M @ qd
# Potential energy: P = m1*g*h1 + m2*g*h2
# For link 1: h1 = r1 * cos(theta1)
# For link 2: h2 = l1*cos(theta1) + r2*cos(theta1+theta2)
h1 = r1 * np.cos(theta1)
h2 = l1 * np.cos(theta1) + r2 * np.cos(theta1 + theta2)
P = m1 * g_val * h1 + m2 * g_val * h2
return K, P
# Example usage
if __name__ == "__main__":
# Robot parameters
m1, m2 = 5.0, 3.0 # kg
l1, l2 = 1.0, 0.8 # m
r1, r2 = 0.5, 0.4 # m (distance from joint to COM)
I1, I2 = 0.1, 0.05 # kg·m²
# State
theta1, theta2 = np.pi/4, np.pi/6 # rad
qd1, qd2 = 0.5, 0.3 # rad/s
qdd1, qdd2 = 0.1, 0.2 # rad/s²
# Compute torques
tau = compute_manipulator_equation(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2, qdd1, qdd2)
# Compute energies
K, P = compute_energy(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2)
print(f"Joint torques: τ₁={tau[0]:.3f} N·m, τ₂={tau[1]:.3f} N·m")
print(f"Kinetic energy: K={K:.3f} J")
print(f"Potential energy: P={P:.3f} J")
Code Breakdown¶
| Component | Description |
|---|---|
M @ qdd |
Matrix-vector multiplication: \(M(q)\ddot{q}\) term |
C |
Coriolis/centrifugal forces function |
g_vec |
Gravity vector function |
0.5 * qd @ M @ qd |
Kinetic energy: \(\frac{1}{2}\dot{q}^T M(q) \dot{q}\) |
m * g * h |
Potential energy for each link |
B. Inertia Matrix for 2-DOF Planar Robot¶
Mathematical Formulation¶
For a 2-DOF planar robot with link masses \(m_1, m_2\), link lengths \(l_1, l_2\), distances to center of mass \(r_1, r_2\), and moments of inertia \(I_1, I_2\):
Inertia Matrix Elements:
\[M_{11} = I_1 + I_2 + m_2(l_1^2 + r_2^2 + 2l_1 r_2 \cos(\theta_2))\]
\[M_{12} = M_{21} = I_2 + m_2 r_2(r_2 + l_1 \cos(\theta_2))\]
\[M_{22} = I_2 + m_2 r_2^2\]
Code Implementation¶
import numpy as np
def compute_inertia_matrix(m1, m2, l1, l2, r1, r2, I1, I2, theta1, theta2):
"""
Compute the inertia matrix M(q) for a 2-DOF planar robot.
The inertia matrix is symmetric and position-dependent.
Args:
m1, m2: Link masses [kg]
l1, l2: Link lengths [m]
r1, r2: Distance from joint to COM [m]
I1, I2: Moment of inertia about COM [kg·m²]
theta1, theta2: Joint angles [rad]
Returns:
M: 2x2 inertia matrix
"""
# M_11 = I1 + I2 + m2*(l1² + r2² + 2*l1*r2*cos(θ2))
# This term depends on θ2 due to coupling between links
M_11 = I1 + I2 + m2 * (l1**2 + r2**2 + 2 * l1 * r2 * np.cos(theta2))
# M_12 = M_21 = I2 + m2*r2*(r2 + l1*cos(θ2))
# Symmetric coupling term - link 2 inertia reflected to joint 1
M_12 = I2 + m2 * r2 * (r2 + l1 * np.cos(theta2))
# M_22 = I2 + m2*r2²
# Independent inertia of link 2 (only its own mass)
M_22 = I2 + m2 * r2**2
# Assemble symmetric inertia matrix
M = np.array([
[M_11, M_12],
[M_12, M_22]
])
return M
# Example usage
if __name__ == "__main__":
# Robot parameters
m1, m2 = 5.0, 3.0 # kg
l1, l2 = 1.0, 0.8 # m
r1, r2 = 0.5, 0.4 # m
I1, I2 = 0.1, 0.05 # kg·m²
# Different configurations
configs = [
(0, 0), # Both links horizontal
(np.pi/2, 0), # First link vertical
(np.pi/4, np.pi/4), # Bent configuration
]
for theta1, theta2 in configs:
M = compute_inertia_matrix(m1, m2, l1, l2, r1, r2, I1, I2, theta1, theta2)
print(f"θ₁={np.degrees(theta1):.1f}°, θ₂={np.degrees(theta2):.1f}°")
print(f"M = \n{M}\n")
Code Breakdown¶
| Matrix Element | Formula | Physical Meaning |
|---|---|---|
M_11 |
\(I_1 + I_2 + m_2(l_1^2 + r_2^2 + 2l_1 r_2 \cos\theta_2)\) | Total inertia seen at joint 1 (largest) |
M_12 |
\(I_2 + m_2 r_2(r_2 + l_1 \cos\theta_2)\) | Coupling inertia between joints |
M_22 |
\(I_2 + m_2 r_2^2\) | Inertia of link 2 alone (smallest) |
Key insight: \(M_{11}\) depends on \(\theta_2\) through the \(\cos(\theta_2)\) term — this is the configuration-dependent coupling effect that makes robot control challenging.
C. Coriolis and Centrifugal Terms¶
Mathematical Formulation¶
The Coriolis and centrifugal terms are derived from the Christoffel symbols:
\[C_i = \sum_{j,k} c_{ijk}(q) \dot{q}_j \dot{q}_k\]
Where the Christoffel symbols are:
\[c_{ijk} = \frac{1}{2}\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right)\]
For a 2-DOF planar robot:
\[C_1 = -m_2 l_1 r_2 \sin(\theta_2)(2\dot{q}_1\dot{q}_2 + \dot{q}_2^2)\]
\[C_2 = m_2 l_1 r_2 \sin(\theta_2)\dot{q}_1^2\]
Code Implementation¶
import numpy as np
def compute_coriolis(m2, l1, r2, theta2, qd1, qd2):
"""
Compute the Coriolis and centrifugal force vector C(q, qd).
These are nonlinear velocity-dependent forces that arise from
the coupling between joint motions.
Args:
m2: Mass of link 2 [kg]
l1: Length of link 1 [m]
r2: Distance to COM of link 2 [m]
theta2: Joint 2 angle [rad]
qd1, qd2: Joint velocities [rad/s]
Returns:
C: 2-element Coriolis/centrifugal force vector
"""
# C_1 = -m2*l1*r2*sin(θ2)*(2*qd1*qd2 + qd2²)
# This term includes both Coriolis (2*qd1*qd2) and centrifugal (qd2²) effects
# Negative sign indicates this force opposes the motion direction
C_1 = -m2 * l1 * r2 * np.sin(theta2) * (2 * qd1 * qd2 + qd2**2)
# C_2 = m2*l1*r2*sin(θ2)*qd1²
# Pure centrifugal term - only depends on qd1 squared
C_2 = m2 * l1 * r2 * np.sin(theta2) * qd1**2
C = np.array([C_1, C_2])
return C
def compute_christoffel_symbols(m2, l1, r2, theta2):
"""
Compute the Christoffel symbols c_ijk for the 2-DOF robot.
These are used to verify the Coriolis computation.
Returns:
c: Dictionary of Christoffel symbols
"""
# c_111 = ∂M_11/∂q_1 - ∂M_11/∂θ1 = 0 (M_11 independent of θ1)
c_111 = 0
# c_112 = 0.5*(∂M_11/∂q_2 + ∂M_12/∂q_1 - ∂M_21/∂q_1)
# = 0.5*(-2*m2*l1*r2*sin(θ2) + 0 - 0) = -m2*l1*r2*sin(θ2)
c_112 = -m2 * l1 * r2 * np.sin(theta2)
# c_121 = 0.5*(∂M_12/∂q_1 + ∂M_11/∂q_2 - ∂M_22/∂q_1)
# = 0.5*(0 - 2*m2*l1*r2*sin(θ2) - 0) = -m2*l1*r2*sin(θ2)
c_121 = -m2 * l1 * r2 * np.sin(theta2)
# c_122 = 0.5*(∂M_12/∂q_2 + ∂M_12/∂q_2 - ∂M_22/∂q_2)
# = 0.5*(-m2*l1*r2*sin(θ2) - m2*l1*r2*sin(θ2) - 0)
# = -m2*l1*r2*sin(θ2)
c_122 = -m2 * l1 * r2 * np.sin(theta2)
return {'c_111': c_111, 'c_112': c_112, 'c_121': c_121, 'c_122': c_122}
# Example usage
if __name__ == "__main__":
# Parameters
m2 = 3.0 # kg
l1 = 1.0 # m
r2 = 0.4 # m
theta2 = np.pi / 4 # 45 degrees
qd1, qd2 = 1.0, 0.5 # rad/s
# Compute Coriolis forces
C = compute_coriolis(m2, l1, r2, theta2, qd1, qd2)
print(f"θ₂ = {np.degrees(theta2):.1f}°, q̇₁ = {qd1} rad/s, q̇₂ = {qd2} rad/s")
print(f"C₁ = {C[0]:.4f} N·m")
print(f"C₂ = {C[1]:.4f} N·m")
Code Breakdown¶
| Term | Formula | Effect |
|---|---|---|
2*qd1*qd2 |
Coriolis | Force due to motion of one joint when another is rotating |
qd2**2 |
Centrifugal | Outward pulling force from rotation |
sin(theta2) |
Configuration factor | Term vanishes when links are aligned |
D. Gravity Terms¶
Mathematical Formulation¶
The gravity vector is derived from the partial derivative of potential energy:
\[g_i = \frac{\partial P}{\partial q_i}\]
For a 2-DOF planar robot with horizontal reference:
\[g_1 = -(m_1 r_1 + m_2 l_1)g\sin(\theta_1) - m_2 r_2 g\sin(\theta_1 + \theta_2)\]
\[g_2 = -m_2 r_2 g\sin(\theta_1 + \theta_2)\]
Note: The negative signs appear because \(g_i = \partial P / \partial q_i\) and \(\partial(\cos\theta)/\partial\theta = -\sin\theta\). These gravity torques oppose the falling direction.
Code Implementation¶
import numpy as np
def compute_gravity(m1, m2, l1, r1, r2, g_val, theta1, theta2):
"""
Compute the gravity vector g(q) for a 2-DOF planar robot.
The gravity vector is derived from g_i = ∂P/∂q_i where P is potential energy.
The negative signs indicate these are restoring forces that oppose falling.
Args:
m1, m2: Link masses [kg]
l1: Length of link 1 [m]
r1: Distance from joint 1 to COM of link 1 [m]
r2: Distance from joint 2 to COM of link 2 [m]
g_val: Gravitational acceleration [m/s²]
theta1, theta2: Joint angles [rad]
Returns:
g_vec: 2-element gravity force vector [N·m]
"""
# g_1 = -(m1*r1 + m2*l1)*g*sin(θ1) - m2*r2*g*sin(θ1+θ2)
# Negative sign: gravity torques oppose the falling direction
# First term: torque from link 1's weight about joint 1
# Second term: torque from link 2's weight about joint 1 (through link 1)
g_1 = -(m1 * r1 + m2 * l1) * g_val * np.sin(theta1) - m2 * r2 * g_val * np.sin(theta1 + theta2)
# g_2 = -m2*r2*g*sin(θ1+θ2)
# Only link 2's weight affects joint 2 torque
g_2 = -m2 * r2 * g_val * np.sin(theta1 + theta2)
g_vec = np.array([g_1, g_2])
return g_vec
def compute_gravity_from_energy(m1, m2, l1, r1, r2, g_val, theta1, theta2, delta=1e-6):
"""
Verify gravity computation by numerical differentiation of potential energy.
P = m1*g*r1*cos(θ1) + m2*g*(l1*cos(θ1) + r2*cos(θ1+θ2))
g_i = ∂P/∂q_i
Note: Uses r1 (distance to COM) not l1 (link length) for link 1's height.
"""
def potential_energy(th1, th2):
"""Compute potential energy P(q)."""
# Height of link 1 COM: r1*cos(θ1) (distance from joint 1 to COM times cos of angle)
h1 = r1 * np.cos(th1)
# Height of link 2 COM: l1*cos(θ1) + r2*cos(θ1+θ2)
h2 = l1 * np.cos(th1) + r2 * np.cos(th1 + th2)
return m1 * g_val * h1 + m2 * g_val * h2
# Numerical differentiation: g_i ≈ (P(q+δ) - P(q-δ)) / (2δ)
g_1_numerical = (potential_energy(theta1 + delta, theta2) -
potential_energy(theta1 - delta, theta2)) / (2 * delta)
g_2_numerical = (potential_energy(theta1, theta2 + delta) -
potential_energy(theta1, theta2 - delta)) / (2 * delta)
return np.array([g_1_numerical, g_2_numerical])
# Example usage
if __name__ == "__main__":
# Parameters
m1, m2 = 5.0, 3.0 # kg
l1 = 1.0 # m (link length)
r1 = 0.5 # m (distance from joint 1 to COM of link 1)
r2 = 0.4 # m (distance from joint 2 to COM of link 2)
g_val = 9.81 # m/s²
# Test different configurations
configs = [(0, 0), (np.pi/2, 0), (np.pi/4, np.pi/4), (np.pi/2, -np.pi/2)]
for theta1, theta2 in configs:
g_analytical = compute_gravity(m1, m2, l1, r1, r2, g_val, theta1, theta2)
g_numerical = compute_gravity_from_energy(m1, m2, l1, r1, r2, g_val, theta1, theta2)
print(f"θ₁={np.degrees(theta1):.0f}°, θ₂={np.degrees(theta2):.0f}°")
print(f" Analytical: g = {g_analytical}")
print(f" Numerical: g = {g_numerical}")
print(f" Error: {np.abs(g_analytical - g_numerical)}\n")
Code Breakdown¶
| Term | Formula | Physical Meaning |
|---|---|---|
-(m1*r1 + m2*l1)*g*sin(θ1) |
Link 1 weight torque | Negative sign: opposes falling |
-m2*r2*g*sin(θ1+θ2) |
Link 2 weight torque | Additional torque from link 2 |
sin(θ) |
Tangential component | Only the tangent component produces torque |
Note: The negative signs mean these torques must be applied by the motor to counteract gravity. When \(\theta_1 = 0\) (link 1 horizontal), gravity has no effect. When \(\theta_1 = \pi/2\) (link 1 vertical), gravity produces maximum torque.
Newton-Euler Formulation¶
Recursive formulation that propagates velocities and accelerations from base to tip, then propagates forces back.
Algorithm Steps:¶
- Outward recursion: Compute \(\omega_i, \dot{v}_i\) for each link
- Inward recursion: Compute \(\tau_i\) for each joint
Animation — Watch how forces propagate through a double pendulum:
E. Newton-Euler Recursive Formulation¶
Mathematical Formulation¶
The Newton-Euler algorithm computes dynamics recursively using:
Outward Recursion (Base to Tip):
\[\omega_i = \omega_{i-1} + \dot{q}_i \hat{z}_{i-1}\]
\[\dot{v}_i = \dot{v}_{i-1} + \dot{\omega}_i \times r_{i,i-1} + \omega_i \times (\omega_i \times r_{i,i-1})\]
Inward Recursion (Tip to Base):
\[f_i = f_{i+1} + m_i \dot{v}_i - m_i g \hat{y}\]
\[\tau_i = \tau_{i+1} + r_{i,i-1} \times f_i + I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i)\]
Where: - \(\omega_i\): Angular velocity of link \(i\) - \(\dot{v}_i\): Linear acceleration of COM of link \(i\) - \(f_i\): Force exerted on link \(i\) by link \(i-1\) - \(\tau_i\): Torque at joint \(i\) - \(r_{i,i-1}\): Vector from joint \(i-1\) to COM of link \(i\)
Code Implementation¶
import numpy as np
def newton_euler_step(q, qd, qdd, link_params, gravity=9.81):
"""
Perform one Newton-Euler iteration for an n-link planar robot.
This implements the recursive Newton-Euler algorithm:
1. Outward pass: propagate velocities and accelerations
2. Inward pass: propagate forces and compute torques
Args:
q: Joint angles [rad] (n elements)
qd: Joint velocities [rad/s] (n elements)
qdd: Joint accelerations [rad/s²] (n elements)
link_params: List of dicts with keys:
- 'mass': Link mass [kg]
- 'length': Link length [m]
- 'com': Distance from joint to COM [m]
- 'inertia': Moment of inertia about COM [kg·m²]
gravity: Gravitational acceleration [m/s²]
Returns:
tau: Joint torques [N·m]
"""
n = len(q)
# Initialize velocity and acceleration arrays
omega = np.zeros(n) # Angular velocities
omega_dot = np.zeros(n) # Angular accelerations
v_dot = np.zeros(n) # Linear accelerations at COM
v_dot_prev = np.zeros(n) # Linear accelerations at joint i-1
# Storage for link properties
r = np.zeros(n) # COM positions relative to previous joint
f = np.zeros(n) # Forces
tau = np.zeros(n) # Torques
# ============== OUTWARD RECURSION ==============
# Propagate velocities and accelerations from base to tip
for i in range(n):
# Angular velocity of link i
# omega_i = omega_{i-1} + qd_i (revolute joint adds qd_i * z)
if i == 0:
omega[i] = qd[i]
else:
omega[i] = omega[i-1] + qd[i]
# Angular acceleration of link i
# omega_dot_i = omega_dot_{i-1} + qdd_i
if i == 0:
omega_dot[i] = qdd[i]
else:
omega_dot[i] = omega_dot[i-1] + qdd[i]
# Position vector from joint i-1 to COM of link i
# For a uniform rod, COM is at l_i/2 from the joint
r[i] = link_params[i]['com']
# Linear acceleration at COM of link i
# v_dot_i = v_dot_{i-1} + omega_dot_i × r_i + omega_i × (omega_i × r_i)
# In 2D, cross products become:
# omega_dot × r = [0, 0, omega_dot] × [r, 0, 0] = [0, omega_dot*r, 0]
# omega × (omega × r) = -omega² * r (points inward)
if i == 0:
v_dot[i] = gravity + omega_dot[i] * r[i] - omega[i]**2 * r[i]
else:
# Add contribution from previous link motion
v_dot[i] = v_dot_prev[i] + omega_dot[i] * r[i] - omega[i]**2 * r[i]
v_dot_prev[i] = v_dot[i]
# ============== INWARD RECURSION ==============
# Propagate forces from tip to base and compute torques
for i in range(n-1, -1, -1):
mass = link_params[i]['mass']
inertia = link_params[i]['inertia']
# Force on link i from link i+1 (f_{i+1} on link i)
if i == n-1:
f_next = np.zeros(2) # No force from tip
else:
f_next = np.array([f[i+1], 0]) # x-component of force
# Linear acceleration of COM
a = v_dot[i]
# Force equation: f_i = f_{i+1} + m_i*v_dot_i - m_i*g
# In x-y: f_x = f_next_x + m*a_x, f_y = f_next_y + m*(a_y - g)
f[i] = f_next[0] + mass * a
# Torque at joint i
# tau_i = tau_{i+1} + r_i × f_i + I_i*omega_dot_i + omega_i × (I_i*omega_i)
# In 2D revolute joint, only z-component of torque is nonzero:
# r × f = r * f_y (in 2D planar motion)
if i == n-1:
tau_next = 0 # No torque from tip
else:
tau_next = tau[i+1]
# Coriolis term in 2D: omega × (I*omega) = 0 (since omega is scalar in 2D)
tau[i] = tau_next + r[i] * f[i] + inertia * omega_dot[i]
return tau
# Simplified version for 2-link planar robot
def newton_euler_2link(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2, qdd1, qdd2, g=9.81):
"""
Newton-Euler for 2-DOF planar robot (explicit formulation).
This verifies the recursive algorithm by computing the same result
as the manipulator equation.
"""
# Link parameters
link1 = {'mass': m1, 'length': l1, 'com': r1, 'inertia': I1}
link2 = {'mass': m2, 'length': l2, 'com': r2, 'inertia': I2}
# Outward recursion
# Joint 0
omega0 = qd1
omega_dot0 = qdd1
v_dot0 = g + omega_dot0 * r1 - omega0**2 * r1 # v_doti = g + alpha*r - omega²*r
# Joint 1 (relative to joint 0)
omega1 = omega0 + qd2
omega_dot1 = omega_dot0 + qdd2
# Linear acceleration includes contribution from joint 0 motion
v_dot1 = v_dot0 + omega_dot1 * r2 - omega1**2 * r2
# Inward recursion
# Link 2
f2 = m2 * v_dot1 # No external force at tip
tau2 = r2 * f2 + I2 * omega_dot1 # Only link 2 inertia contributes
# Link 1
f1 = f2 + m1 * v_dot0 # Add link 1's weight
tau1 = tau2 + r1 * f1 + I1 * omega_dot0 # Link 1 must support link 2
return np.array([tau1, tau2])
# Example usage
if __name__ == "__main__":
# Robot parameters
m1, m2 = 5.0, 3.0 # kg
l1, l2 = 1.0, 0.8 # m
r1, r2 = 0.5, 0.4 # m
I1, I2 = 0.1, 0.05 # kg·m²
# State
theta1, theta2 = np.pi/4, np.pi/6
qd1, qd2 = 0.5, 0.3
qdd1, qdd2 = 0.1, 0.2
# Link parameters for recursive algorithm
link_params = [
{'mass': m1, 'length': l1, 'com': r1, 'inertia': I1},
{'mass': m2, 'length': l2, 'com': r2, 'inertia': I2}
]
# Compute using Newton-Euler
tau_ne = newton_euler_step([theta1, theta2], [qd1, qd2], [qdd1, qdd2], link_params)
tau_explicit = newton_euler_2link(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2, qdd1, qdd2)
# Compare with Lagrangian formulation
tau_lagrangian = compute_manipulator_equation(m1, m2, l1, l2, r1, r2, I1, I2,
theta1, theta2, qd1, qd2, qdd1, qdd2)
print("Torque comparison:")
print(f" Newton-Euler (recursive): {tau_ne}")
print(f" Newton-Euler (explicit): {tau_explicit}")
print(f" Lagrangian: {tau_lagrangian}")
print(f" Match: {np.allclose(tau_ne, tau_lagrangian) and np.allclose(tau_ne, tau_explicit)}")
Code Breakdown¶
| Phase | Operation | Key Formula |
|---|---|---|
| Outward | Angular velocity | \(\omega_i = \omega_{i-1} + \dot{q}_i\) |
| Outward | Linear acceleration | \(\dot{v}_i = \dot{v}_{i-1} + \dot{\omega}_i r_i - \omega_i^2 r_i\) |
| Inward | Force propagation | \(f_i = f_{i+1} + m_i\dot{v}_i\) |
| Inward | Torque computation | \(\tau_i = \tau_{i+1} + r_i f_i + I_i\dot{\omega}_i\) |
Reference: See the double pendulum animation for visualization of force propagation.
Manipulator Equations of Motion¶
\[M(q)\ddot{q} + N(q,\dot{q}) = \tau\]
Where \(N\) represents all nonlinear terms including Coriolis, centrifugal, and gravity effects.
Dynamic Parameters¶
Key parameters affecting robot dynamics: - Link masses - Moments of inertia - Center of mass locations - Friction coefficients
Animation — Observe how joint torques vary with configuration:
Practical Considerations¶
Friction Models¶
- Coulomb friction
- Viscous friction
- Stiction (static + kinetic)
Actuator Dynamics¶
- Motor constants
- Gear ratios
- Backlash
