-
Notifications
You must be signed in to change notification settings - Fork 67
/
Copy pathcontroller.py
148 lines (109 loc) · 5.49 KB
/
controller.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
"""
PID Controller
components:
follow attitude commands
gps commands and yaw
waypoint following
"""
import numpy as np
from frame_utils import euler2RM
DRONE_MASS_KG = 0.5
GRAVITY = -9.81
MOI = np.array([0.005, 0.005, 0.01])
MAX_THRUST = 10.0
MAX_TORQUE = 1.0
class NonlinearController(object):
def __init__(self):
"""Initialize the controller object and control gains"""
return
def trajectory_control(self, position_trajectory, yaw_trajectory, time_trajectory, current_time):
"""Generate a commanded position, velocity and yaw based on the trajectory
Args:
position_trajectory: list of 3-element numpy arrays, NED positions
yaw_trajectory: list yaw commands in radians
time_trajectory: list of times (in seconds) that correspond to the position and yaw commands
current_time: float corresponding to the current time in seconds
Returns: tuple (commanded position, commanded velocity, commanded yaw)
"""
ind_min = np.argmin(np.abs(np.array(time_trajectory) - current_time))
time_ref = time_trajectory[ind_min]
if current_time < time_ref:
position0 = position_trajectory[ind_min - 1]
position1 = position_trajectory[ind_min]
time0 = time_trajectory[ind_min - 1]
time1 = time_trajectory[ind_min]
yaw_cmd = yaw_trajectory[ind_min - 1]
else:
yaw_cmd = yaw_trajectory[ind_min]
if ind_min >= len(position_trajectory) - 1:
position0 = position_trajectory[ind_min]
position1 = position_trajectory[ind_min]
time0 = 0.0
time1 = 1.0
else:
position0 = position_trajectory[ind_min]
position1 = position_trajectory[ind_min + 1]
time0 = time_trajectory[ind_min]
time1 = time_trajectory[ind_min + 1]
position_cmd = (position1 - position0) * \
(current_time - time0) / (time1 - time0) + position0
velocity_cmd = (position1 - position0) / (time1 - time0)
return (position_cmd, velocity_cmd, yaw_cmd)
def lateral_position_control(self, local_position_cmd, local_velocity_cmd, local_position, local_velocity,
acceleration_ff = np.array([0.0, 0.0])):
"""Generate horizontal acceleration commands for the vehicle in the local frame
Args:
local_position_cmd: desired 2D position in local frame [north, east]
local_velocity_cmd: desired 2D velocity in local frame [north_velocity, east_velocity]
local_position: vehicle position in the local frame [north, east]
local_velocity: vehicle velocity in the local frame [north_velocity, east_velocity]
acceleration_cmd: feedforward acceleration command
Returns: desired vehicle 2D acceleration in the local frame [north, east]
"""
return np.array([0.0, 0.0])
def altitude_control(self, altitude_cmd, vertical_velocity_cmd, altitude, vertical_velocity, attitude, acceleration_ff=0.0):
"""Generate vertical acceleration (thrust) command
Args:
altitude_cmd: desired vertical position (+up)
vertical_velocity_cmd: desired vertical velocity (+up)
altitude: vehicle vertical position (+up)
vertical_velocity: vehicle vertical velocity (+up)
attitude: the vehicle's current attitude, 3 element numpy array (roll, pitch, yaw) in radians
acceleration_ff: feedforward acceleration command (+up)
Returns: thrust command for the vehicle (+up)
Hints:
- Note that the return variable is thrust, not just acceleration, so make sure to take into account the
drone's mass accordingly!
"""
return 0.0
def roll_pitch_controller(self, acceleration_cmd, attitude, thrust_cmd):
""" Generate the rollrate and pitchrate commands in the body frame
Args:
target_acceleration: 2-element numpy array (north_acceleration_cmd,east_acceleration_cmd) in m/s^2
attitude: 3-element numpy array (roll, pitch, yaw) in radians
thrust_cmd: vehicle thruts command in Newton
Returns: 2-element numpy array, desired rollrate (p) and pitchrate (q) commands in radians/s
Hints:
- Note that the last command parameter is thrust, not acceleration. If you plan on using acceleration in your
controller, make sure to account for the drone's mass!
"""
return np.array([0.0, 0.0])
def body_rate_control(self, body_rate_cmd, body_rate):
""" Generate the roll, pitch, yaw moment commands in the body frame
Args:
body_rate_cmd: 3-element numpy array (p_cmd,q_cmd,r_cmd) in radians/second^2
body_rate: 3-element numpy array (p,q,r) in radians/second^2
Returns: 3-element numpy array, desired roll moment, pitch moment, and yaw moment commands in Newtons*meters
Hints:
- Note the output of this function in the desired moment for the drone, so make sure to take the moment of
inertia of the drone into account accordingly!
"""
return np.array([0.0, 0.0, 0.0])
def yaw_control(self, yaw_cmd, yaw):
""" Generate the target yawrate
Args:
yaw_cmd: desired vehicle yaw in radians
yaw: vehicle yaw in radians
Returns: target yawrate in radians/sec
"""
return 0.0