In [1]:
import modern_robotics as mr
import numpy as np

Modern Robotics Course 3: Robot Dynamics¶

Aug 12 2025

Goal:¶

Implement a function to simulate a trajectory for a UDF5 robot given initial conditions

Given robot specifications are:

  • home matrix
  • spatial inertia matrix
  • screw axes
In [2]:
M01 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.089159], [0, 0, 0, 1]]
M12 = [[0, 0, 1, 0.28], [0, 1, 0, 0.13585], [-1, 0, 0, 0], [0, 0, 0, 1]]
M23 = [[1, 0, 0, 0], [0, 1, 0, -0.1197], [0, 0, 1, 0.395], [0, 0, 0, 1]]
M34 = [[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0.14225], [0, 0, 0, 1]]
M45 = [[1, 0, 0, 0], [0, 1, 0, 0.093], [0, 0, 1, 0], [0, 0, 0, 1]]
M56 = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0.09465], [0, 0, 0, 1]]
M67 = [[1, 0, 0, 0], [0, 0, 1, 0.0823], [0, -1, 0, 0], [0, 0, 0, 1]]
G1 = np.diag([0.010267495893, 0.010267495893,  0.00666, 3.7, 3.7, 3.7])
G2 = np.diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393])
G3 = np.diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275])
G4 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219])
G5 = np.diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219])
G6 = np.diag([0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879])
Glist = [G1, G2, G3, G4, G5, G6]
Mlist = [M01, M12, M23, M34, M45, M56, M67] 
Slist = [[0,         0,         0,         0,        0,        0],
         [0,         1,         1,         1,        0,        1],
         [1,         0,         0,         0,       -1,        0],
         [0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491],
         [0,         0,         0,         0,  0.81725,        0],
         [0,         0,     0.425,   0.81725,        0,  0.81725]]

Initial conditions (for simulation 1):

  • in home position
  • not moving
  • no initial joint torques
  • no external wrench (acting on end-effector)

Essentially, robot is only affected by gravity

In [40]:
theta_0 = np.array([0,0,0,0,0,0])
dtheta_0 = np.zeros(6)
tau_0 = np.zeros(6)
F_tip = np.zeros(6)
theta_0, dtheta_0, F_tip, tau_0
Out[40]:
(array([0, 0, 0, 0, 0, 0]),
 array([0., 0., 0., 0., 0., 0.]),
 array([0., 0., 0., 0., 0., 0.]),
 array([0., 0., 0., 0., 0., 0.]))

Function needs to

  • accept initial conditions
  • perform forward dynamics to find acceleration
  • perform an integration step knowing position, velocity, and acceleration to find the next position and velocity
In [37]:
def write_trajectory(theta_0, dtheta_0, tau_0, F_tip, sim_time, N, file_name):
    """Provided initial conditions, simulate trajectory for sim_time with N steps and write to csv
    

    Input:

    theta_0: initial joint configuration
    dtheta_0: initial joint velocity
    tau_0: initial joint torque (if any)
    F_tip: wrench applied to end effector (if any)
    sim_time is the total time we'd like the trajectory to simulate
    N is number of integration steps we'd like to do over the simulation time
    file_name: where to save the joint configuration trajectory
    """

    g = np.array([0,0,-9.81])

    # Setup traces for forward and inverse calculations
    theta_trace = np.array(theta_0, ndmin=2)
    dtheta_trace = np.array(dtheta_0, ndmin=2)

    # timestep
    dt = sim_time/N

    for i in range(0,N):
    
        ddtheta = mr.ForwardDynamics(theta_trace[i],dtheta_trace[i],tau_0,g,F_tip,Mlist,Glist,Slist)    
        theta_next, dtheta_next = mr.EulerStep(theta_trace[i], dtheta_trace[i], ddtheta, dt)    
    
        theta_trace = np.vstack((theta_trace, theta_next))
        dtheta_trace = np.vstack((dtheta_trace, dtheta_next))

    np.savetxt(file_name, theta_trace, delimiter=",")
In [38]:
write_trajectory(theta_0, dtheta_0, tau_0, F_tip, 3, 3000, "simulation1.csv")

Simulation 1:

In [42]:
from IPython.display import YouTubeVideo

# Replace with the YouTube video ID (the part after `v=` in the URL)
YouTubeVideo("bqfhuXzYpPI", width=640, height=360)
Out[42]:

Simulation 2:

In [43]:
from IPython.display import YouTubeVideo

# Replace with the YouTube video ID (the part after `v=` in the URL)
YouTubeVideo("U6cf2WuVPiM", width=640, height=360)
Out[43]:

Worth noting in both videos is the slight increase in energy that is visible. Unless enough integration steps are used (small enough dt), the error or drift introduced creates energy. A more sophisticated integration formula is required to remove this kind of error

In [ ]:
In [ ]: