swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
spencerfolk
2023-03-15 15:38:14 -04:00
commit 4d7fca10e4
62 changed files with 5891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
rotorpy/trajectories/README.md Normal file
View File

@@ -0,0 +1,7 @@
# Trajectory Module
Trajectories define the desired motion of the multirotor in time. There are a few options for trajectory generation. `minsnap.py` is an example of trajectory generators based on waypoints like in [Minimum Snap Trajectory Generation and Control for Quadrotors](https://ieeexplore.ieee.org/document/5980409), while the others are examples of time-parameterized shapes.
If you'd like to make your own trajectory you can reference `traj_template.py` to see the required structure.
Currently only trajectories that output flat outputs (position and yaw) are implemented to complement the SE3 controller, but you can develop your own trajectories that can be paired with different hierarchies of controllers.

0
rotorpy/trajectories/__init__.py Normal file
View File

189
rotorpy/trajectories/circular_traj.py Normal file
View File

@@ -0,0 +1,189 @@
import numpy as np
import sys
class CircularTraj(object):
"""
A circle.
"""
def __init__(self, center=np.array([0,0,0]), radius=1, freq=0.2, yaw_bool=False, plane='XY', direction='CCW'):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
Inputs:
center, the center of the circle (m)
radius, the radius of the circle (m)
freq, the frequency with which a circle is completed (Hz)
yaw_bool, determines if yaw motion is desired
plane, the plane with which the circle lies on, 'XY', 'YZ', or 'XZ'
direcition, the direction of the circle, 'CCW' or 'CW'
"""
# Check and assign inputs
if plane == "XY" or plane == "YZ" or plane == "XZ":
self.plane = plane
else:
print("CircularTraj Error: incorrect specification of plane. Must be 'XY', 'YZ', or 'XZ' ")
sys.exit(1)
if direction == "CW" or direction == "CCW":
if direction == "CW":
self.sign = -1
else:
self.sign = 1
else:
print("CircularTraj Error: incorrect specification of direction. Must be 'CW' or 'CCW' ")
sys.exit(1)
self.center = center
self.cx, self.cy, self.cz = center[0], center[1], center[2]
self.radius = radius
self.freq = freq
self.omega = 2*np.pi*self.freq
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
"""
if self.plane == "XY":
x = np.array([self.cx + self.radius*np.cos(self.sign*self.omega*t),
self.cy + self.radius*np.sin(self.sign*self.omega*t),
self.cz])
x_dot = np.array([-self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t),
0])
x_ddot = np.array([-self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
-self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t),
0])
x_dddot = np.array([self.radius*((self.sign*self.omega)**3)*np.sin(self.sign*self.omega*t),
-self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t),
0])
x_ddddot = np.array([self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t),
0])
elif self.plane == "YZ":
x = np.array([self.cx,
self.cy + self.radius*np.cos(self.sign*self.omega*t),
self.cz + self.radius*np.sin(self.sign*self.omega*t)])
x_dot = np.array([0,
-self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t)])
x_ddot = np.array([0,
-self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
-self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t)])
x_dddot = np.array([0,
self.radius*((self.sign*self.omega)**3)*np.sin(self.sign*self.omega*t),
-self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t)])
x_ddddot = np.array([0,
self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t)])
elif self.plane == "XZ":
x = np.array([self.cx + self.radius*np.cos(self.sign*self.omega*t),
self.cy,
self.cz + self.radius*np.sin(self.sign*self.omega*t)])
x_dot = np.array([-self.radius*self.sign*self.omega*np.sin(self.sign*self.omega*t),
0,
self.radius*self.sign*self.omega*np.cos(self.sign*self.omega*t)])
x_ddot = np.array([-self.radius*((self.sign*self.omega)**2)*np.cos(self.sign*self.omega*t),
0,
-self.radius*((self.sign*self.omega)**2)*np.sin(self.sign*self.omega*t)])
x_dddot = np.array([self.radius*((self.sign*self.omega)**3)*np.sin(self.omega*t),
0,
-self.radius*((self.sign*self.omega)**3)*np.cos(self.sign*self.omega*t)])
x_ddddot = np.array([self.radius*((self.sign*self.omega)**4)*np.cos(self.sign*self.omega*t),
0,
self.radius*((self.sign*self.omega)**4)*np.sin(self.sign*self.omega*t)])
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
class ThreeDCircularTraj(object):
"""
"""
def __init__(self, center=np.array([0,0,0]), radius=np.array([1,1,1]), freq=np.array([0.2,0.2,0.2]), yaw_bool=False):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
Inputs:
center, the center of the circle (m)
radius, the radius of the circle (m)
freq, the frequency with which a circle is completed (Hz)
"""
self.center = center
self.cx, self.cy, self.cz = center[0], center[1], center[2]
self.radius = radius
self.freq = freq
self.omega = 2*np.pi*self.freq
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.cx + self.radius[0]*np.cos(self.omega[0]*t),
self.cy + self.radius[1]*np.sin(self.omega[1]*t),
self.cz + self.radius[2]*np.sin(self.omega[2]*t)])
x_dot = np.array([-self.radius[0]*self.omega[0]*np.sin(self.omega[0]*t),
self.radius[1]*self.omega[1]*np.cos(self.omega[1]*t),
self.radius[2]*self.omega[2]*np.cos(self.omega[2]*t)])
x_ddot = np.array([-self.radius[0]*(self.omega[0]**2)*np.cos(self.omega[0]*t),
-self.radius[1]*(self.omega[1]**2)*np.sin(self.omega[1]*t),
-self.radius[2]*(self.omega[2]**2)*np.sin(self.omega[2]*t)])
x_dddot = np.array([ self.radius[0]*(self.omega[0]**3)*np.sin(self.omega[0]*t),
-self.radius[1]*(self.omega[1]**3)*np.cos(self.omega[1]*t),
self.radius[2]*(self.omega[2]**3)*np.cos(self.omega[2]*t)])
x_ddddot = np.array([self.radius[0]*(self.omega[0]**4)*np.cos(self.omega[0]*t),
self.radius[1]*(self.omega[1]**4)*np.sin(self.omega[1]*t),
self.radius[2]*(self.omega[2]**4)*np.sin(self.omega[2]*t)])
if self.yaw_bool:
yaw = 0.8*np.pi/2*np.sin(2.5*t)
yaw_dot = 0.8*2.5*np.pi/2*np.cos(2.5*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

41
rotorpy/trajectories/hover_traj.py Normal file
View File

@@ -0,0 +1,41 @@
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):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
"""
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.zeros((3,))
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
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

