#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) resultpalmmat = wamik.run_fk(resultangles) print "\nresultpalmmat:" wamik.ppmat4(resultpalmmat) else: print "No analytical IK solution found!" print "optresultangles: ", wamik.pplisttostr(optresultangles) optresultpalmmat = wamik.run_fk(optresultangles) print "\noptresultpalmmat:"
def optimization_inverse_kinematics(req): print "running optimization inverse kinematics" resultangles = wamik.run_opt_ik(req.palmmat4, req.currentangles) return InverseKinematicsResponse(resultangles, 'foundsomething')
#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) resultpalmmat = wamik.run_fk(resultangles) print "\nresultpalmmat:" wamik.ppmat4(resultpalmmat) else: print "No analytical IK solution found!" print "optresultangles: ", wamik.pplisttostr(optresultangles) optresultpalmmat = wamik.run_fk(optresultangles) print "\noptresultpalmmat:"
def optimization_inverse_kinematics(req): print "running optimization inverse kinematics" resultangles = wamik.run_opt_ik(req.palmmat4, req.currentangles) return InverseKinematicsResponse(resultangles, "foundsomething")