def moveWps(self, new_waypoints):
"""
Move the position of the waypoints
"""
if (len(new_waypoints) != len(self.knots)):
print("The lenght of the waypoints list should not change")
return False
# Store the waypoints
self.wp = new_waypoints
# Interpolation problem
(sol, null, _, coeff_m) = trjg.interpolPolys(self.wp, self.degree,
self.knots, True)
# Update the data
# Solution vector of the polynomial coefficient (1 row)
self.coeff_v = np.array(sol)
# Base of the null space of the interpolation solution
self.null_b = np.array(null)
# Matrix of polynomials coefficients
self.coeff_m = np.array(coeff_m)
# Number of pieces
self.npieces = coeff_m.shape[0]
return True
def __init__(self, waypoints=None, knots=None, degree=None):
if knots is not None:
# Knots (It's assumed that the knot of the first point is 0.0)
self.knots = np.array(knots)
self.duration = max(self.knots)
if degree is not None:
# Degree of the polynomial
self.degree = degree
if (waypoints is not None and knots is not None
and degree is not None):
# Store the waypoints
self.wp = waypoints
# Interpolation problem
(sol, null, _,
coeff_m) = trjg.interpolPolys(waypoints, degree, knots, True)
# Solution vector of the polynomial coefficient (1 row)
self.coeff_v = np.array(sol)
# Base of the null space of the interpolation solution
self.null_b = np.array(null)
# Matrix of polynomials coefficients
self.coeff_m = np.array(coeff_m)
# Number of pieces
self.npieces = coeff_m.shape[0]
Y = np.array([
[0, 0.0, 1.5, 1.5, 0.0],
[0, 0.0, 0.0, 0.0, 0.0],
[0, np.nan, np.nan, np.nan, 0.0],
[0, np.nan, np.nan, np.nan, 0.0],
])
Z = np.array([
[0, 0.0, 0.0, 0.0, 0.0],
[0, np.nan, np.nan, np.nan, 0.0],
[0, np.nan, np.nan, np.nan, 0.0],
[0, np.nan, np.nan, np.nan, 0.0],
])
# Compute the polynomials
(solx, nullx, resx, polysx) = tj.interpolPolys(X, ndeg, Dt, False)
(soly, nully, resy, polysy) = tj.interpolPolys(Y, ndeg, Dt, False)
(solz, nullz, resz, polysz) = tj.interpolPolys(Z, ndeg, Dt, False)
#M1y = (np.matmul(np.matmul(nullx.transpose(), Q), nullx))
#M2y = -np.matmul(np.matmul(np.linalg.inv(M1y), nullx.transpose()), Q)
#vy = np.matmul(M2y, soly)
#
#M1y = (np.matmul(np.matmul(nullx.transpose(), Q), nullx))
#M2y = -np.matmul(np.matmul(np.linalg.inv(M1y), nullx.transpose()), Q)
#vy = np.matmul(M2y, soly)
# Compute the trajectory from the polynomials
(T, Xtj, Ytj, Ztj) = tj_h.polysTo3D(Dt, 100, polysx, polysy, polysz, [0, 1, 2])
#tj.plotPoly3D(Dt, 1000, polysx, polysy, polysz, [0])
print('\n')
print('Waypoints')
print('{:1s} | {:^5s}| {:^5s}| {:^5s}'.format("N", "x", "x\'", "x\'\'"))
for i in range(X.shape[1]):
print('{:1d} | {:^5.2f}, {:^5.2f}, {:^5.2f}'.format(
i, X[0][i], X[1][i], X[2][i]))
[A, b] = tj.buildInterpolationProblem(X, ndeg, Dt, False)
print("Interpolation Problem Ax = b: ")
print("A = \n", A)
print("b = \n", b)
print('\n')
print("Solution to the problem: ")
(sol, nullx, res, polys) = tj.interpolPolys(X, ndeg, Dt, abstime=False)
for i in range(nW - 1):
print("Polynomial {:d}: \n".format(i), polys[i, :])
print("Null Space basis: \n", nullx)
print("Residuals: \n", res)
if (res.size > 0 and abs(res) > 0.001):
print("WARNING! PROBABLY YOU WON\'T BE SATISFIED WITH THE SOLUTION")
print("\n-------- Test Some Constraints-------")
S_test = np.zeros((nconstr, 4))
T0 = tj.constrMat(tj.t_vec(0, ndeg), [i for i in range(nconstr)])