75
rotorpy/trajectories/lissajous_traj.py Normal file
View File

@@ -0,0 +1,75 @@
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

351
rotorpy/trajectories/minsnap.py Normal file
View File

@@ -0,0 +1,351 @@
"""
Imports
"""
import numpy as np
import cvxopt
from scipy.linalg import block_diag
def cvxopt_solve_qp(P, q=None, G=None, h=None, A=None, b=None):
"""
From https://scaron.info/blog/quadratic-programming-in-python.html . Infrastructure code for solving quadratic programs using CVXOPT.
The structure of the program is as follows:
min 0.5 xT P x + qT x
s.t. Gx <= h
Ax = b
Inputs:
P, numpy array, the quadratic term of the cost function
q, numpy array, the linear term of the cost function
G, numpy array, inequality constraint matrix
h, numpy array, inequality constraint vector
A, numpy array, equality constraint matrix
b, numpy array, equality constraint vector
Outputs:
The optimal solution to the quadratic program
"""
P = .5 * (P + P.T) # make sure P is symmetric
args = [cvxopt.matrix(P), cvxopt.matrix(q)]
if G is not None:
args.extend([cvxopt.matrix(G), cvxopt.matrix(h)])
if A is not None:
args.extend([cvxopt.matrix(A), cvxopt.matrix(b)])
sol = cvxopt.solvers.qp(*args)
if 'optimal' not in sol['status']:
return None
return np.array(sol['x']).reshape((P.shape[1],))
def H_fun(dt):
"""
Computes the cost matrix for a single segment in a single dimension.
*** Assumes that the decision variables c_i are e.g. x(t) = c_0 + c_1*t + c_2*t^2 + c_3*t^3 + c_4*t^4 + c_5*t^5 + c_6*t^6 + c_7*t^7
Inputs:
dt, scalar, the duration of the segment (t_(i+1) - t_i)
Outputs:
H, numpy array, matrix containing the min snap cost function for that segment. Assumes the polynomial is of order 7.
"""
cost = np.array([[576*dt, 1440*dt**2, 2880*dt**3, 5040*dt**4],
[1440*dt**2, 4800*dt**3, 10800*dt**4, 20160*dt**5],
[2880*dt**3, 10800*dt**4, 25920*dt**5, 50400*dt**6],
[5040*dt**4, 20160*dt**5, 50400*dt**6, 100800*dt**7]])
H = np.zeros((8,8))
H[4:,4:] = cost
return H
def get_1d_equality_constraints(keyframes, delta_t, m, vmax=2):
"""
Computes the equality constraints for the min snap problem.
*** Assumes that the decision variables c_i are e.g. x(t) = c_0 + c_1*t + c_2*t^2 + c_3*t^3 + c_4*t^4 + c_5*t^5 + c_6*t^6 + c_7*t^7
Inputs:
keyframes, numpy array, a list of m waypoints IN ONE DIMENSION (x,y,z, or yaw)
"""
# N = keyframes.shape[0] # the number of keyframes
K = 8*m # the number of decision variables
A = np.zeros((8*m, K))
b = np.zeros((8*m,))
G = np.zeros((m, K))
h = np.zeros((m))
for i in range(m): # for each segment...
# Compute the segment duration
dt = delta_t[i]
# Position continuity at the beginning of the segment
A[i, 8*i:(8*i+8)] = np.array([1, 0, 0, 0, 0, 0, 0, 0])
b[i] = keyframes[i]
# Position continuity at the end of the segment
A[m+i, 8*i:(8*i+8)] = np.array([1, dt, dt**2, dt**3, dt**4, dt**5, dt**6, dt**7])
b[m+i] = keyframes[i+1]
###### At this point we have 2*m constraints..
# Intermediate smoothness constraints
if i < (m-1): # we don't want to include the last segment for this loop
A[2*m + 6*i + 0, 8*i:(8*i+16)] = np.array([0, -1, -2*dt, -3*dt**2, -4*dt**3, -5*dt**4, -6*dt**5, -7*dt**6, 0, 1, 0, 0, 0, 0, 0, 0]) # Velocity
A[2*m + 6*i + 1, 8*i:(8*i+16)] = np.array([0, 0, -2, -6*dt, -12*dt**2, -20*dt**3, -30*dt**4, -42*dt**5, 0, 0, 2, 0, 0, 0, 0, 0]) # Acceleration
A[2*m + 6*i + 2, 8*i:(8*i+16)] = np.array([0, 0, 0, -6, -24*dt, -60*dt**2, -120*dt**3, -210*dt**4, 0, 0, 0, 6, 0, 0, 0, 0]) # Jerk
A[2*m + 6*i + 3, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, -24, -120*dt, -360*dt**2, -840*dt**3, 0, 0, 0, 0, 24, 0, 0, 0]) # Snap
A[2*m + 6*i + 4, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, 0, -120, -720*dt, -2520*dt**2, 0, 0, 0, 0, 0, 120, 0, 0]) # 5TH derivative
A[2*m + 6*i + 5, 8*i:(8*i+16)] = np.array([0, 0, 0, 0, 0, 0, -720, -5040*dt, 0, 0, 0, 0, 0, 0, 720, 0]) # 6TH derivative
b[2*m + 6*i + 0] = 0
b[2*m + 6*i + 1] = 0
b[2*m + 6*i + 2] = 0
b[2*m + 6*i + 3] = 0
b[2*m + 6*i + 4] = 0
b[2*m + 6*i + 5] = 0
G[i, 8*i:(8*i + 8)] = np.array([0, 1, 2*(0.5*dt), 3*(0.5*dt)**2, 4*(0.5*dt)**3, 5*(0.5*dt)**4, 6*(0.5*dt)**5, 7*(0.5*dt)**6])
h[i] = vmax
# Now we have added an addition 6*(m-1) constraints, total 2*m + 6*(m-1) = 2m + 8m - 6 = 8m - 6 constraints
# Last constraints are on the first and last trajectory segments. Higher derivatives are = 0
A[8*m - 6, 0:8] = np.array([0, 1, 0, 0, 0, 0, 0, 0]) # Velocity = 0 at start
b[8*m - 6] = 0
A[8*m - 5, -8:] = np.array([0, 1, 2*dt, 3*dt**2, 4*dt**3, 5*dt**4, 6*dt**5, 7*dt**6]) # Velocity = 0 at end
b[8*m - 5] = 0
A[8*m - 4, 0:8] = np.array([0, 0, 2, 0, 0, 0, 0, 0]) # Acceleration = 0 at start
b[8*m - 4] = 0
A[8*m - 3, -8:] = np.array([0, 0, 2, 6*dt,12*dt**2, 20*dt**3, 30*dt**4, 42*dt**5]) # Acceleration = 0 at end
b[8*m - 3] = 0
A[8*m - 2, 0:8] = np.array([0, 0, 0, 6, 0, 0, 0, 0]) # Jerk = 0 at start
b[8*m - 2] = 0
A[8*m - 1, -8:] = np.array([0, 0, 0, 6, 24*dt, 60*dt**2, 120*dt**3, 210*dt**4]) # Jerk = 0 at end
b[8*m - 1] = 0
return (A, b, G, h)
class MinSnap(object):
"""
MinSnap generates a minimum snap trajectory for the quadrotor, following https://ieeexplore.ieee.org/document/5980409.
The trajectory is a piecewise 7th order polynomial (minimum degree necessary for snap optimality).
"""
def __init__(self, points, yaw_angles=None, v_avg=2):
"""
Waypoints and yaw angles compose the "keyframes" for optimizing over.
Inputs:
points, numpy array of m 3D waypoints.
yaw_angles, numpy array of m yaw angles corresponding to each waypoint.
v_avg, the average speed between waypoints, this is used to do the time allocation.
"""
if yaw_angles is None:
self.yaw = np.zeros((points.shape[0]))
else:
self.yaw = yaw_angles
self.v_avg = v_avg
# Compute the distances between each waypoint.
seg_dist = np.linalg.norm(np.diff(points, axis=0), axis=1)
seg_mask = np.append(True, seg_dist > 1e-3)
self.points = points[seg_mask,:]
self.null = False
m = self.points.shape[0]-1 # Get the number of segments
# Compute the derivatives of the polynomials
self.x_dot_poly = np.zeros((m, 3, 7))
self.x_ddot_poly = np.zeros((m, 3, 6))
self.x_dddot_poly = np.zeros((m, 3, 5))
self.x_ddddot_poly = np.zeros((m, 3, 4))
self.yaw_dot_poly = np.zeros((m, 1, 7))
# If two or more waypoints remain, solve min snap
if self.points.shape[0] >= 2:
################## Time allocation
self.delta_t = seg_dist/self.v_avg # Compute the segment durations based on the average velocity
self.t_keyframes = np.concatenate(([0], np.cumsum(self.delta_t))) # Construct time array which indicates when the quad should be at the i'th waypoint.
################## Cost function
# First get the cost segment for each matrix:
H = [H_fun(self.delta_t[i]) for i in range(m)]
# Now concatenate these costs using block diagonal form:
P = block_diag(*H)
# Lastly the linear term in the cost function is 0
q = np.zeros((8*m,1))
################## Constraints for each axis
(Ax,bx,Gx,hx) = get_1d_equality_constraints(self.points[:,0], self.delta_t, m)
(Ay,by,Gy,hy) = get_1d_equality_constraints(self.points[:,1], self.delta_t, m)
(Az,bz,Gz,hz) = get_1d_equality_constraints(self.points[:,2], self.delta_t, m)
(Ayaw,byaw,Gyaw,hyaw) = get_1d_equality_constraints(self.yaw, self.delta_t, m)
################## Solve for x, y, z, and yaw
c_opt_x = np.linalg.solve(Ax,bx)
c_opt_y = np.linalg.solve(Ay,by)
c_opt_z = np.linalg.solve(Az,bz)
c_opt_yaw = np.linalg.solve(Ayaw,byaw)
################## Construct polynomials from c_opt
self.x_poly = np.zeros((m, 3, 8))
self.yaw_poly = np.zeros((m, 1, 8))
for i in range(m):
self.x_poly[i,0,:] = np.flip(c_opt_x[8*i:(8*i+8)])
self.x_poly[i,1,:] = np.flip(c_opt_y[8*i:(8*i+8)])
self.x_poly[i,2,:] = np.flip(c_opt_z[8*i:(8*i+8)])
self.yaw_poly[i,0,:] = np.flip(c_opt_yaw[8*i:(8*i+8)])
for i in range(m):
for j in range(3):
self.x_dot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=1)
self.x_ddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=2)
self.x_dddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=3)
self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
self.yaw_dot_poly[i,0,:] = np.polyder(self.yaw_poly[i,0,:], m=1)
else:
# Otherwise, there is only one waypoint so we just set everything = 0.
self.null = True
m = 1
self.T = np.zeros((m,))
self.x_poly = np.zeros((m, 3, 6))
self.x_poly[0,:,-1] = points[0,:]
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.zeros((3,))
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
if self.null:
# If there's only one waypoint
x = self.points[0,:]
else:
# Find interval index i and time within interval t.
t = np.clip(t, self.t_keyframes[0], self.t_keyframes[-1])
for i in range(self.t_keyframes.size-1):
if self.t_keyframes[i] + self.delta_t[i] >= t:
break
t = t - self.t_keyframes[i]
# Evaluate polynomial.
for j in range(3):
x[j] = np.polyval( self.x_poly[i,j,:], t)
x_dot[j] = np.polyval( self.x_dot_poly[i,j,:], t)
x_ddot[j] = np.polyval( self.x_ddot_poly[i,j,:], t)
x_dddot[j] = np.polyval( self.x_dddot_poly[i,j,:], t)
x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
yaw = np.polyval(self.yaw_poly[i, 0, :], t)
yaw_dot = np.polyval(self.yaw_dot_poly[i,0,:], t)
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
if __name__=="__main__":
import matplotlib.pyplot as plt
from matplotlib import cm
waypoints = np.array([[0,0,0],
[1,0,0],
[1,1,0],
[0,1,0],
[0,2,0],
[2,2,0]
])
yaw_angles = np.array([0, np.pi/2, 0, np.pi/4, 3*np.pi/2, 0])
traj = MinSnap(waypoints, yaw_angles, v_avg=2)
N = 1000
time = np.linspace(0,5,N)
x = np.zeros((N, 3))
xdot = np.zeros((N,3))
xddot = np.zeros((N,3))
xdddot = np.zeros((N,3))
xddddot = np.zeros((N,3))
yaw = np.zeros((N,))
yaw_dot = np.zeros((N,))
for i in range(N):
flat = traj.update(time[i])
x[i,:] = flat['x']
xdot[i,:] = flat['x_dot']
xddot[i,:] = flat['x_ddot']
xdddot[i,:] = flat['x_dddot']
xddddot[i,:] = flat['x_ddddot']
yaw[i] = flat['yaw']
yaw_dot[i] = flat['yaw_dot']
(fig, axes) = plt.subplots(nrows=5, ncols=1, sharex=True, num="Translational Flat Outputs")
ax = axes[0]
ax.plot(time, x[:,0], 'r-', label="X")
ax.plot(time, x[:,1], 'g-', label="Y")
ax.plot(time, x[:,2], 'b-', label="Z")
ax.legend()
ax.set_ylabel("x")
ax = axes[1]
ax.plot(time, xdot[:,0], 'r-', label="X")
ax.plot(time, xdot[:,1], 'g-', label="Y")
ax.plot(time, xdot[:,2], 'b-', label="Z")
ax.set_ylabel("xdot")
ax = axes[2]
ax.plot(time, xddot[:,0], 'r-', label="X")
ax.plot(time, xddot[:,1], 'g-', label="Y")
ax.plot(time, xddot[:,2], 'b-', label="Z")
ax.set_ylabel("xddot")
ax = axes[3]
ax.plot(time, xdddot[:,0], 'r-', label="X")
ax.plot(time, xdddot[:,1], 'g-', label="Y")
ax.plot(time, xdddot[:,2], 'b-', label="Z")
ax.set_ylabel("xdddot")
ax = axes[4]
ax.plot(time, xddddot[:,0], 'r-', label="X")
ax.plot(time, xddddot[:,1], 'g-', label="Y")
ax.plot(time, xddddot[:,2], 'b-', label="Z")
ax.set_ylabel("xddddot")
ax.set_xlabel("Time, s")
(fig, axes) = plt.subplots(nrows=2, ncols=1, sharex=True, num="Yaw vs Time")
ax = axes[0]
ax.plot(time, yaw, 'k', label="yaw")
ax.set_ylabel("yaw")
ax = axes[1]
ax.plot(time, yaw_dot, 'k', label="yaw")
ax.set_ylabel("yaw dot")
ax.set_xlabel("Time, s")
speed = np.sqrt(xdot[:,0]**2 + xdot[:,1]**2)
(fig, axes) = plt.subplots(nrows=1, ncols=1, num="XY Trajectory")
ax = axes
ax.scatter(x[:,0], x[:,1], c=cm.winter(speed/speed.max()), s=4, label="Flat Output")
ax.plot(waypoints[:,0], waypoints[:,1], 'ko', markersize=10, label="Waypoints")
ax.grid()
ax.legend()
plt.show()

