Skip to content

Commit

Permalink
further testing and commenting
Browse files Browse the repository at this point in the history
  • Loading branch information
@blake5634
blake5634 committed Dec 17, 2021
1 parent f75c90c commit 9fa9fe0
Show file tree
Hide file tree
Showing 3 changed files with 397 additions and 370 deletions.
361 changes: 10 additions & 351 deletions ikbtbasics/ik_classes.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,15 +367,10 @@ def sum_of_angles_transform(self,variables):
# substitute th_23 for th_2+th_3 etc.
# (april: separate out for easier testing)

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
def sum_of_angles_sub(R, expr, variables):
aw = sp.Wild('aw')
bw = sp.Wild('bw')
cw = sp.Wild('cw')
s1 = sp.Wild('s1')
s2 = sp.Wild('s2')
cw = sp.Wild('cw')
newjoint = None
tmpeqn = None
found2 = found3 = False
Expand Down Expand Up @@ -445,12 +440,12 @@ def sum_of_angles_sub(R, expr, variables):
# Add the def of this SOA to list: eg th23 = th2+th3
# BUT it needs to be embedded into a 4x4 mequation so
# that solvers can scan it properly
R.kequation_aux_list.append(tmpeqn)
R.kequation_aux_list.append(tmpeqn)

# substitute new variable into the kinematic equations


# substitute new variable into the kinematic equations ((WHY twice??))
# If there is a three-way sub, prefer it to a two-way sub. e.g:
# Problem Dec'21:
# If there is a three-way sub, prefer it to a two-way sub. e.g:
# (a+b+c) -> (abc) instead of (a+bc)(!)
#

Expand Down Expand Up @@ -539,343 +534,7 @@ def erank(list_L): # rearrange list of eqns by length
############# main test the library #########################
#
if __name__ == "__main__": # tester code for the classes in this file

#j1 = joint_var(th_12)

sp.var('a b c d e')

a = sp.atan2(b,c) # make sure this function compiles/loads


# Test .subs operator on atan2() function

print('Original Function: ', a)
print('Substitute b<-e: ', a.subs(b,e), ' (Expect atanw(e, c))')
assert(a.subs(b,e) == sp.atan2(e,c))

###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()
print("--------------")


####Test unknown class

ua = kc.unknown(a)
ub = kc.unknown(b)

print("\n\nTesting unknown(symbol) (one-arg form)")
print("Unknown a: ", ua.symbol)
fs = ' unknown object element "solved" FAIL'
assert(ua.solved == False), fs
print("a is solved: ", ua.solved , ' (Expect False)')
print("Unknown b: ", ub.symbol)
ub.solved = True
print("b is solved: ", ub.solved, ' (Expect True)')
assert(ub.solved == True), fs

##Test matrix_equation class


print("\n\nTesting matrix_equation(T1,T2) class")
T1 = ik_lhs()
T2 = sp.zeros(5)
T2[1,1] = a # note: a = atan2(b,c) above
T2[1,2] = a+b
T2[2,2] = sp.sin(c)
T2[2,3] = l_1*sp.sin(d) + 2*l_2*sp.cos(d)
T2[3,1] = c+sp.cos(c)*l_1

sp.pprint(T2)

tme = kc.matrix_equation(T1,T2)
print('')
print("Mat eqn 1,2: ", tme.Td[1,2], " '=' ", tme.Ts[1,2], "(not a kequation type!)")
print('')

sp.var('e22 ')

fs = 'Matrix Equation Class, FAIL'
assert(tme.Ts[1,1] == a), fs
assert(tme.Td[1,1] == sp.var('r_22')), fs
assert(tme.Ts[1,2] == a+b), fs
assert(tme.Ts[2,2] == sp.sin(c)), fs
assert(tme.Ts[2,3] == l_1*sp.sin(d)+2*l_2*sp.cos(d)), fs


print(' Test equation sorting: ')

e1 = kc.kequation(l_1, sp.sin(th_1) + sp.cos(th_1)*l_1)
e2 = kc.kequation(l_2, sp.sin(th_1))
e3 = kc.kequation(l_3, sp.sin(th_1) + sp.cos(th_1)*l_1 + sp.cos(th_3)*l_2)
l = [e1, e2, e3]

