Week 05 | Day 07

Week 5 Summary: Dynamics & Control

Published: April 24, 2026 | Author: Smartotics Learning Journey | Reading Time: 10 min

This week we moved beyond geometry into the physics of robot motion. Dynamics is where robots transition from theoretical models to physical machines that must overcome inertia, compensate for gravity, and safely interact with the world.

Week 5 Overview

Learning Objectives

We’ve covered:

  1. Robot Dynamics Fundamentals: Mass properties, inertia tensors, Newton-Euler for rigid bodies
  2. Lagrangian Dynamics: Energy-based derivation of equations of motion
  3. Recursive Newton-Euler: O(n) algorithm for real-time dynamics computation
  4. Force and Impedance Control: Strategies for physical interaction with environments

Key Concepts Review

1. Dynamics vs. Kinematics

What we learned:

Key Implementation:

def robot_dynamics(q, dq, ddq, M, C, G):
    """
    Standard robot dynamics equation
    
    τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q)
    """
    return M @ ddq + C @ dq + G

2. Mass Matrix and Configuration-Dependent Inertia

What we learned:

Key Implementation:

def compute_2dof_mass_matrix(q1, q2, m1, m2, L1, L2):
    """Compute mass matrix for 2-DOF planar arm"""
    c2 = np.cos(q2)
    
    M11 = m1*L1**2/3 + m2*(L1**2 + L1*L2*c2 + L2**2/3)
    M12 = m2*(L1*L2*c2/2 + L2**2/3)
    M22 = m2*L2**2/3
    
    return np.array([[M11, M12], [M12, M22]])

3. Lagrangian Mechanics

What we learned:

Key Implementation:

# Extract mass matrix from kinetic energy
def extract_mass_matrix(T, dq_vars):
    """M[i,j] = coefficient of dq_i * dq_j in T"""
    n = len(dq_vars)
    M = np.zeros((n, n))
    for i in range(n):
        for j in range(n):
            if i == j:
                M[i,j] = 2 * T.coeff(dq_vars[i], 2)
            else:
                M[i,j] = T.coeff(dq_vars[i]*dq_vars[j], 1)
    return M

4. Recursive Newton-Euler Algorithm

What we learned:

Key Implementation:

# Forward pass
for i in range(n):
    omega[i] = omega[i-1] + dq[i] * z_axis
    alpha[i] = alpha[i-1] + ddq[i] * z_axis + cross(omega[i-1], dq[i]*z_axis)
    accel[i] = accel[i-1] + cross(alpha[i], r_i) + cross(omega[i], cross(omega[i], r_i))

# Backward pass
for i in range(n-1, -1, -1):
    f[i] = f[i+1] + m * a_com - F_external
    tau[i] = tau[i+1] + I@alpha + cross(omega, I@omega) + cross(r_com, f[i])

5. Force and Impedance Control

What we learned:

Key Implementation:

class ImpedanceController:
    def compute_force(self, x, dx, x_des, dx_des):
        pos_err = x_des - x
        vel_err = dx_des - dx
        return self.M @ ddx_err + self.B @ vel_err + self.K @ pos_err

Practical Applications

1. Industrial Robot Control

Industrial arms use recursive Newton-Euler at high frequency:

class IndustrialRobotController:
    def __init__(self, robot_model):
        self.dynamics = RecursiveNewtonEuler(robot_model)
        self.Kp = np.diag([200, 200, 150, 100, 80, 50])
        self.Kd = np.diag([40, 40, 30, 20, 16, 10])
    
    def control(self, q, dq, q_des, dq_des, ddq_des):
        # Feedforward: compute torques for desired trajectory
        tau_ff = self.dynamics.compute_dynamics(q_des, dq_des, ddq_des)
        
        # Feedback: PD correction
        tau_fb = self.Kp @ (q_des - q) + self.Kd @ (dq_des - dq)
        
        return tau_ff + tau_fb

2. Collaborative Robotics

Impedance control enables safe human-robot interaction:

# Soft impedance for human interaction
soft_impedance = ImpedanceController(
    M_virtual=5.0,    # Low inertia
    B_virtual=50.0,   # Moderate damping
    K_virtual=100.0   # Low stiffness
)

# When human pushes, robot complies like a soft spring
F_human = measure_human_force()
x_compliant = admittance_update(F_human, x_desired)

3. Precision Assembly

Hybrid control for peg-in-hole insertion:

# XY: position control (alignment)
# Z: force control (insertion)
hybrid = HybridController(
    S=[0, 0, 1],  # Z is force-controlled
    Kp=[500, 500, 0],
    Kf=5.0
)

Performance Comparison

AlgorithmComplexityUse Case
Symbolic LagrangianO(n²)Derivation, analysis, learning
Numeric LagrangianO(n²)Medium-speed simulation
Recursive Newton-EulerO(n)Real-time control, 1-8 kHz
Impedance controlO(n) per cycleHuman interaction, assembly
Hybrid controlO(n) per cycleTasks with natural decomposition

Integration: Complete Control Pipeline

