Skip to content

Instantly share code, notes, and snippets.

@mdecourse

mdecourse/meArm_ik_symbolic.py

Created October 16, 2021 09:25
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save mdecourse/f94f353570340fddab22c08656fc92de to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save mdecourse/f94f353570340fddab22c08656fc92de to your computer and use it in GitHub Desktop.
Download ZIP
cad2021 meArm IK
Raw
meArm_ik_symbolic.py
import sympy as sp
# Define symbols
theta1, theta2, theta3, theta4, l1, l3, l4, l6, xb,zb = sp.symbols("theta1 theta2 theta3 theta4 l1 l3 l4 l6 xb zb")
# Define equations, rearranged so expressions equal 0
eq1 = l1 * sp.cos(theta1) + l3 * sp.cos(theta2) - xb
eq2 = l1 * sp.sin(theta1) - l3 * sp.sin(theta2) - zb
eq3 = l4 * sp.cos(theta3) + l6*sp.cos(theta4) - xb
eq4 = l4 * sp.sin(theta3) - l6*sp.sin(theta4) - zb
# Solve for theta1 & theta2
solution = sp.solve([eq1, eq2, eq3, eq4], [theta1, theta2], dict=True)
print(solution)
'''
[{theta1: -acos(-(l3*cos(2*atan((2*l3*zb - sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))) - xb)/l1) + 2*pi,
theta2: -2*atan((2*l3*zb - sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))},
{theta1: -acos(-(l3*cos(2*atan((2*l3*zb + sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))) - xb)/l1) + 2*pi,
theta2: -2*atan((2*l3*zb + sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))},
{theta1: acos(-(l3*cos(2*atan((2*l3*zb - sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))) - xb)/l1),
theta2: -2*atan((2*l3*zb - sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))},
{theta1: acos(-(l3*cos(2*atan((2*l3*zb + sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))) - xb)/l1),
theta2: -2*atan((2*l3*zb + sqrt(-l1**4 + 2*l1**2*l3**2 + 2*l1**2*xb**2 + 2*l1**2*zb**2 - l3**4 + 2*l3**2*xb**2 + 2*l3**2*zb**2 - xb**4 - 2*xb**2*zb**2 - zb**4))/(-l1**2 + l3**2 + 2*l3*xb + xb**2 + zb**2))}]
'''
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment