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]