机器人手臂轨迹规划

#!/usr/bin/env pythonimport numpy as npfrom scipy.optimize import minimizefrom math import sin,cosfrom scipy.linalg.misc import normimport scipy.optimize as optP_target = np.zeros((2,), np.float)P_target[0] = float(input("x: "))P_target[1] = float(input("y: "))def f(theta):P = np.zeros((2,), np.float)P[0] = sin(theta[0])-sin(theta[0]+theta[1])P[1] = cos(theta[0])+cos(theta[0]+theta[1])return Pdef g(theta):global P_targetreturn norm(f(theta)-P_target)theta_opt = opt.minimize(g, (1, 1)).xP_opt = f(theta_opt)print "Optimized Theta = (%f, %f)\n"%(theta_opt[0], theta_opt[1])print "Nearest Point = (%f, %f)\n"%(P_opt[0], P_opt[1])