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.'
