def scan_for_equations(self,variables): self.l1 = [] # equations with one unk nown (if any) self.l2 = [] # equations with two unknowns self.l3p = [] # 3 OR MORE unknowns sp.var('x') #this will be used to generate 'algebraic zero' elist = self.mequation_list #.append(self.kequation_aux_list) #elist = self.kequation_aux_list + self.mequation_list #print '------------------------- elist----' #print elist #print '--------' #quit() assert (len(elist) > 0), ' not enough equations ' #i=0 #for e in self.kequation_aux_list: #elist[0][3][i] = e # HACK: put aux eqns into row 4 Meqn[0] #print 'scan_for_equns: putting ', e, 'into eqn' #i+=1 for eqn in elist: lhs = eqn.Td #4x4 matrix rhs = eqn.Ts #4x4 matrix for i in [0,1,2,3]: for j in range(0,4): lh1x1 = lhs[i,j] rh1x1 = rhs[i,j] n = count_unknowns(variables, lh1x1) + count_unknowns(variables, rh1x1) e1 = kc.kequation(lh1x1, rh1x1) if(n==1): flag = False if e1 not in self.l1: self.l1.append(e1) # only append if not already there if(n==2): flag = False if e1 not in self.l2: self.l2.append(e1) # only append if not already there if(n > 2): if e1 not in self.l3p: self.l3p.append(e1) # only append if not already there for e in self.kequation_aux_list: lhs = e.LHS rhs = e.RHS n = count_unknowns(variables, lhs) + count_unknowns(variables, rhs) if(n==1): self.l1.append(kc.kequation(lhs, rhs)) # change from 0, rhs-lhs !! ************ if(n==2): self.l2.append(kc.kequation(lhs, rhs)) self.l1 = erank(self.l1) # sort the equations (in place) so solvers get preferred eqns first self.l2 = erank(self.l2) self.l3p = erank(self.l3p) return [self.l1, self.l2, self.l3p]
def scan_for_equations(self, variables): self.l1 = [] # equations with one unk nown (if any) self.l2 = [] # equations with two unknowns self.l3p = [] # 3 OR MORE unknowns sp.var('x') # this will be used to generate 'algebraic zero' assert (len(self.mequation_list) > 0), ' not enough equations ' for eqn in self.mequation_list: lhs = eqn.Td # 4x4 matrix rhs = eqn.Ts # 4x4 matrix for i in [0, 1, 2, 3]: for j in range(0, 4): lh1x1 = lhs[i, j] rh1x1 = rhs[i, j] n = count_unknowns(variables, lh1x1) + \ count_unknowns(variables, rh1x1) e1 = kc.kequation(lh1x1, rh1x1) if (n == 1): flag = False if e1 not in self.l1: # only append if not already there self.l1.append(e1) if (n == 2): flag = False if e1 not in self.l2: # only append if not already there self.l2.append(e1) if (n > 2): if e1 not in self.l3p: # only append if not already there self.l3p.append(e1) for e in self.kequation_aux_list: lhs = e.LHS rhs = e.RHS n = count_unknowns(variables, lhs) + count_unknowns(variables, rhs) if (n == 1): # change from 0, rhs-lhs !! ************ self.l1.append(kc.kequation(lhs, rhs)) if (n == 2): self.l2.append(kc.kequation(lhs, rhs)) # sort the equations (in place) so solvers get preferred eqns first self.l1 = erank(self.l1) self.l2 = erank(self.l2) self.l3p = erank(self.l3p) return [self.l1, self.l2, self.l3p]
def scan(self, MatEqn): # find list of kequations containing this UNK self.eqnlist = [] # reset eqn list rng = [0, 1, 2, 3] for i in rng: for j in rng: eqn = MatEqn.Ts[i, j] if (eqn != 0): if eqn.has(self.symbol): self.eqnlist.append(kc.kequation(MatEqn.Td[i, j], eqn)) eqn = MatEqn.Td[i, j] if (eqn != 0): if eqn.has(self.symbol): # print "Equation [", eqn.string, "] has ", self.symbol self.eqnlist.append(kc.kequation(MatEqn.Ts[i, j], eqn)) self.eqnlist = erank(self.eqnlist) # sort them in place
def sum_of_angles_transform(self,variables): unkn_sums_sym = set() #keep track of joint variable symbols thx = sp.Wild('thx') thy = sp.Wild('thy') sgn = sp.Wild('sgn') success_flag = False for k in range(0,len(self.mequation_list)): Meq = self.mequation_list[k] # get next matrix equation for i in [0,1,2]: # only first three rows are interesting for j in [0,1,2,3]: # simplfy with lasting effect Meq.Ts[i,j] = sp.simplify(Meq.Ts[i,j]) # simplify should catch c1s2+s1c2 etc. (RHS) Meq.Td[i,j] = sp.simplify(Meq.Td[i,j]) # simplify should catch c1s2+s1c2 etc. (LHS) lhs = Meq.Td[i,j] rhs = Meq.Ts[i,j] for expr in [lhs, rhs]: sub_sin = expr.find(sp.sin(thx + sgn * thy)) #returns a subset of expressions with the quary pattern, this finds sin(thx) too sub_cos = expr.find(sp.cos(thx + sgn * thy)) found = False while len(sub_sin) > 0 and not found: sin_expr = sub_sin.pop() d = sin_expr.match(sp.sin(thx + sgn * thy)) if d[thx] != 0 and d[sgn] != 0 and d[thy] != 0: #has to be joint variable found = True while len(sub_cos) > 0 and not found: cos_expr = sub_cos.pop() d = cos_expr.match(sp.cos(thx + sgn * thy)) if d[thx] != 0 and d[sgn] != 0 and d[thy] != 0: found = True if found: #print 'SoA: found ', sin_expr, ' in ', expr success_flag = True th_xy = find_xy(d[thx], d[thy]) #if not exists in the unknown list (this requires proper hashing), create variable exists = False for v in variables: if v.symbol == th_xy: exists = True if not exists: print "found new 'joint' (sumofangle) variable: ", th_xy # try moving soa equation to Tm.auxeqns #unkn_sums_sym.add(th_xy) #add into the joint variable set newjoint = unknown(th_xy) newjoint.solved = False # just to be clear variables.append(newjoint) #add it to unknowns list tmpeqn = kc.kequation(th_xy, d[thx] + d[sgn] * d[thy]) print 'sumofanglesT: appending ', tmpeqn self.kequation_aux_list.append(tmpeqn) print d[thx] + d[sgn]*d[thy] #substitute all thx +/- thy expression with th_xy self.mequation_list[k].Td[i,j] = Meq.Td[i,j].subs(d[thx] + d[sgn] * d[thy], th_xy) self.mequation_list[k].Ts[i,j] = Meq.Ts[i,j].subs(d[thx] + d[sgn] * d[thy], th_xy)
###Test the Left Hand Side Generator m = ik_lhs() fs = 'ik_lhs() matrix generator FAIL' assert (m[0,0] == sp.var('r_11')), fs assert (m[0,1] == sp.var('r_12')),fs assert (m[3,3] == 1), fs assert (m[3,2] == 0), fs assert (m[3,1] == 0), fs assert (m[3,0] == 0), fs # ###Test kequation class E1 = kc.kequation(0, sp.cos(d)) E2 = kc.kequation(5, sp.sin(e)) E3 = kc.kequation(5, d+e+5) print "\n\nTesting kequation()" print "kequation sample: " print E1.LHS, " = ", E1.RHS fs = ' kequation method FAIL' assert(E1.LHS == 0), fs assert(E1.RHS == sp.cos(d)), fs assert(E2.RHS == sp.sin(e)), fs assert(E3.RHS == d+e+5), fs print "---------testing equation print method-----" E1.prt()
def generate_notation(self, R): #pass #TODO: generate individual notation # bh change "footnote" to "subscript" # bh: "notation" means indices eg: th_2011 #global notation_graph # TODO: debug the situation where parents are not related # but at different levels if len(self.parents) == 0: #root node special case if self.nsolutions < 2: self.sol_notations.add(self.symbol) R.notation_graph.add(Edge(self.symbol, -1)) R.notation_collections.append([self.symbol]) print '//////////////////////// 1 sol' print 'curr: ', self.symbol self.solution_with_notations[self.symbol] = kc.kequation(\ self.symbol, self.solutions[0]) self.arguments[self.symbol] = self.argument else: for i in range(1, self.nsolutions + 1): curr = str(self.symbol) + 's' + str( i) #convert to str, then to symbol? curr = sp.var(curr) self.sol_notations.add(curr) R.notation_graph.add(Edge(curr, -1)) R.notation_collections.append([curr]) # add into subgroup curr_solution = self.solutions[i - 1] self.solution_with_notations[curr] = kc.kequation( curr, curr_solution) print '//////////////////////// > 1 sol' print 'curr: ', curr print self.argument self.arguments[curr] = self.argument # simple because root else: # Non-root node # (find the deepest level of parents and) get product of parents # getting the product is safe here because we already # trimmed the infeasible pairs from last step (redundency detection) parents_notation_list = [] if len(self.parents) == 1: # convert to single symbol to list for one_sym in self.parents[0].sol_notations: parents_notation_list.append([one_sym]) elif len(self.parents) == 2: parents_notation_list = itt.product(self.parents[0].sol_notations, \ self.parents[1].sol_notations) elif len(self.parents) == 3: parents_notation_list = itt.product(self.parents[0].sol_notations, \ self.parents[1].sol_notations, \ self.parents[2].sol_notations) elif len(self.parents) == 4: parents_notation_list = itt.product(self.parents[0].sol_notations, \ self.parents[1].sol_notations, \ self.parents[2].sol_notations, \ self.parents[3].sol_notations) elif len(self.parents) == 5: parents_notation_list = itt.product(self.parents[0].sol_notations, \ self.parents[1].sol_notations, \ self.parents[2].sol_notations, \ self.parents[3].sol_notations, \ self.parents[4].sol_notations) isub = 1 for parents_tuple in parents_notation_list: # find all parents notations, this is done outside of # the solution loop because multiple solutions share the same parents parents_notations = [] # look for higher level parent notation connected to this parent for parent_sym in parents_tuple: parents_notations.append(parent_sym) for higher_parent in self.upper_level_parents: goal_notation = goal_search(parent_sym, \ higher_parent.sol_notations, R.notation_graph) if goal_notation is not None: parents_notations.append(goal_notation) for curr_solution in self.solutions: # creat new symbols and link to graph curr = str(self.symbol) + 's' + str(isub) curr = sp.var(curr) self.sol_notations.add(curr) #R.notation_collections.append(curr) isub = isub + 1 # link to graph for parent_sym in parents_tuple: R.notation_graph.add(Edge(curr, parent_sym)) # substitute for solutions rhs = curr_solution expr_notation_list = [curr] for parent in (self.parents + self.upper_level_parents): curr_parent = None for parent_sym in parents_notations: if parent_sym in parent.sol_notations: curr_parent = parent_sym expr_notation_list.append(curr_parent) try: rhs = rhs.subs(parent.symbol, curr_parent) tmp_arg = self.argument.subs( parent.symbol, curr_parent) # also sub the arg except: print "problmematic step: ", parent.symbol print "solution: ", rhs print "parents notations" print parents_notation_list R.notation_collections.append(expr_notation_list) #parents_notations.remove(curr_parent) # can't remove here, or won't be able to generate solution for # multiple solution case self.solution_with_notations[curr] = kc.kequation( curr, rhs) self.arguments[curr] = tmp_arg
def sum_of_angles_sub(R, expr, variables): thx = sp.Wild('thx') # a theta_x term thy = sp.Wild('thy') # a theta_x term sgn = sp.Wild('sgn') # 1 or -1 aw = sp.Wild('aw') bw = sp.Wild('bw') cw = sp.Wild('cw') s1 = sp.Wild('s1') s2 = sp.Wild('s2') newjoint = None tmpeqn = None found2 = found3 = False matches = expr.find(sp.sin(aw+bw+cw)) | expr.find(sp.cos(aw+bw+cw)) #print '- - - - -' #print expr #print matches for m in matches: # analyze each match d = m.match(sp.cos(aw + bw + cw)) d1 = m.match(sp.sin(aw + bw + cw)) if d != None and d1 != None: d.update(d1) if d == None and d1 != None: d = d1 # To find sum-of-angles arguments, # count number of non-zero elements among aw ... cw nzer = 0 varlist = [] for k1 in d.keys(): if d[k1] == 0: nzer += 1 else: varlist.append(d[k1]) #print 'varlist: ', varlist if len(varlist) == 2: found2 = True if len(varlist) == 3: found3 = True newjoint = None tmpeqn = None if(found2 or found3): # we've got a SOA! # generate index of the SOA variable nil = [] #new index list = 'nil' for v in varlist: # build the new subscript nil.append( str(get_variable_index(variables, v)) ) nil.sort() # get consistent order of indices ni = '' for c in nil: # make into a string ni += c #print 'New index: '+ni vexists = False # has this SOA been found before? Did we already make it? for v in variables: if v.n == int(ni): # i.e. if th_23 is aready defined th_subval = v vexists = True newjoint = None tmpeqn = None th_new = sp.var('th_'+ni) # create iff doesn't yet exist th_subval = th_new if not vexists: print ": found new 'joint' (sumofangle) variable: ", th_new # try moving soa equation to Tm.auxeqns #unkn_sums_sym.add(th_new) #add into the joint variable set newjoint = kc.unknown(th_new) newjoint.n = int(ni) # generate e.g. 234 = 10*2 + 34 newjoint.solved = False # just to be clear for count_unknowns variables.append(newjoint) #add it to unknowns list tmpeqn = kc.kequation(th_new, d[aw] + d[bw] + d[cw]) print 'sum_of_angles_sub: created new equation:', tmpeqn # add this new equation to the Robot aux list R.kequation_aux_list.append(tmpeqn) # substitute new variable into the kinematic equations ((WHY twice??)) #self.mequation_list[k].Td[i,j] = Meq.Td[i,j].subs(d[aw] + d[bw] + d[cw], th_subval) #self.mequation_list[k].Ts[i,j] = Meq.Ts[i,j].subs(d[aw] + d[bw] + d[cw], th_subval) expr = expr.subs(d[aw] + d[bw] + d[cw], th_subval) #print 'sum of angles (ik_classes): NEW Eqns (k,i,j)',k,i,j #print self.mequation_list[k].Td[i,j] #print ' = ' #print self.mequation_list[k].Ts[i,j] #print '========' if tmpeqn is not None: print 'sum_of_angles_sub: Ive found a new SOA equation, ', tmpeqn #else: #print '>>m>>m>>m I didnt find a new SOA equation' return (expr,newjoint, tmpeqn)
def sum_of_angles_transform(self,variables): print 'Starting sum-of-angles scan. Please be patient' unkn_sums_sym = set() #keep track of joint variable symbols thx = sp.Wild('thx') # a theta_x term thy = sp.Wild('thy') # a theta_x term sgn = sp.Wild('sgn') # 1 or -1 aw = sp.Wild('aw') bw = sp.Wild('bw') cw = sp.Wild('cw') s1 = sp.Wild('s1') s2 = sp.Wild('s2') #k = equation number #i = row, j=col nits = len(self.mequation_list) * 3 * 4 # total number of equations barlen = nits/2 it_number = 0 for k in range(0,len(self.mequation_list)): Meq = self.mequation_list[k] # get next matrix equation for i in [0,1,2]: # only first three rows are interesting for j in [0,1,2,3]: it_number += 1 #print ' .. ' prog_bar(it_number, nits, barlen, 'Sum of Angles') #print 'Sum of Angles: eqn,row,col: ', k,i,j # simplify with lasting effect (note: try sp.trigsimp() for faster????) Meq.Ts[i,j] = sp.simplify(Meq.Ts[i,j]) # simplify should catch c1s2+s1c2 etc. (RHS) Meq.Td[i,j] = sp.simplify(Meq.Td[i,j]) # simplify should catch c1s2+s1c2 etc. (LHS) lhs = Meq.Td[i,j] rhs = Meq.Ts[i,j] for expr in [lhs, rhs]: found2 = found3 = False matches = expr.find(sp.sin(aw+bw+cw)) | expr.find(sp.cos(aw+bw+cw)) #print '- - - - -' #print expr #print matches for m in matches: # analyze each match d = m.match(sp.cos(aw + bw + cw)) d1 = m.match(sp.sin(aw + bw + cw)) if d != None and d1 != None: d.update(d1) if d == None and d1 != None: d = d1 # To find sum-of-angles arguments, # count number of non-zero elements among aw ... cw nzer = 0 varlist = [] for k1 in d.keys(): if d[k1] == 0: nzer += 1 else: varlist.append(d[k1]) #print 'varlist: ', varlist if len(varlist) == 2: found2 = True if len(varlist) == 3: found3 = True if(found2 or found3): # we've got a SOA! # generate index of the SOA variable nil = [] #new index list = 'nil' for v in varlist: # build the new subscript nil.append( str(get_variable_index(variables, v)) ) nil.sort() # get consistent order of indices ni = '' for c in nil: # make into a string ni += c #print 'New index: '+ni exists = False # has this SOA been found before? Did we already make it? for v in variables: if v.n == int(ni): # i.e. if th_23 is aready defined th_subval = v exists = True if not exists: th_new = sp.var('th_'+ni) # create iff doesn't yet exists th_subval = th_new print "found new 'joint' (sumofangle) variable: (k=",k,") ", th_new # try moving soa equation to Tm.auxeqns #unkn_sums_sym.add(th_new) #add into the joint variable set newjoint = unknown(th_new) newjoint.n = int(ni) # generate e.g. 234 = 10*2 + 34 newjoint.solved = False # just to be clear for count_unknowns variables.append(newjoint) #add it to unknowns list tmpeqn = kc.kequation(th_new, d[aw] + d[bw] + d[cw]) print 'sumofanglesT: appending new equation:', tmpeqn self.kequation_aux_list.append(tmpeqn) # substitute new variable into the kinematic equations self.mequation_list[k].Td[i,j] = Meq.Td[i,j].subs(d[aw] + d[bw] + d[cw], th_subval) self.mequation_list[k].Ts[i,j] = Meq.Ts[i,j].subs(d[aw] + d[bw] + d[cw], th_subval) #print 'sum of angles (ik_classes): NEW Eqns (k,i,j)',k,i,j #print self.mequation_list[k].Td[i,j] #print ' = ' #print self.mequation_list[k].Ts[i,j] #print '========' prog_bar(-1,100,100, '') # clear the progress bar #x = raw_input('<enter> to cont...') print 'Completed sum-of-angles scan.'