def ref_extract(T, t, Cin):
#extract nominal state from Bezier control points
n = 7 #degree of cureve
d = 3 #derivative degree of Cin
Cout = np.zeros((d + 1, 3))
tn = min(t / T, 1) #nomarlized time, [0,1]
# calculte basis
Polybasis = np.zeros((d + 1, n + 1))
for k in range(d + 1):
for kk in range(n - k + 1):
Polybasis[k,
kk] = ft(n) / ft(n - k) * nCk(n - k, kk) * (tn**kk) * (
(1 - tn)**(n - k - kk))
for i in range(3):
Ctemp = np.zeros((d + 1, n + 1))
# cal 3 dimensional nominal states
Ctemp[0, :] = np.transpose(Cin[:, i])
for j in range(d):
if j == 0:
Ctemp[1, 0:-1] = Ctemp[j, 1:] - Ctemp[j, 0:-1]
else:
Ctemp[j + 1, 0:-1 - j] = Ctemp[j, 1:-j] - Ctemp[j, 0:-1 - j]
# cal 0-4th order derivatives
for k in range(d + 1):
Cout[k, i] = np.dot(Polybasis[k, :], Ctemp[k, :]) / T**k
return Cout
def state_from_bez(T, t, Cin):
# Extract nominal state from Bezier control points
d = 4 # derivative degree of Cin
n = 9 # degree of curve
tn = min(t/T,1) # normalized time, [0,1]
Ctemp = np.zeros((d+1,n+1))
phat = np.zeros((d+1,3))
for i in range(3):
# cal 3 dimensional nominal states
Ctemp[0,:] = np.transpose(Cin[:,i])
for j in range(d):
if j == 0:
Ctemp[ 1, 0:-1] = Ctemp[j, 1:] - Ctemp[j, 0:-1]
else:
Ctemp[j+1, 0:-1-j] = Ctemp[j, 1:-j] - Ctemp[j, 0:-1-j]
# cal 0-4th order derivatives
for k in range(d+1):
temp = 0
for kk in range(n-k+1):
Btemp = nCk(n-k,kk)*(tn**kk)*((1-tn)**(n-k-kk))
temp = temp + ft(n)/ft(n-k)*Ctemp[k,kk]*Btemp
phat[k,i] = temp/T**k
return phat
def counts(r,d,n):
return np.array([nCk(d,m)*nCk(n-d, r-m) for m in range(min(r,d)+1)])