Sympy：一旦找到解决方案（在找到所有解决方案之前），是否可以让求解器返回？

Question

我正在开发一个项目，我正在编写一个虚拟机器人 arm。 在脚本中，我为机器人的末端执行器生成了大约 1350 个目标 [x, y, z]，然后我使用坐标来计算机器人关节的 3 个角度。 我正在使用sympy.solve ：

`defsolve_robot（坐标_x，坐标_y）：

try:
    w, z = sym.symbols('w, z')

    # Sympy solver cannot handle trig functions that contain a symbolic and a float.
    # Have to round to integer to work around. (Introduces rounding error to calculation).
    angle = round(radians(90))

    eq1 = sym.Eq(60 * sym.sin(w) + 80 * sym.cos(z - angle), coordinate_x)
    eq2 = sym.Eq(37.03 + 60 * sym.cos(w) - 80 * sym.sin(z - angle) - 20, coordinate_y)

    result = sym.solve([eq1, eq2], (w, z))

    if len(result) > 0:                     #
        omega, beta = result[0]             #
        omega = round(degrees(omega), 2)    # If boom 1 angle smaller than -26 degrees then a collision
        if omega < -26.0:                   # with the robot body will occur. Thus, output invalid result.
            result = []                     #

    return result
except:  # (To handle problems) If solver is unable to solve, return empty.
    return []`

完成所有计算大约需要 7 分钟。 我已经尝试过manual=True、simplify=False等标志，但仍然需要很长时间。 几乎我所有的解决方案都有 2 个解决方案，有没有办法在找到一个解决方案后强制 sympy 停止？ 那么理论上代码会快一倍吗？

Answer 1

我不确定你为什么要四舍五入radians(90) 。 如果您想要以弧度表示的精确角度 90 度，请使用 SymPy 的pi/2 。 这是你的方程式：

from sympy import *

x, y, w, z = symbols('x, y, w, z')

angle = pi/2

eq1 = Eq(60 * sin(w) + 80 * cos(z - angle), x)
eq2 = Eq(37.03 + 60 * cos(w) - 80 * sin(z - angle) - 20, y)

eqs = [eq1, eq2]

pi/2自然简化，因此方程变为：

In [17]: eq1
Out[17]: 60⋅sin(w) + 80⋅sin(z) = x

In [18]: eq2
Out[18]: 60⋅cos(w) + 80⋅cos(z) + 17.03 = y

如果我们转换为具有替换sw = sin(w) 、 cw = cos(w)等的多项式，我们可以使用 Groebner 基解决这种系统：

In [23]: sw, cw, sz, cz = symbols('sw, cw, sz, cz')

In [24]: rep = {sin(w): sw, cos(w): cw, sin(z): sz, cos(z): cz}

In [25]: eqs2 = [eq.subs(rep) for eq in eqs]

In [26]: eqs2
Out[26]: [60⋅sw + 80⋅sz = x, 60⋅cw + 80⋅cz + 17.03 = y]

我们现在有四个未知数的两个方程，但我们知道我们可以制作新的多项式方程，因为sin(x)**2 + cos(x)**2 = 1 ：

In [27]: eqs3 = eqs2 + [Eq(sz**2 + cz**2, 1), Eq(sw**2 + cw**2, 1)]

In [28]: for eq in eqs3: pprint(eq)
60⋅sw + 80⋅sz = x
60⋅cw + 80⋅cz + 17.03 = y
  2     2    
cz  + sz  = 1
  2     2    
cw  + sw  = 1

我们将计算 Groebner 基，但为此最好使用精确的有理数而不是浮点数：

In [30]: eqs4 = [nsimplify(eq) for eq in eqs3]

In [31]: eqs4
Out[31]: 
⎡                                   1703        2     2        2     2    ⎤
⎢60⋅sw + 80⋅sz = x, 60⋅cw + 80⋅cz + ──── = y, cz  + sz  = 1, cw  + sw  = 1⎥
⎣                                   100                                   ⎦

现在我们可以为这些多项式计算 Groebner 基

In [35]: gb = groebner(eqs4, [sw, sz, cw, cz])