class CompleteRobotController:
    """
    Full robot controller integrating dynamics and interaction control
    """
    
    def __init__(self, robot_model):
        self.dynamics = RecursiveNewtonEuler(robot_model)
        self.impedance = ImpedanceController(
            M_virtual=10.0, B_virtual=100.0, K_virtual=500.0
        )
        self.force_sensor = ForceSensor()
        
    def control_cycle(self, q, dq, task_type='free_motion'):
        """
        One control cycle (runs at 1 kHz)
        """
        # Measure state
        F_ext = self.force_sensor.read()
        
        # Select control strategy
        if task_type == 'free_motion':
            # Position control with feedforward
            tau = self.position_control(q, dq)
        
        elif task_type == 'contact':
            # Impedance control
            x = forward_kinematics(q)
            dx = jacobian(q) @ dq
            x_des = get_desired_position()
            
            F_cmd = self.impedance.compute_force(x, dx, x_des, np.zeros(3))
            tau = jacobian(q).T @ F_cmd
        
        elif task_type == 'hybrid':
            # Hybrid position/force
            tau = self.hybrid_control(q, dq, F_ext)
        
        # Add gravity compensation
        tau += self.dynamics.compute_dynamics(q, np.zeros_like(q), np.zeros_like(q))
        
        return tau

Week 5 Achievement Summary

Technical Skills Acquired

  1. Physical Intuition: Understanding how mass distribution affects robot motion
  2. Energy Methods: Using Lagrangian mechanics for systematic derivation
  3. Efficient Algorithms: Implementing O(n) recursive Newton-Euler
  4. Interaction Control: Designing controllers for contact tasks
  5. System Integration: Combining dynamics, kinematics, and control into complete systems

Key Takeaways

  1. Dynamics is essential: You cannot control what you don’t model
  2. Energy methods scale: Lagrangian mechanics handles complexity better than force balance
  3. Speed matters: Recursive algorithms enable real-time control
  4. Contact is different: Position control alone is insufficient for real-world interaction
  5. Impedance shapes behavior: The robot’s “feel” is programmable through M, B, K parameters

Week 6 Preview

Looking ahead to Week 6, we’ll explore:

1. Robot Perception and Sensing

2. Computer Vision for Robotics

3. SLAM (Simultaneous Localization and Mapping)

4. State Estimation

Practice Exercise for Week 5

To solidify your understanding, implement a comprehensive robot dynamics and control system:

  1. Derive dynamics: Use Lagrangian mechanics for a 2-DOF arm
  2. Implement RNE: Build the recursive algorithm and verify against Lagrangian result
  3. Simulate motion: Integrate equations forward in time under various controllers
  4. Compare controllers:
    • Position control (free space)
    • Force control (contact)
    • Impedance control (varying M, B, K)
  5. Analyze performance: Track energy conservation, compute power consumption, verify stability
def comprehensive_dynamics_project():
    """Complete Week 5 practice project"""
    
    # 1. Define robot
    robot = define_2dof_arm(m1=1.0, m2=0.8, L1=0.6, L2=0.5)
    
    # 2. Derive dynamics (symbolic)
    M_sym, C_sym, G_sym = derive_lagrangian_dynamics(robot)
    
    # 3. Implement RNE
    rne = RecursiveNewtonEuler(robot)
    
    # 4. Verify equivalence
    q_test = [0.5, 1.0]
    M_lag = evaluate_mass_matrix(M_sym, q_test)
    M_rne = compute_mass_matrix_rne(rne, q_test)
    assert np.allclose(M_lag, M_rne, atol=1e-10)
    
    # 5. Simulate under different controllers
    controllers = {
        'position': PositionController(Kp=100, Kd=20),
        'force': ForceController(Kf=2.0),
        'impedance_soft': ImpedanceController(M=5, B=50, K=100),
        'impedance_stiff': ImpedanceController(M=20, B=200, K=2000)
    }
    
    results = {}
    for name, ctrl in controllers.items():
        results[name] = simulate(robot, ctrl, duration=5.0)
    
    # 6. Analyze and compare
    compare_controllers(results)
    
    return results

if __name__ == "__main__":
    comprehensive_dynamics_project()

Conclusion

Week 5 has been a deep dive into the physics of robot motion. We’ve learned that dynamics is not just an extension of kinematics — it’s a fundamentally different perspective that considers forces, inertia, and energy. The equations of motion we’ve derived are the bridge between high-level task specifications and low-level motor commands.

The key insight is that modeling and control are inseparable. A perfect trajectory planner is useless without a controller that can achieve the required forces. Conversely, the best controller cannot compensate for a poorly modeled system. Mastering dynamics gives you the ability to predict how a robot will behave before it moves, and to design controllers that exploit the physics rather than fighting it.

As we move into Week 6, we’ll build upon this foundation by adding perception — giving our robots the ability to see, sense, and understand their environment. This completes the sense-plan-act loop that defines autonomous robotics.

Remember: the most elegant robot controller is one that works with physics, not against it. Gravity compensation, feedforward terms, and impedance shaping are not hacks — they’re expressions of physical understanding.

Further Reading

Disclaimer

For educational purposes only. This article is part of a structured learning curriculum and does not constitute professional engineering advice.