def optfunc(N):
penalty = 0.
if verbose:
rotdiffarray = []
posdiffarray = []
for index in range(len(calibrationpositions)):
#if index%9<=2: #just upright
#if index%9>2: #just sideways
# continue
motorangles = calibrationmotorangles[index]
palmmat = calibrationpositions[index]
if type(motorangles) != int:
jointangles = convertMotorToJoint(scipy.matrix(motorangles).transpose(), N)
jointangleslist = jointangles.transpose().tolist()[0]
fkresultpalmmat = wamik.mat4(wamik.run_fk(jointangleslist))
fkbasepalmmat = joint0_to_base_mat * fkresultpalmmat
rotanglediff = wamik.rotangle(fkbasepalmmat[0:3,0:3], palmmat[0:3,0:3])
if rotanglediff > .5: #clearly got the wrong pose
continue
posdiff = fkbasepalmmat[0:3,3]-palmmat[0:3,3]
posdiffmagsq = (posdiff.transpose()*posdiff)[0,0]
pospenalty = posfact * posdiffmagsq
rotpenalty = rotfact * rotanglediff**2
penalty += pospenalty + rotpenalty
if verbose:
#print "fkbasepalmmat:\n", ppmat4tostr(fkbasepalmmat)
#print "actualrot:\n", ppmat4tostr(palmmat)
#print "rotanglediff:", rotanglediff
#print "posdiff:", ppscipyvecttostr(posdiff)
#print "posdiffmagsq:", posdiffmagsq
#print "posdiffmag:", posdiffmagsq**.5
#print "pospenalty:", pospenalty
#print "rotpenatly:", rotpenalty
posdiffarray.append(posdiffmagsq**.5)
rotdiffarray.append(rotanglediff)
#raw_input()
#print "N:", ppdoublelisttostr(N)
#print "N:", N
#print "total penalty:", penalty
#raw_input()
if verbose:
print "posdiffarray:", ppdoublelisttostr(posdiffarray)
print "rotdiffarray:", ppdoublelisttostr(rotdiffarray)
print "average pos diff:%.3f"%(sum(posdiffarray)/len(posdiffarray))
print "average rot diff:%.3f"%(sum(rotdiffarray)/len(rotdiffarray))
return penalty
'''test program for wamik.py (both analytical and optimziation IK) author: Kaijen Hsiao ([email protected])''' import wamik import scipy #init wamik.init_wamik() #run forward kinematics to find an appropriate pos and rot #should be [.31, 0, .42] theta = [0.00, -0.32, 0.01, 2.45, 0.06, -0.59, -0.05] print "angles:", wamik.pplisttostr(theta) palmmat = wamik.run_fk(theta) print "\npalmmat:" wamik.ppmat4(palmmat) #run inverse kinematics currentangles = [0] * 7 #analytical IK resultangles = wamik.run_ik(palmmat, currentangles) #optimization (need to start nearby-ish) #currentangles = (scipy.array(theta) + scipy.randn(7)*.1).tolist() optresultangles = wamik.run_opt_ik(palmmat, currentangles) #run forward kinematics again to check the resulting angles if resultangles != None: print "resultangles: ", wamik.pplisttostr(resultangles)
'''test program for wamik.py (both analytical and optimziation IK) author: Kaijen Hsiao ([email protected])''' import wamik import scipy #init wamik.init_wamik() #run forward kinematics to find an appropriate pos and rot #should be [.31, 0, .42] theta = [0.00, -0.32, 0.01, 2.45, 0.06, -0.59, -0.05] print "angles:", wamik.pplisttostr(theta) palmmat = wamik.run_fk(theta) print "\npalmmat:" wamik.ppmat4(palmmat) #run inverse kinematics currentangles = [0]*7 #analytical IK resultangles = wamik.run_ik(palmmat, currentangles) #optimization (need to start nearby-ish) #currentangles = (scipy.array(theta) + scipy.randn(7)*.1).tolist() optresultangles = wamik.run_opt_ik(palmmat, currentangles) #run forward kinematics again to check the resulting angles if resultangles != None: print "resultangles: ", wamik.pplisttostr(resultangles)
def forward_kinematics(req):
print "running forward kinematics"
palmmat = wamik.run_fk(req.angles)
return ForwardKinematicsResponse(palmmat)
def forward_kinematics(req):
print "running forward kinematics"
palmmat = wamik.run_fk(req.angles)
return ForwardKinematicsResponse(palmmat)
