forked from aerorobotics/simple-quad-sim
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsim.py
187 lines (163 loc) · 6.92 KB
/
sim.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
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
import scipy.spatial.transform
import numpy as np
from animate_function import QuadPlotter
def quat_mult(q, p):
# q * p
# p,q = [w x y z]
return np.array(
[
p[0] * q[0] - q[1] * p[1] - q[2] * p[2] - q[3] * p[3],
q[1] * p[0] + q[0] * p[1] + q[2] * p[3] - q[3] * p[2],
q[2] * p[0] + q[0] * p[2] + q[3] * p[1] - q[1] * p[3],
q[3] * p[0] + q[0] * p[3] + q[1] * p[2] - q[2] * p[1],
]
)
def quat_conjugate(q):
return np.array([q[0], -q[1], -q[2], -q[3]])
def quaternion_from_vectors(v_from, v_to):
v_from = normalized(v_from)
v_to = normalized(v_to)
v_mid = normalized(v_from + v_to)
q = np.array([np.dot(v_from, v_mid), *np.cross(v_from, v_mid)])
return q
def normalized(v):
norm = np.linalg.norm(v)
return v / norm
NO_STATES = 13
IDX_POS_X = 0
IDX_POS_Y = 1
IDX_POS_Z = 2
IDX_VEL_X = 3
IDX_VEL_Y = 4
IDX_VEL_Z = 5
IDX_QUAT_W = 6
IDX_QUAT_X = 7
IDX_QUAT_Y = 8
IDX_QUAT_Z = 9
IDX_OMEGA_X = 10
IDX_OMEGA_Y = 11
IDX_OMEGA_Z = 12
class Robot:
'''
frames:
B - body frame
I - inertial frame
states:
p_I - position of the robot in the inertial frame (state[0], state[1], state[2])
v_I - velocity of the robot in the inertial frame (state[3], state[4], state[5])
q - orientation of the robot (w=state[6], x=state[7], y=state[8], z=state[9])
omega - angular velocity of the robot (state[10], state[11], state[12])
inputs:
omega_1, omega_2, omega_3, omega_4 - angular velocities of the motors
'''
def __init__(self):
self.m = 1.0 # mass of the robot
self.arm_length = 0.25 # length of the quadcopter arm (motor to center)
self.height = 0.05 # height of the quadcopter
self.body_frame = np.array([(self.arm_length, 0, 0, 1),
(0, self.arm_length, 0, 1),
(-self.arm_length, 0, 0, 1),
(0, -self.arm_length, 0, 1),
(0, 0, 0, 1),
(0, 0, self.height, 1)])
self.J = 0.025 * np.eye(3) # [kg m^2]
self.J_inv = np.linalg.inv(self.J)
self.constant_thrust = 10e-4
self.constant_drag = 10e-6
self.omega_motors = np.array([0.0, 0.0, 0.0, 0.0])
self.state = self.reset_state_and_input(np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0, 0.0]))
self.time = 0.0
def reset_state_and_input(self, init_xyz, init_quat_wxyz):
state0 = np.zeros(NO_STATES)
state0[IDX_POS_X:IDX_POS_Z+1] = init_xyz
state0[IDX_VEL_X:IDX_VEL_Z+1] = np.array([0.0, 0.0, 0.0])
state0[IDX_QUAT_W:IDX_QUAT_Z+1] = init_quat_wxyz
state0[IDX_OMEGA_X:IDX_OMEGA_Z+1] = np.array([0.0, 0.0, 0.0])
return state0
def update(self, omegas_motor, dt):
p_I = self.state[IDX_POS_X:IDX_POS_Z+1]
v_I = self.state[IDX_VEL_X:IDX_VEL_Z+1]
q = self.state[IDX_QUAT_W:IDX_QUAT_Z+1]
omega = self.state[IDX_OMEGA_X:IDX_OMEGA_Z+1]
R = scipy.spatial.transform.Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
thrust = self.constant_thrust * np.sum(omegas_motor**2)
f_b = np.array([0, 0, thrust])
tau_x = self.constant_thrust * (omegas_motor[3]**2 - omegas_motor[1]**2) * 2 * self.arm_length
tau_y = self.constant_thrust * (omegas_motor[2]**2 - omegas_motor[0]**2) * 2 * self.arm_length
tau_z = self.constant_drag * (omegas_motor[0]**2 - omegas_motor[1]**2 + omegas_motor[2]**2 - omegas_motor[3]**2)
tau_b = np.array([tau_x, tau_y, tau_z])
v_dot = 1 / self.m * R @ f_b + np.array([0, 0, -9.81])
omega_dot = self.J_inv @ (np.cross(self.J @ omega, omega) + tau_b)
q_dot = 1 / 2 * quat_mult(q, [0, *omega])
p_dot = v_I
x_dot = np.concatenate([p_dot, v_dot, q_dot, omega_dot])
self.state += x_dot * dt
self.state[IDX_QUAT_W:IDX_QUAT_Z+1] /= np.linalg.norm(self.state[IDX_QUAT_W:IDX_QUAT_Z+1]) # Re-normalize quaternion.
self.time += dt
def control(self, p_d_I):
p_I = self.state[IDX_POS_X:IDX_POS_Z+1]
v_I = self.state[IDX_VEL_X:IDX_VEL_Z+1]
q = self.state[IDX_QUAT_W:IDX_QUAT_Z+1]
omega_b = self.state[IDX_OMEGA_X:IDX_OMEGA_Z+1]
# Position controller.
k_p = 1.0
k_d = 10.0
v_r = - k_p * (p_I - p_d_I)
a = -k_d * (v_I - v_r) + np.array([0, 0, 9.81])
f = self.m * a
f_b = scipy.spatial.transform.Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix().T @ f
thrust = np.max([0, f_b[2]])
# Attitude controller.
q_ref = quaternion_from_vectors(np.array([0, 0, 1]), normalized(f))
q_err = quat_mult(quat_conjugate(q_ref), q) # error from Body to Reference.
if q_err[0] < 0:
q_err = -q_err
k_q = 20.0
k_omega = 100.0
omega_ref = - k_q * 2 * q_err[1:]
alpha = - k_omega * (omega_b - omega_ref)
tau = self.J @ alpha + np.cross(omega_b, self.J @ omega_b)
# Compute the motor speeds.
B = np.array([
[self.constant_thrust, self.constant_thrust, self.constant_thrust, self.constant_thrust],
[0, -self.arm_length * self.constant_thrust, 0, self.arm_length * self.constant_thrust],
[-self.arm_length * self.constant_thrust, 0, self.arm_length * self.constant_thrust, 0],
[self.constant_drag, -self.constant_drag, self.constant_drag, -self.constant_drag]
])
B_inv = np.linalg.inv(B)
omega_motor_square = B_inv @ np.concatenate([np.array([thrust]), tau])
omega_motor = np.sqrt(np.clip(omega_motor_square, 0, None))
return omega_motor
PLAYBACK_SPEED = 1
CONTROL_FREQUENCY = 200 # Hz for attitude control loop
dt = 1.0 / CONTROL_FREQUENCY
time = [0.0]
def get_pos_full_quadcopter(quad):
""" position returns a 3 x 6 matrix
where row is [x, y, z] column is m1 m2 m3 m4 origin h
"""
origin = quad.state[IDX_POS_X:IDX_POS_Z+1]
quat = quad.state[IDX_QUAT_W:IDX_QUAT_Z+1]
rot = scipy.spatial.transform.Rotation.from_quat(quat, scalar_first=True).as_matrix()
wHb = np.r_[np.c_[rot,origin], np.array([[0, 0, 0, 1]])]
quadBodyFrame = quad.body_frame.T
quadWorldFrame = wHb.dot(quadBodyFrame)
pos_full_quad = quadWorldFrame[0:3]
return pos_full_quad
def control_propellers(quad):
t = quad.time
T = 1.5
r = 2*np.pi * t / T
prop_thrusts = quad.control(p_d_I = np.array([np.cos(r/2), np.sin(r), 0.0]))
# Note: for Hover mode, just replace the desired trajectory with [1, 0, 1]
quad.update(prop_thrusts, dt)
def main():
quadcopter = Robot()
def control_loop(i):
for _ in range(PLAYBACK_SPEED):
control_propellers(quadcopter)
return get_pos_full_quadcopter(quadcopter)
plotter = QuadPlotter()
plotter.plot_animation(control_loop)
if __name__ == "__main__":
main()