two-link-kinematics.py¶
This script demonstrates the symbolic derivation of equations for forward and inverse kinematics for a planar two-link arm. It uses SymPy, a symbolic algebra package for Python, to solve the equations.
1"""Kinematics calculations for a two-link serial chain (e.g., double-pendulum).
2
3Given the link lengths, calculate the endpoint positions.
4
5Note that since this is a serial drive mechanism, the forward kinematics is
6simpler than the inverse kinematics.
7
8Q0: shoulder angle in radians, zero pointing down, positive is CCW
9Q1: elbow angle in radians relative to upper link, zero pointing down, positive is CCW
10
11"""
12import sympy as sym
13
14##### forward kinematics #############
15L1, L2, x, y, q0, q1, theta = sym.symbols('L1 L2 x y q0 q1 theta')
16
17# The elbow location is uniquely by the shoulder angle q0.
18elbow = sym.Matrix([L1 * sym.sin(q0), -L1 * sym.cos(q0)])
19
20# The endpoint location locus is a circle centered on the elbow.
21endpoint = elbow + sym.Matrix([L2 * sym.sin(q0+q1), -L2 * sym.cos(q0+q1)])
22
23print("\n\nForward kinematics, i.e. endpoint location as a function of joint angles:")
24print(endpoint)
25
26##### inverse kinematics #############
27
28# the square of the target distance
29R_squared = x**2 + y**2
30
31# apply the law of cosines to calculate the elbow angle based on the target distance and the link lengths
32elbowEqn = sym.Eq(R_squared, L1**2 + L2**2 - 2*L1*L2*sym.cos(sym.pi - q1))
33soln = sym.solve(elbowEqn, [q1])
34
35print("\n\nElbow solution:", soln)
36
37# the overall angle from the center to the endpoint
38theta = sym.atan2(x, -y)
39
40# apply the law of sines
41alphaEqn = sym.Eq(L2 * sym.sin(sym.pi - q1), sym.sqrt(R_squared) * sym.sin(theta - q0))
42
43soln = sym.solve([elbowEqn, alphaEqn], [q0, q1])
44
45print("\n\nInverse kinematics, i.e. joint angles as a function of endpoint location:")
46print(soln)
47
48##### demonstrate a solution ##############
49
50values = {L1:0.5, L2:0.5, x:0.01, y:-0.99}
51angles = [[expr[0].evalf(subs=values), expr[1].evalf(subs=values)] for expr in soln]
52
53print("\n\nangle solutions:", angles)
Sample Output¶
Forward kinematics, i.e. endpoint location as a function of joint angles:
Matrix([[L1*sin(q0) + L2*sin(q0 + q1)], [-L1*cos(q0) - L2*cos(q0 + q1)]])
Elbow solution: [-acos(-(L1**2 + L2**2 - x**2 - y**2)/(2*L1*L2)) + 2*pi, acos((-L1**2 - L2**2 + x**2 + y**2)/(2*L1*L2))]
Inverse kinematics, i.e. joint angles as a function of endpoint location:
[(-asin(L2*sqrt((2*L1**2*L2**2 + L1**2*(-L1**2 + 2*x**2 + 2*y**2) + L2**2*(-L2**2 + 2*x**2 + 2*y**2) - x**4 - 2*x**2*y**2 - y**4)/(L1**2*L2**2))/(2*sqrt(x**2 + y**2))) + atan2(x, -y), acos((-L1**2 - L2**2 + x**2 + y**2)/(2*L1*L2))), (asin(L2*sqrt((2*L1**2*L2**2 + L1**2*(-L1**2 + 2*x**2 + 2*y**2) + L2**2*(-L2**2 + 2*x**2 + 2*y**2) - x**4 - 2*x**2*y**2 - y**4)/(L1**2*L2**2))/(2*sqrt(x**2 + y**2))) + atan2(x, -y), -acos(-(L1**2 + L2**2 - x**2 - y**2)/(2*L1*L2)) + 2*pi), (-asin(L2*sqrt((2*L1**2*L2**2 + L1**2*(-L1**2 + 2*x**2 + 2*y**2) + L2**2*(-L2**2 + 2*x**2 + 2*y**2) - x**4 - 2*x**2*y**2 - y**4)/(L1**2*L2**2))/(2*sqrt(x**2 + y**2))) + atan2(x, -y) + pi, -acos(-(L1**2 + L2**2 - x**2 - y**2)/(2*L1*L2)) + 2*pi), (asin(L2*sqrt((2*L1**2*L2**2 + L1**2*(-L1**2 + 2*x**2 + 2*y**2) + L2**2*(-L2**2 + 2*x**2 + 2*y**2) - x**4 - 2*x**2*y**2 - y**4)/(L1**2*L2**2))/(2*sqrt(x**2 + y**2))) + atan2(x, -y) + pi, acos((-L1**2 - L2**2 + x**2 + y**2)/(2*L1*L2)))]
angle solutions: [[-0.131080344167084, 0.282362021504811], [0.151281677337727, 6.00082328567478], [3.01051230942271, 6.00082328567478], [3.29287433092752, 0.282362021504811]]