In [36]: for eq in gb: pprint(eq)
                           2     2                    
   ⎛1703   4⋅y⎞           x     y    1703⋅y   30900209
cz⋅⎜──── - ───⎟ + sw⋅x - ─── + ─── - ────── + ────────
   ⎝ 75     3 ⎠          120   120    6000    1200000 
                         2     2                    
   ⎛    1703⎞           x     y    1703⋅y   30900209
cz⋅⎜y - ────⎟ + sz⋅x - ─── - ─── + ────── - ────────
   ⎝    100 ⎠          160   160    8000    1600000 
     4⋅cz   y    1703
cw + ──── - ── + ────
      3     60   6000
                                      ⎛   2           2    3         2                           ⎞ 
  2 ⎛ 2    2   1703⋅y   2900209⎞      ⎜  x ⋅y   1703⋅x    y    5109⋅y    36700627⋅y   52623055927⎟ 
cz ⋅⎜x  + y  - ────── + ───────⎟ + cz⋅⎜- ──── + ─────── - ── + ─────── - ────────── + ───────────⎟ 
    ⎝            50      10000 ⎠      ⎝   80      8000    80     8000      800000       80000000 ⎠ 

     4     2  2         2               2      4          3             2                          
    x     x ⋅y    1703⋅x ⋅y   97099791⋅x      y     1703⋅y    36700627⋅y    52623055927⋅y   9548229
+ ───── + ───── - ───────── - ─────────── + ───── - ─────── + ─────────── - ───────────── + ───────
  25600   12800     640000     128000000    25600    640000    128000000      6400000000     256000

        
16243681
────────
0000000 

最终方程是cz的二次方程。 前 3 个在sw 、 sz和cw中是线性的（尽管x=0处的奇异值需要特别处理）。 因此，我们可以像这样计算解决方案：

In [40]: gb = groebner(eqs4, [sw, sz, cw, cz])

In [41]: [lsol] = linsolve(gb[:-1], [sw, sz, cw])

In [42]: cz1, cz2 = roots(gb[-1], cz)

In [43]: sol1 = lsol.subs(cz, cz1) + (cz1,)

In [44]: sol2 = lsol.subs(cz, cz2) + (cz2,)

这两个表达式sol1和sol2是就参数x和y而言的解的一般形式。 您可以将一些特定值替换为那些以获得数字答案：

In [49]: sol1.subs({x:100, y:100})
Out[49]: 
⎛704201045    8297⋅√4477025624836319  8297⋅√4477025624836319   984201045   5⋅√4477025624836319   11
⎜────────── - ──────────────────────, ────────────────────── + ──────────, ─────────────────── + ──
⎝1013041254       2026082508000           2701443344000        1350721672       1013041254       20

68551214073  1633183214073   5⋅√4477025624836319⎞
───────────, ───────────── - ───────────────────⎟
26082508000  2701443344000        1350721672    ⎠

In [50]: [s.n(3) for s in sol1.subs({x:100, y:100})]
Out[50]: [0.421, 0.934, 0.907, 0.357]

In [51]: [s.n(3) for s in sol2.subs({x:100, y:100})]
Out[51]: [0.969, 0.523, 0.247, 0.852]

当然这些是sz等的答案，但你想要z本身。 我们可以使用atan2恢复z和w ，然后使用lambdify进行更快的评估：

In [52]: swsol, szsol, cwsol, czsol = sol1

In [54]: zsol = atan2(szsol, czsol)

In [55]: wsol = atan2(swsol, cwsol)

In [56]: f = lambdify((x, y), (zsol, wsol))

In [57]: f(100, 100)
Out[57]: (1.2058759278150635, 0.4346913079154993)

In [58]: %time f(100, 100)
CPU times: user 0 ns, sys: 0 ns, total: 0 ns
Wall time: 239 µs
Out[58]: (1.2058759278150635, 0.4346913079154993)

In [59]: %time f(102, 95)
CPU times: user 0 ns, sys: 0 ns, total: 0 ns
Wall time: 256 µs
Out[59]: (1.2700124590995348, 0.4406497868037883)

现在您可以看到任何给定x和y的答案都可以在不到一毫秒的时间内计算出来。