45 lines
1.4 KiB
Python
45 lines
1.4 KiB
Python
import numpy as np
|
|
|
|
class HoverTraj(object):
|
|
"""
|
|
This trajectory simply has the quadrotor hover at the origin indefinitely.
|
|
By modifying the initial condition, you can create step response
|
|
experiments.
|
|
"""
|
|
def __init__(self, x0=np.array([0, 0, 0])):
|
|
"""
|
|
This is the constructor for the Trajectory object. A fresh trajectory
|
|
object will be constructed before each mission.
|
|
"""
|
|
|
|
self.x0 = x0
|
|
|
|
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 = self.x0
|
|
x_dot = np.zeros((3,))
|
|
x_ddot = np.zeros((3,))
|
|
x_dddot = np.zeros((3,))
|
|
x_ddddot = np.zeros((3,))
|
|
yaw = 0
|
|
yaw_dot = 0
|
|
yaw_ddot = 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, 'yaw_ddot':yaw_ddot}
|
|
return flat_output
|