17-3-1 Jansen Linkage Analysis Code
This is for the people who would like a more technical breakdown of the code. First the entire code:
import numpy as np
import matplotlib.pyplot as plt
# 1. Positional Solver
def solve_dyad_analytical(Rx, Ry, L_a, L_b, delta):
R_mag = np.sqrt(Rx**2 + Ry**2)
phi = np.arctan2(Ry, Rx)
arg = (L_a**2 + R_mag**2 - L_b**2) / (2 * L_a * R_mag)
alpha = np.arccos(np.clip(arg, -1, 1))
theta_a = phi + delta * alpha
theta_b = np.arctan2(Ry - L_a*np.sin(theta_a), Rx - L_a*np.cos(theta_a))
return theta_a, theta_b
# 2. Velocity Solver
def solve_dyad_velocity(th_a, th_b, L_a, L_b, dRx, dRy):
det = L_a * L_b * np.sin(th_b - th_a)
if np.abs(det) < 1e-9: return 0.0, 0.0
dth_a = (dRx * np.cos(th_b) + dRy * np.sin(th_b)) / (L_a * np.sin(th_b - th_a))
dth_b = -(dRx * np.cos(th_a) + dRy * np.sin(th_a)) / (L_b * np.sin(th_b - th_a))
return dth_a, dth_b
# 3. Acceleration Solver
def solve_dyad_acceleration(th_a, th_b, dth_a, dth_b, L_a, L_b, ddRx, ddRy):
det = L_a * L_b * np.sin(th_b - th_a)
if np.abs(det) < 1e-9: return 0.0, 0.0
Kx = ddRx + L_a * (dth_a**2) * np.cos(th_a) + L_b * (dth_b**2) * np.cos(th_b)
Ky = ddRy + L_a * (dth_a**2) * np.sin(th_a) + L_b * (dth_b**2) * np.sin(th_b)
ddth_a = (Kx * np.cos(th_b) + Ky * np.sin(th_b)) / (L_a * np.sin(th_b - th_a))
ddth_b = -(Kx * np.cos(th_a) + Ky * np.sin(th_a)) / (L_b * np.sin(th_b - th_a))
return ddth_a, ddth_b
# 4. Kinematics Propagator
def jansen_kinematics(th2_deg, omega_crank, L, deltas):
th2 = np.radians(th2_deg)
O = np.array([0.0, 0.0])
A = np.array([-38.0, -7.8])
# Crank Pin
i = O + np.array([L['r2'] * np.cos(th2), L['r2'] * np.sin(th2)])
vi = np.array([-L['r2'] * omega_crank * np.sin(th2), L['r2'] * omega_crank * np.cos(th2)])
ai = np.array([-L['r2'] * (omega_crank**2) * np.cos(th2), -L['r2'] * (omega_crank**2) * np.sin(th2)])
# Loop 1 (i-j-A)
R1 = A - i
dR1 = -vi
ddR1 = -ai
th3, th4 = solve_dyad_analytical(R1[0], R1[1], L['r3'], L['r4'], deltas[0])
dth3, dth4 = solve_dyad_velocity(th3, th4, L['r3'], L['r4'], dR1[0], dR1[1])
ddth3, ddth4 = solve_dyad_acceleration(th3, th4, dth3, dth4, L['r3'], L['r4'], ddR1[0], ddR1[1])
j = i + np.array([L['r3']*np.cos(th3), L['r3']*np.sin(th3)])
vj = vi + np.array([-L['r3']*dth3*np.sin(th3), L['r3']*dth3*np.cos(th3)])
aj = ai + np.array([-L['r3']*ddth3*np.sin(th3) - L['r3']*(dth3**2)*np.cos(th3),
L['r3']*ddth3*np.cos(th3) - L['r3']*(dth3**2)*np.sin(th3)])
# Loop 2 (i-m-A)
th10, th8 = solve_dyad_analytical(R1[0], R1[1], L['r10'], L['r8'], deltas[1])
dth10, dth8 = solve_dyad_velocity(th10, th8, L['r10'], L['r8'], dR1[0], dR1[1])
ddth10, ddth8 = solve_dyad_acceleration(th10, th8, dth10, dth8, L['r10'], L['r8'], ddR1[0], ddR1[1])
m = i + np.array([L['r10']*np.cos(th10), L['r10']*np.sin(th10)])
vm = vi + np.array([-L['r10']*dth10*np.sin(th10), L['r10']*dth10*np.cos(th10)])
am = ai + np.array([-L['r10']*ddth10*np.sin(th10) - L['r10']*(dth10**2)*np.cos(th10),
L['r10']*ddth10*np.cos(th10) - L['r10']*(dth10**2)*np.sin(th10)])
# Loop 3 (j-k-A)
R2 = A - j
dR2 = -vj
ddR2 = -aj
th5, th6 = solve_dyad_analytical(R2[0], R2[1], L['r5'], L['r6'], deltas[2])
dth5, dth6 = solve_dyad_velocity(th5, th6, L['r5'], L['r6'], dR2[0], dR2[1])
ddth5, ddth6 = solve_dyad_acceleration(th5, th6, dth5, dth6, L['r5'], L['r6'], ddR2[0], ddR2[1])
k = j + np.array([L['r5']*np.cos(th5), L['r5']*np.sin(th5)])
vk = vj + np.array([-L['r5']*dth5*np.sin(th5), L['r5']*dth5*np.cos(th5)])
ak = aj + np.array([-L['r5']*ddth5*np.sin(th5) - L['r5']*(dth5**2)*np.cos(th5),
L['r5']*ddth5*np.cos(th5) - L['r5']*(dth5**2)*np.sin(th5)])
# Loop 4 (k-l-m)
R3 = m - k
dR3 = vm - vk
ddR3 = am - ak
th7, th9 = solve_dyad_analytical(R3[0], R3[1], L['r7'], L['r9'], deltas[3])
dth7, dth9 = solve_dyad_velocity(th7, th9, L['r7'], L['r9'], dR3[0], dR3[1])
ddth7, ddth9 = solve_dyad_acceleration(th7, th9, dth7, dth9, L['r7'], L['r9'], ddR3[0], ddR3[1])
l = k + np.array([L['r7']*np.cos(th7), L['r7']*np.sin(th7)])
vl = vk + np.array([-L['r7']*dth7*np.sin(th7), L['r7']*dth7*np.cos(th7)])
al = ak + np.array([-L['r7']*ddth7*np.sin(th7) - L['r7']*(dth7**2)*np.cos(th7),
L['r7']*ddth7*np.cos(th7) - L['r7']*(dth7**2)*np.sin(th7)])
# Loop 5 (l-n-m) -> Toe Point n
R4 = m - l
dR4 = vm - vl
ddR4 = am - al
th12, th11 = solve_dyad_analytical(R4[0], R4[1], L['r12'], L['r11'], deltas[4])
dth12, dth11 = solve_dyad_velocity(th12, th11, L['r12'], L['r11'], dR4[0], dR4[1])
ddth12, ddth11 = solve_dyad_acceleration(th12, th11, dth12, dth11, L['r12'], L['r11'], ddR4[0], ddR4[1])
n = l + np.array([L['r12']*np.cos(th12), L['r12']*np.sin(th12)])
vn = vl + np.array([-L['r12']*dth12*np.sin(th12), L['r12']*dth12*np.cos(th12)])
an = al + np.array([-L['r12']*ddth12*np.sin(th12) - L['r12']*(dth12**2)*np.cos(th12),
L['r12']*ddth12*np.cos(th12) - L['r12']*(dth12**2)*np.sin(th12)])
return n, vn, an
# 5. Data and Execution
L_vals = {
'r2': 15.0, 'r3': 50.0, 'r4': 41.5, 'r5': 55.8, 'r6': 40.1,
'r7': 39.4, 'r8': 39.3, 'r9': 36.7, 'r10': 61.9, 'r11': 49.0, 'r12': 65.7
}
deltas = [-1, 1, -1, -1, -1]
omega_crank = 1.0
torque_motor = 150.0 # N-mm
# Pre-scan for Ground Level
temp_y = [jansen_kinematics(d, omega_crank, L_vals, deltas)[0][1] for d in np.linspace(0, 360, 100)]
ground_level = min(temp_y)
ground_threshold = 2.0 # mm
th2_vec = np.linspace(0, 360, 500)
pos_list, vel_list, acc_list, ground_force_list = [], [], [], []
for deg in th2_vec:
p, v, a = jansen_kinematics(deg, omega_crank, L_vals, deltas)
# Calculate Ground Force (Only if toe is at ground level)
if p[1] <= (ground_level + ground_threshold):
v_mag = np.sqrt(v[0]**2 + v[1]**2)
f_out = (torque_motor * omega_crank) / (v_mag + 1e-6)
else:
f_out = 0.0
pos_list.append(p)
vel_list.append(v)
acc_list.append(a)
ground_force_list.append(f_out)
pos_list = np.array(pos_list)
vel_list = np.array(vel_list)
acc_list = np.array(acc_list)
ground_force_list = np.array(ground_force_list)
# 6. Plotting Dashboard
fig = plt.figure(figsize=(14, 18))
# PLOT 1: Position
ax_pos = plt.subplot(4, 2, (1, 2))
ax_pos.plot(pos_list[:, 0], pos_list[:, 1], 'r-', linewidth=2, label="Toe Path")
ax_pos.axhline(y=ground_level, color='k', linestyle='--', alpha=0.5, label="Ground")
ax_pos.scatter([0, -38], [0, -7.8], c='k', marker='o', s=50, label="Pivots")
ax_pos.set_aspect('equal')
ax_pos.set_title("1. Positional Analysis: Toe Trace", fontsize=14, fontweight='bold')
ax_pos.legend()
# PLOT 2 & 3: Velocity
ax_vx = plt.subplot(4, 2, 3); ax_vx.plot(th2_vec, vel_list[:, 0], 'g-'); ax_vx.set_title("2. X-Velocity")
ax_vy = plt.subplot(4, 2, 4); ax_vy.plot(th2_vec, vel_list[:, 1], 'm-'); ax_vy.set_title("3. Y-Velocity")
# PLOT 4 & 5: Acceleration
ax_ax = plt.subplot(4, 2, 5); ax_ax.plot(th2_vec, acc_list[:, 0], 'b-'); ax_ax.set_title("4. X-Acceleration")
ax_ay = plt.subplot(4, 2, 6); ax_ay.plot(th2_vec, acc_list[:, 1], 'c-'); ax_ay.set_title("5. Y-Acceleration")
# PLOT 6: Ground Contact Force (The New Addition)
ax_force = plt.subplot(4, 2, (7, 8))
ax_force.fill_between(th2_vec, ground_force_list, color='orange', alpha=0.3)
ax_force.plot(th2_vec, ground_force_list, 'r-', linewidth=2)
ax_force.set_title("6. Vertical Load Capacity (Ground Contact Only)", fontsize=14, fontweight='bold')
ax_force.set_xlabel("Crank Angle (deg)")
ax_force.set_ylabel("Force (N)")
ax_force.grid(True, linestyle=':')
plt.tight_layout()
plt.show()Now we will go over some key parts of the code. I have used ClaudeAI to generate a helpful visualization to see every loop that that is solved and what “parts” it needs.
Figure 1. Visualization of solved Loops (AI generated)
First the positional solver:
# 1. Positional Solver
def solve_dyad_analytical(Rx, Ry, L_a, L_b, delta):
R_mag = np.sqrt(Rx**2 + Ry**2)
phi = np.arctan2(Ry, Rx)
arg = (L_a**2 + R_mag**2 - L_b**2) / (2 * L_a * R_mag)
alpha = np.arccos(np.clip(arg, -1, 1))
theta_a = phi + delta * alpha
theta_b = np.arctan2(Ry - L_a*np.sin(theta_a), Rx - L_a*np.cos(theta_a))
return theta_a, theta_bGiven a target vector (Rx, Ry) and two link lengths the positional solver is able to use the law of cosines to find two angles which define the exact orientation of the two links in the dyad. Delta is used to correct for the direction in which the join bends. Phi is the offset angle between the two known joints. Alpha is “interior” angle (A-i-j). And theta_a and theta_b are the absolute angles we are solving for such as (x-axis-i-j).
The velocity solver solve_dyad_velocity and acceleration solver solve_dyad_acceleration simply take the derivative of position once for velocity and twice for acceleration.
The jansen_kinematics part of the code solves for each point for a total of 6 of them. 5 through dyads. The first one is the simplest.
# Crank Pin
i = O + np.array([L['r2'] * np.cos(th2), L['r2'] * np.sin(th2)])
vi = np.array([-L['r2'] * omega_crank * np.sin(th2), L['r2'] * omega_crank * np.cos(th2)])
ai = np.array([-L['r2'] * (omega_crank**2) * np.cos(th2), -L['r2'] * (omega_crank**2) * np.sin(th2)])To solve for the crank pin you are given the length of the crank link length and the input angle. This is all you need to use simple geometry and some derivatives to solve. However this is an important step as it gives us the key “2nd point” to solve all the dyads from. I will use Loop 1 as an example.
# Loop 1 (i-j-A)
R1 = A - i
dR1 = -vi
ddR1 = -ai
th3, th4 = solve_dyad_analytical(R1[0], R1[1], L['r3'], L['r4'], deltas[0])
dth3, dth4 = solve_dyad_velocity(th3, th4, L['r3'], L['r4'], dR1[0], dR1[1])
ddth3, ddth4 = solve_dyad_acceleration(th3, th4, dth3, dth4, L['r3'], L['r4'], ddR1[0], ddR1[1])
j = i + np.array([L['r3']*np.cos(th3), L['r3']*np.sin(th3)])
vj = vi + np.array([-L['r3']*dth3*np.sin(th3), L['r3']*dth3*np.cos(th3)])
aj = ai + np.array([-L['r3']*ddth3*np.sin(th3) - L['r3']*(dth3**2)*np.cos(th3),
L['r3']*ddth3*np.cos(th3) - L['r3']*(dth3**2)*np.sin(th3)])To solve any dyad you need to location of two points. In this case we have the position of the ground pin A and we just solved for the crank pin i. We also have the link lengths of both. This gives us 3 sides of the triangle that we can then use law of cosine on and solve for angles required to find the exact location of pin joint j.
Figure 2. Positional Analysis of Toe for Jansen Linkage
This process is repeated for the rest of the pin joints to get a precise location of the toe which is then repeated for a whole 360 degree loop of the input crank and then plotted (Figure 2)
This provides a very elegant way to solve an otherwise very complicated problem.