107
rotorpy/trajectories/polynomial_traj.py Normal file
View File

@@ -0,0 +1,107 @@
import numpy as np
class Polynomial(object):
"""
"""
def __init__(self, points, v_avg=1.2):
"""
Inputs:
points, (N, 3) array of N waypoint coordinates in 3D
v_avg, the average speed between segments
"""
def get_poly(xi, xf, T):
"""
Return fully constrained polynomial coefficients from xi to xf in
time interval [0,T]. Low derivatives are all zero at the endpoints.
"""
A = np.array([[ 0, 0, 0, 0, 0, 1],
[ T**5, T**4, T**3, T**2, T, 1],
[ 0, 0, 0, 0, 1, 0],
[ 5*T**4, 4*T**3, 3*T**2, 2*T, 1, 0],
[ 0, 0, 0, 2, 0, 0],
[20*T**3, 12*T**2, 6*T, 2, 0, 0]])
b = np.array([xi, xf, 0, 0, 0, 0])
poly = np.linalg.solve(A,b)
return poly
self.v_avg = v_avg
# Remove any sequential duplicate points; always keep the first point.
seg_dist = np.linalg.norm(np.diff(points, axis=0), axis=1)
seg_mask = np.append(True, seg_dist > 1e-3)
points = points[seg_mask,:]
# If at least two waypoints remain, calculate segment polynomials.
if points.shape[0] >= 2:
N = points.shape[0]-1
self.T = seg_dist/self.v_avg # segment duration
self.x_poly = np.zeros((N, 3, 6))
for i in range(N):
for j in range(3):
self.x_poly[i,j,:] = get_poly(points[i,j], points[i+1,j], self.T[i])
# Otherwise, hard code constant polynomial at initial waypoint position.
else:
N = 1
self.T = np.zeros((N,))
self.x_poly = np.zeros((N, 3, 6))
self.x_poly[0,:,-1] = points[0,:]
# Calculate global start time of each segment.
self.t_start = np.concatenate(([0], np.cumsum(self.T[:-1])))
# Calculate derivative polynomials.
self.x_dot_poly = np.zeros((N, 3, 5))
self.x_ddot_poly = np.zeros((N, 3, 4))
self.x_dddot_poly = np.zeros((N, 3, 3))
self.x_ddddot_poly = np.zeros((N, 3, 2))
for i in range(N):
for j in range(3):
self.x_dot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=1)
self.x_ddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=2)
self.x_dddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=3)
self.x_ddddot_poly[i,j,:] = np.polyder(self.x_poly[i,j,:], m=4)
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.zeros((3,))
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
# Find interval index i and time within interval t.
t = np.clip(t, self.t_start[0], self.t_start[-1]+self.T[-1])
for i in range(self.t_start.size):
if self.t_start[i] + self.T[i] >= t:
break
t = t - self.t_start[i]
# Evaluate polynomial.
for j in range(3):
x[j] = np.polyval( self.x_poly[i,j,:], t)
x_dot[j] = np.polyval( self.x_dot_poly[i,j,:], t)
x_ddot[j] = np.polyval( self.x_ddot_poly[i,j,:], t)
x_dddot[j] = np.polyval( self.x_dddot_poly[i,j,:], t)
x_ddddot[j] = np.polyval(self.x_ddddot_poly[i,j,:], t)
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

