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