使用 Sympy 求解逆运动学

Question

我想用 Sympy 解决简单的二维逆运动学问题。我知道 x 和 y 位置的正向运动方程。

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

如果我知道这两个方程，如何用 Sympy 求解 theta1 和 theta2 值？

Answer 1

我认为这些方程没有解决方案，但如果有，您可以使用以下方法：

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)

我试图使用 sympy nonlinsolve 求解器来解决类似的逆运动学问题，但在docs 中注意到了这条评论：

目前 nonlinsolve 不能正确地求解具有三角函数的方程组。 solve 可以用于这种情况（但不给出所有解决方案）