机器人手臂轨迹规划

#!/usr/bin/env python

import numpy as np
from scipy.optimize import minimize
from math import sin,cos
from scipy.linalg.misc import norm
import scipy.optimize as opt

P_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 P

def g(theta):
	global P_target
	return norm(f(theta)-P_target)

theta_opt = opt.minimize(g, (1, 1)).x
P_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])