1

I want to solve simple 2D inverse kinematic problem with Sympy. I know forward kinematic equation of x and y position. 

x = l1*cos(theta1) + l2*cos(theta1+theta2)
y = l1*sin(theta1) + l2*sin(theta1+theta2)

How to solve theta1 and theta2 value with Sympy if I know those two equations?

Sociopath
  • 13,068
  • 19
  • 47
  • 75
Eko Rudiawan
  • 187
  • 3
  • 9

1 Answers1

0

I don't think there is a solution to those equations, but if there were you could use the follwing method: 

import sympy as sp

# Define symbols
theta1, theta2, l1, l2, x, y = sp.symbols("theta1 theta2 l1 l2 x y")

# Define equations, rearranged so expressions equal 0
eq1 = l1 * sp.cos(theta1) + l2 * sp.cos(theta1 + theta2) - x
eq2 = l1 * sp.sin(theta1) + l2 * sp.sin(theta1 + theta2) - y

# Solve for theta1 & theta2
solution = sp.solve([eq1, eq2], [theta1, theta2], dict=True)
print(solution)

I was trying to use the sympy nonlinsolve solver for a similar inverse kinematics problem but noticed this comment in the docs:

Currently nonlinsolve is not properly capable of solving the system of equations having trigonometric functions. solve can be used for such cases (but does not give all solution)

bill88
  • 13
  • 4