76 lines
2.9 KiB
Python
76 lines
2.9 KiB
Python
|
import numpy as np
|
||
|
|
"""
|
||
|
|
Lissajous curves are defined by trigonometric functions parameterized in time.
|
||
|
|
See https://en.wikipedia.org/wiki/Lissajous_curve
|
||
|
|
|
||
|
|
"""
|
||
|
|
class TwoDLissajous(object):
|
||
|
|
"""
|
||
|
|
The standard Lissajous on the XY curve as defined by https://en.wikipedia.org/wiki/Lissajous_curve
|
||
|
|
This is planar in the XY plane at a fixed height.
|
||
|
|
"""
|
||
|
|
def __init__(self, A=1, B=1, a=1, b=1, delta=0, height=0, yaw_bool=False):
|
||
|
|
"""
|
||
|
|
This is the constructor for the Trajectory object. A fresh trajectory
|
||
|
|
object will be constructed before each mission.
|
||
|
|
|
||
|
|
Inputs:
|
||
|
|
A := amplitude on the X axis
|
||
|
|
B := amplitude on the Y axis
|
||
|
|
a := frequency on the X axis
|
||
|
|
b := frequency on the Y axis
|
||
|
|
delta := phase offset between the x and y parameterization
|
||
|
|
height := the z height that the lissajous occurs at
|
||
|
|
yaw_bool := determines whether the vehicle should yaw
|
||
|
|
"""
|
||
|
|
|
||
|
|
self.A, self.B = A, B
|
||
|
|
self.a, self.b = a, b
|
||
|
|
self.delta = delta
|
||
|
|
self.height = height
|
||
|
|
|
||
|
|
self.yaw_bool = yaw_bool
|
||
|
|
|
||
|
|
def update(self, t):
|
||
|
|
"""
|
||
|
|
Given the present time, return the desired flat output and derivatives.
|
||
|
|
|
||
|
|
Inputs
|
||
|
|
t, time, s
|
||
|
|
Outputs
|
||
|
|
flat_output, a dict describing the present desired flat outputs with keys
|
||
|
|
x, position, m
|
||
|
|
x_dot, velocity, m/s
|
||
|
|
x_ddot, acceleration, m/s**2
|
||
|
|
x_dddot, jerk, m/s**3
|
||
|
|
x_ddddot, snap, m/s**4
|
||
|
|
yaw, yaw angle, rad
|
||
|
|
yaw_dot, yaw rate, rad/s
|
||
|
|
"""
|
||
|
|
x = np.array([self.A*np.sin(self.a*t + self.delta),
|
||
|
|
self.B*np.sin(self.b*t),
|
||
|
|
self.height])
|
||
|
|
x_dot = np.array([self.a*self.A*np.cos(self.a*t + self.delta),
|
||
|
|
self.b*self.B*np.cos(self.b*t),
|
||
|
|
0])
|
||
|
|
x_ddot = np.array([-(self.a)**2*self.A*np.sin(self.a*t + self.delta),
|
||
|
|
-(self.b)**2*self.B*np.sin(self.b*t),
|
||
|
|
0])
|
||
|
|
x_dddot = np.array([-(self.a)**3*self.A*np.cos(self.a*t + self.delta),
|
||
|
|
-(self.b)**3*self.B*np.cos(self.b*t),
|
||
|
|
0])
|
||
|
|
x_ddddot = np.array([(self.a)**4*self.A*np.sin(self.a*t + self.delta),
|
||
|
|
(self.b)**4*self.B*np.sin(self.b*t),
|
||
|
|
0])
|
||
|
|
|
||
|
|
if self.yaw_bool:
|
||
|
|
yaw = np.pi/4*np.sin(np.pi*t)
|
||
|
|
yaw_dot = np.pi*np.pi/4*np.cos(np.pi*t)
|
||
|
|
else:
|
||
|
|
yaw = 0
|
||
|
|
yaw_dot = 0
|
||
|
|
|
||
|
|
flat_output = { 'x':x, 'x_dot':x_dot, 'x_ddot':x_ddot, 'x_dddot':x_dddot, 'x_ddddot':x_ddddot,
|
||
|
|
'yaw':yaw, 'yaw_dot':yaw_dot}
|
||
|
|
return flat_output
|