print('Original List: ')
for e in l:
e.prt()

l= erank(l) # should sort in place by increasing length of expression


print('Sorted List: ')
for e in l:
e.prt()

assert (l == [e2, e1, e3]), ' Equation length sorting FAIL'



# unknown class hash function testing
tmpv1 = kc.unknown(th_1)
tmpv2 = kc.unknown(th_1)
print('a.symbol: ', tmpv1)
print('b.symbol: ', tmpv2)
c = set()
c.add(tmpv1)
c.add(tmpv2)
print('-------------')
print(tmpv1.__hash__())
print(tmpv2.__hash__())
print('Length of set: ', len(c))
print('-------------')
assert(len(c) == 1), "hashing (unknown/variable) class fail"



### Test Robot class
# Robot class is tested in updateL.py
print('Sum of Angles testing')
#


############################################################3
#
# basic sum of angles testing:
s = 'Basic Sum of Angles Testing'
print('\n\n ' + s + '\n\n')

sp.var('a1 a2 a3') # note subscripts required

vars01 = [kc.unknown(a1), kc.unknown(a2), kc.unknown(a3)]

i=1
for v in vars01: # variables need an index to place them in mechanism order
# must start with 1 for DH/Craig compatibility
v.n = i
i+=1

term1 = sp.sin(a1+a2)
term2 = sp.sin(a1+a2+a3)
# normally sum_of_angles_sub is used in the context of a Robot Object
# (which holds results). So lets make one:
rtest = Robot()
term1a, newj, newe = sum_of_angles_sub(rtest,term1, vars01)
fs = ' new equation not correctly established'
assert str(newj.symbol) == 'th_12', fs
print('new eqn', newe)
term2a, newj, newe = sum_of_angles_sub(rtest,term2, vars01)
assert str(newj.symbol) == 'th_123', fs
print('new eqn', newe)

#print 'Initial Test: '
#print term1, ' --> ', term1a
#print term2, ' --> ', term2a

fs = 'Sum of angles: basic test fail'
assert term1a == sp.sin(th_12), fs
assert term2a == sp.sin(th_123), fs


#####################################################################
#
# Problem-specific SOA test
#

s = 'Problem Specific Sum of Angles Testing'
print('\n\n ' + s + '\n\n')

sp.var('Px Py Pz')
unks01 = [kc.unknown(th_1), kc.unknown(th_2), kc.unknown(th_3), kc.unknown(th_4), kc.unknown(th_5), kc.unknown(th_6)]
i=1
for v in unks01: # variables need an index to place them in mechanism order
# must start with 1 for DH/Craig compatibility
v.n = i
i+=1


eqnterm = Px*sp.sin(th_2 + th_3 + th_4)*sp.cos(th_1) + Py*sp.sin(th_1)*sp.sin(th_2 + th_3 + th_4) + Pz*sp.cos(th_2 + th_3 + th_4) - a_2*sp.sin(th_3 + th_4) - a_3*sp.sin(th_4) - d_1*sp.cos(th_2 + th_3 + th_4)

term2, newj, newe = sum_of_angles_sub(rtest,eqnterm, unks01)
if newj:
unks01.append(newj)
if newe:
print(' NEW Equation: ', newe)

print('---')
print(eqnterm)
print('---')
print(term2)
print('---')
print('new unknown list: ', unks01)

fs = 'FAIL problem specific SOA tests'
assert str(term2) == 'Px*sin(th_234)*cos(th_1) + Py*sin(th_1)*sin(th_234) + Pz*cos(th_234) - a_2*sin(th_34) - a_3*sin(th_4) - d_1*cos(th_234)', fs
assert kc.unknown(th_234) in unks01, fs
assert kc.unknown(th_34) in unks01, fs



#####################################################
#
# advanced SOA testing
#
# Generate some SUM of Angles Kin eqns

s = 'Advanced Sum of Angles Testing: DH parameters/UR5'
print('\n\n ' + s + '\n\n')


dh = sp.Matrix([
[ 0 , 0 , d_1 , th_1 ], # based on UR5 but simplified
[-sp.pi/2 , 0 , 0 , th_2 ],
[ 0 , a_2, 0 , th_3 ],
[ 0 , a_3, 0 , th_4 ],
[ sp.pi/2 , 0 , 0 , th_5 ],
[ sp.pi/2 , 0 , 0 , th_6 ]
])
vv = [1,1,1,1,1,1]

