def _paul_solve(robo, symo, nTm, n, m, known_vars=None):
if known_vars is None:
knowns = set()
else:
knowns = set(known_vars)
chain = robo.loop_chain(m, n)
th_all = set()
r_all = set()
# Create the set of all knowns symbols
for i in chain:
if i >= 0:
if robo.sigma[i] == 0 and isinstance(robo.theta[i], Expr):
th_all.add(robo.theta[i])
if isinstance(robo.r[i], Expr):
knowns |= robo.r[i].atoms(Symbol)
if robo.sigma[i] == 1 and isinstance(robo.r[i], Expr):
r_all.add(robo.r[i])
if isinstance(robo.theta[i], Expr):
knowns |= robo.theta[i].atoms(Symbol)
if isinstance(robo.gamma[i], Expr):
knowns |= robo.gamma[i].atoms(Symbol)
if isinstance(robo.alpha[i], Expr):
knowns |= robo.alpha[i].atoms(Symbol)
if isinstance(robo.d[i], Expr):
knowns |= robo.d[i].atoms(Symbol)
if isinstance(robo.b[i], Expr):
knowns |= robo.b[i].atoms(Symbol)
while True:
repeat = False
iTm = nTm.copy()
tr_list = transform_list(robo, n, m)
_replace_EMPTY(iTm, tr_list)
tr_list.reverse()
tr_const, tr_list = _extract_const_transforms(tr_list, knowns)
for trc in tr_const:
iTm = iTm * trc.matrix_inv()
tr_list.reverse()
while tr_list:
tr_const, tr_list = _extract_const_transforms(tr_list, knowns)
for trc in tr_const:
iTm = trc.matrix_inv() * iTm
tr = tr_list.pop(0)
if tr.val.atoms(Symbol) - knowns:
M_eq = tr.matrix() * to_matrix(tr_list, simplify=False)
while True:
found = _look_for_eq(symo, M_eq - iTm,
knowns, th_all, r_all)
repeat |= found
if not found or th_all | r_all <= knowns:
break
iTm = tr.matrix_inv() * iTm
if th_all | r_all <= knowns:
break
if not repeat or th_all | r_all <= knowns:
break
return knowns
def _replace_EMPTY(T, tr_list):
T_sym = to_matrix(tr_list, simplify=True)
for e1 in xrange(4):
for e2 in xrange(4):
if T[e1, e2].has(EMPTY):
T[e1, e2] = T_sym[e1, e2]
