def main():
# Initialises two Polynomial objects of the Polynomial class.
polynomialA = Polynomial([2, 0, 4, -1, 0, 6])
polynomialB = Polynomial([-1, -3, 0, 4.5])
# Prints the order of the first polynomial.
print("The order of Polynomial A is " + str(polynomialA.returnOrder()) + "\n")
# Initialises a third Polynomial object that is the sum of the first two.
polynomialC = polynomialA.addPolynomial(polynomialB)
# Prints the third polynomial as a formatted string.
print("The sum of Polynomial A and Polynomial B is:\n" + polynomialC.show() + "\n")
# Initialises a fourth polynomial that is the derivative of the first.
polynomialD = polynomialA.derivative()
# Prints the fourth polynomial as a formatted string.
print("The derivative of Polynomial A is:\n" + polynomialD.show() + "\n")
# Initialises a fifth polynomial that is the antiderivative of the fourth.
polynomialE = polynomialD.integral()
# Prints the fifth polynomial as a formatted string.
print("This is the antiderivative of the expression above:\n" + polynomialE.show() + "\n")
def run(self):
poly1 = Polynomial([2, 0, 4, -1, 0, 6])
poly2 = Polynomial([-1, -3, 0, 4.5])
print("The order of the polynomial (P1(x) = " + str(poly1) + ") is " +
str(poly1.order()))
print("The sum of the polynomials (P1(x) = " + str(poly1) +
") and (P2(x) = " + str(poly2) + ") is " + str(poly1 + poly2))
print("The derivative of the polynomial (P1(x) = " + str(poly1) +
") is " + str((poly1.derivative())))
print("The antiderivative of the derivative of (P(x) = " + str(poly1) +
") is " + str((poly1.derivative()).antiderivative(2)))
from Polynomial import Polynomial
a = Polynomial([9.0, 0.0, 2.3])
b = Polynomial([-2.0, 4.5, 0.0, 2.1])
print(a, "plus", b, "is", a + b)
print(a, "times", b, "is", a * b)
print(a, "times", 3, "is", a * 3)
print(a, "minus", b, "is", a - b)
c = b.derivative()
print("Derivative of", b, "is", c)
slope = c(3)
print("Value of the derivative at 3 is", slope)
class Polynomial4D:
def __init__(self, duration, px, py, pz, pyaw):
self.duration = duration
self.px = Polynomial(px)
self.py = Polynomial(py)
self.pz = Polynomial(pz)
self.pyaw = Polynomial(pyaw)
# compute and return derivative
def derivative(self):
return Polynomial4D(self.duration,
self.px.derivative().p,
self.py.derivative().p,
self.pz.derivative().p,
self.pyaw.derivative().p)
@staticmethod
def normalize(v):
norm = np.linalg.norm(v)
assert norm > 0
return v / norm
def eval(self, t):
result = TrajectoryOutput()
# flat variables
result.pos = np.array(
[self.px.eval(t),
self.py.eval(t),
self.pz.eval(t)])
result.yaw = self.pyaw.eval(t)
# 1st derivative
derivative = self.derivative()
result.vel = np.array([
derivative.px.eval(t),
derivative.py.eval(t),
derivative.pz.eval(t)
])
dyaw = derivative.pyaw.eval(t)
# 2nd derivative
derivative2 = derivative.derivative()
result.acc = np.array([
derivative2.px.eval(t),
derivative2.py.eval(t),
derivative2.pz.eval(t)
])
# 3rd derivative
derivative3 = derivative2.derivative()
jerk = np.array([
derivative3.px.eval(t),
derivative3.py.eval(t),
derivative3.pz.eval(t)
])
thrust = result.acc + np.array([0, 0, 9.81]) # add gravity
z_body = self.normalize(thrust)
x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0])
y_body = self.normalize(np.cross(z_body, x_world))
x_body = np.cross(y_body, z_body)
jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body)
h_w = jerk_orth_zbody / np.linalg.norm(thrust)
result.omega = np.array(
[-np.dot(h_w, y_body),
np.dot(h_w, x_body), z_body[2] * dyaw])
return result