variables = [kc.unknown(th_1), kc.unknown(th_2), kc.unknown(th_3), kc.unknown(th_4), kc.unknown(th_5), kc.unknown(th_6)]
i=1
for v in variables:
v.n = i # set the variable index (joint number)
i += 1

params = [d_1, a_2, a_3]
pvals = {d_1:1, a_2:1, a_3:1} # meters

robot = 'SOA Test Robot'

testing = False # not using this now
[m, R, tmpvars] = kinematics_pickle(robot, dh, params, pvals, vv, variables, testing)
print('GOT HERE: robot name: ', R.name)

variables = tmpvars
R.name = robot
R.params = params

## check the pickle in case DH params were changed
check_the_pickle(m.DH, dh) # check that two mechanisms have identical DH params

print('\n the variables: ', variables)
assert len(variables) == 9, 'wrong number of variables'
print('\n\n')

# Below replaced by pickle code above

#m = kc.mechanism(dh, params, vv)
#m.pvals = pvals # store numerical values of parameters
#print "Starting SOA Test Forward Kinematics"
#m.forward_kinematics()
#print "Completed Forward Kinematics"
##print 'Starting Sum of Angles scan (slow!)'
# set up Robot Object instance
#R = Robot(m, 'SOA 2,3 TEST Robot') # set up IK structs et
#R.scan_for_equations(variables) # generate equation lists



## below is commented out for testing and devel of sum_of_angles_transform

# need to re-do for debugging if test fails.
#R.sum_of_angles_transform(variables) # find sum of angles



#### Temp code: manually review all the FK matrix equations(!)
eqn = 0
row = 2
col = 3
print('Equations: ')
enbr = 0
for eqn in R.mequation_list:
print(enbr, ': ', eqn.Td[row,col], ' = ', eqn.Ts[row,col])
enbr += 1
print('\n\n\n')
####



# Now we test some expected RHS results
sp.var('A1 A2') # two possible answers

# Assertion 1
k = 0 # equation
i = 0 # row
j = 0 # col
A1 = R.mequation_list[k].Ts[i,j]
# expected correct answer: (contains a 3-way SOA: th_234)
A2 = -(sp.sin(th_1)*sp.sin(th_5) - sp.cos(th_1)*sp.cos(th_234)*sp.cos(th_5))*sp.cos(th_6) + sp.sin(th_234)*sp.sin(th_6)*sp.cos(th_1)

print('\n\n\n')
print('--- (round 1)')
print(A1)
print('---')
print(A2)
print('---')
print(sp.simplify(A1-A2))
print('\n\n\n')

fs = 'ik_classes: sum_of_angles_transform FAILS 3-way sum'
assert sp.simplify(A1-A2)== 0, fs+' 001'


# Assertion 2
sp.var('Px Py Pz')
k = 5 # equation
i = 2 # row
j = 3 # col
A1 = R.mequation_list[k].Td[i,j]
# expected correct answer:
#A2 = Px*sp.sin(th_234)*sp.cos(th_1) + Py*sp.sin(th_1)*sp.sin(th_234) + Pz*sp.cos(th_234) - a_2*sp.sin(th_34) - a_3*sp.sin(th_4) - d_1*sp.cos(th_234)
A2 = Px*sp.sin(th_234)*sp.cos(th_1) + Py*sp.sin(th_1)*sp.sin(th_234) + Pz*sp.cos(th_234) - a_2*sp.sin(th_34) - a_3*sp.sin(th_4) - d_1*sp.cos(th_234)

print('\n\n\n')
print('--- (round 2)')
print(A1) ## failing to substitute 3-way SOA right now
print('---')
print(A2)
print('---')
print(sp.simplify(A1-A2))
print('\n\n\n')
assert sp.simplify(A1-A2)==0, fs+' 002'


print('\n\n\n ik_classes PASSES all tests \n\n')
# testing for these classes and methods now in tests/leavestest.py
# TBD properly integrate with unittest module
pass

0 comments on commit 9fa9fe0

Please sign in to comment.