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)