I'm working on a project whereby I am coding a virtual robot arm. Within the script I generate roughly 1350 targets [x, y, z] for the robot's end-effector and then I use the coordinates to calculate 3 angles for the robot's joints. I'm using sympy.solve:
def solve_robot(coordinate_x, coordinate_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 []
It takes rougly 7 minutes to complete all calculations. I've tried the flags such as manual=True, simplify=False, but it still takes a long time. Almost all my solutions have 2 solutions, is there a way to force sympy to stop after finding one solution? So theoretically the code will be twice as fast?
Edit: I needed to round the angle otherwise this error occurs: TypeError: can't convert -oo to int
I'm not sure why you are rounding radians(90). If you want the exact angle 90 degrees in radians then use SymPy's pi/2. Here are your equations:
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]
The pi/2 naturally simplifies so the equations become:
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
We can solve this kind of system using Groebner bases if we convert to polynomials with the substitutions sw = sin(w), cw = cos(w) etc.:
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]
We now have two equations for four unknowns but we know that we can make new polynomial equations because 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
We are going to compute a Groebner basis but for that it is much better to have exact rational numbers rather than floats:
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 ⎦
Now we can compute a Groebner basis for these polynomials
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
The final equation is a quadratic in cz. The first 3 are linear in sw, sz and cw (although singular at x=0 which would need to be handled specially). We can therefore compute the solutions like this:
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,)
These two expressions sol1 and sol2 are the general form of the solution in terms of the parameters x and y. You can substitute some particular values for those to get numeric answers:
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]
Of course these are the answers for sz etc but you want z itself. We can recover z and w using atan2 and then use lambdify for faster evaluation:
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)
Now you can see that the answer for any given x and y can be computed in less than a millisecond.
In fact you can do all 1350 points in a few milliseconds:
In [60]: x = y = np.linspace(50, 100, 1350)
In [61]: %time f(x, y)
CPU times: user 4 ms, sys: 0 ns, total: 4 ms
Wall time: 3.41 ms
Out[61]:
(array([1.82911048, 1.82883992, 1.82856907, ..., 1.20763722, 1.20675767,
1.20587593]),
array([-0.47322886, -0.47263842, -0.47204816, ..., 0.43240847,
0.43354848, 0.43469131]))