105
rotorpy/trajectories/speed_traj.py Normal file
View File

@@ -0,0 +1,105 @@
"""
Imports
"""
import numpy as np
class ConstantSpeed(object):
"""
"""
def __init__(self, init_pos, dist=1, speed=1, axis=0, repeat=False):
"""
Constant speed will command a step response in speed on a specified axis. The following inputs
try to ensure that the vehicle is not commanded to go beyond the boundaries of the environment.
INPUTS
init_pos := the inital position for the trajectory to begin, should be the current quadrotor's position
dist := the length of the trajectory in meters.
speed := the speed of the trajectory in meters.
axis := the axis to travel (0 -> X, 1 -> Y, 2 -> Z)
repeat := determines if the trajectory should repeat, where the direction is switched from its current state.
"""
self.pt1 = init_pos
self.dist = dist # m
self.speed = speed # m/s
# Based on the length of the trajectory (distance) and the desired speed, compute how long the desired speed should be commaned.
self.t_width = self.dist/self.speed # seconds
# Create a vector where the "axis'th" element is 1 and the other elements are 0. This will multiplied by the flat outputs
# so that the only "active" axis is the one specified by "axis"
self.active_axis = np.zeros((3,))
self.active_axis[axis] = 1
self.direction = 1
self.repeat = repeat
self.reverse_threshold = 0.01
self.pt2 = self.pt1 + self.speed*self.t_width*self.active_axis
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
"""
v = (self.speed)*self.active_axis*self.direction
if t <= self.t_width:
x = self.pt1 + v*t
x_dot = v
else:
x = self.pt2
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
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
if __name__=="__main__":
import matplotlib.pyplot as plt
traj = ConstantSpeed(init_pos=np.array([0,0,0]))
t = np.linspace(0,10,500)
x = np.zeros((t.shape[0], 3))
for i in range(t.shape[0]):
flat = traj.update(t[i])
x[i,:] = flat['x']
(fig, axes) = plt.subplots(nrows=3, ncols=1, num="Constant Speed Trajectory")
ax = axes[0]
ax.plot(t, x[:,0], 'r', linewidth=1.5)
ax.set_ylabel("X, m")
ax = axes[1]
ax.plot(t, x[:,1], 'g', linewidth=1.5)
ax.set_ylabel("Y, m")
ax = axes[2]
ax.plot(t, x[:,2], 'b', linewidth=1.5)
ax.set_ylabel("Z, m")
ax.set_xlabel("Time, s")
plt.show()

46
rotorpy/trajectories/traj_template.py Normal file
View File

@@ -0,0 +1,46 @@
"""
Imports
"""
import numpy as np
class TrajTemplate(object):
"""
The trajectory is implemented as a class. There are two required methods for each trajectory class: __init__() and update().
The __init__() is required for instantiating the class with appropriate parameterizations. For example, if you are doing
a circular trajectory you might want to specify the radius of the circle.
The update() method is called at each iteration of the simulator. The only input to update is time t. The output of update()
should be the desired flat outputs in a dictionary, as specified below.
"""
def __init__(self):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
"""
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.zeros((3,))
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
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