Question

我的任务是在python中模拟3自由度机械臂系统。目的是找出3D高斯曲面。输入数据是使用3D高斯方程获得的末端执行器的坐标。该代码应求解三角方程组，以找出角度theta1，theta2和theta3。我使用“ solve（[eqn1，eqn2，eqn3]，theta1，theta2，theta3）”来求解方程组。 Python花费的时间太长了。

我尝试使用fsolve进行求解，但是结果与MATLAB的解决方案无法相比。顺便说一句，我在MATLAB中使用“ solve”进行了很好的仿真。

我正在Windows 10的Anaconda 2019.03环境下通过spyder使用Python 3.7.3。

生成数据集（高斯表面）：P矩阵


a = 8
x0 = 0
y0 = 0

sigmax = 3
sigmay = 3

P_x = np.arange(7,-8,-1)
P_y = np.arange(7,-8,-1)

xx , yy = np.meshgrid(P_x,P_y)

s_x,s_y = np.subtract(xx,x0),np.subtract(yy,y0)

sq_x,sq_y = np.square(s_x),np.square(s_y)

X,Y = np.divide(sq_x,2*(sigmax**2)),np.divide(sq_y,2*(sigmay**2))

E = np.exp(-X-Y)

zz = np.multiply(E,a)

xx,yy,zz = xx.flatten(),yy.flatten(),zz.flatten()
P = np.vstack([yy,xx,zz]).T

import numpy as np
from sympy import *
from sympy import Symbol, solve, Eq

for j in range(len(P)):

    theta1 = Symbol('theta1',real = True)
    theta2 = Symbol('theta2',real = True)
    theta3 = Symbol('theta3',real = True)

    x,y,z = P[j,0],P[j,1],P[j,2]

    eq1 = Eq(R1*cos(theta1) + R2*cos(theta1)*cos(theta2) + 
    R3*cos(theta1)*cos(theta3) - x)

    eq2 = Eq(R1*sin(theta1) + R2*cos(theta2)*sin(theta1) + 
    R3*cos(theta3)*sin(theta1) - y)

    eq3 = Eq(R2*sin(theta2) + R3*sin(theta3) -z )

    solve([eq1,eq2,eq3],theta1,theta2,theta3)

    solution[j,0] = degrees(theta1)

    solution[j,1] = degrees(theta3)

    solution[j,2] = degrees(theta2)

预期结果：解决方案矩阵，值分别为theta1，theta2和theta3。

Python需要太多时间来进行响应。

Python中的同时三角函数方程

0 个答案: