def step(self, action):
"""Propagate dynamics."""
pos = self.state[0:3]
vel = self.state[3:6]
quat = self.state[6:10]
ang_vel = action[0:3]
thrust = action[4]
goal_state = np.array([5, 5, 5, 0, 0, 0, 1, 0, 0, 0])
cost = np.transpose(self.state - goal_state) @ \
np.eye(self.state.shape[0]) @ (self.state - goal_state)
quat = Quaternion(quat)
rotation_matrix = quat.rotation_matrix
pos_dot = vel
vel_dot = (thrust / self.mass) * \
np.array([0, 0, 1]) @ rotation_matrix - \
np.array([0, 0, 1]) * self.gravity
quat_dot = quat.derivative(ang_vel)
pos = pos + self.dt * pos_dot
vel = vel + self.dt * vel_dot
quat = quat + self.dt * quat_dot
quat = quat.normalize
self.time += self.dt
self.state = np.hstack((pos, vel, quat))
return self.state, -cost, False, {'time': self.time}
def step(self, action):
thrust = action[0] # Thrust command
w = action[1:4] # Angular velocity command
state = self.state
ref_pos = self.ref_pos
ref_vel = self.ref_vel
pos = np.array([state[0], state[1], state[2]]).flatten()
att = np.array([state[3], state[4], state[5], state[6]]).flatten()
vel = np.array([state[7], state[8], state[9]]).flatten()
att_quaternion = Quaternion(att)
acc = thrust / self.mass * att_quaternion.rotation_matrix.dot(
np.array([0.0, 0.0, 1.0])) + self.g
pos = pos + vel * self.dt + 0.5 * acc * self.dt * self.dt
vel = vel + acc * self.dt
q_dot = att_quaternion.derivative(w)
att = att + q_dot.elements * self.dt
self.state = (pos[0], pos[1], pos[2], att[0], att[1], att[2], att[3],
vel[0], vel[1], vel[2])
done = linalg.norm(pos, 2) < -self.pos_threshold \
and linalg.norm(pos, 2) > self.pos_threshold \
and linalg.norm(vel, 2) < -self.vel_threshold \
and linalg.norm(vel, 2) > self.vel_threshold
done = bool(done)
if not done:
reward = (-linalg.norm(pos, 2))
elif self.steps_beyond_done is None:
# Pole just fell!
self.steps_beyond_done = 0
reward = 1.0
else:
if self.steps_beyond_done == 0:
logger.warn(
"You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior."
)
self.steps_beyond_done += 1
reward = 0.0
return np.array(self.state), reward, done, {}
def step(self, action):
thrust = action[0] # Thrust command
w = action[1:4] # Angular velocity command
state = self.state
ref_pos = self.ref_pos
ref_vel = self.ref_vel
pos = np.array([state[0], state[1], state[2]]).flatten()
att = np.array([state[3], state[4], state[5], state[6]]).flatten()
vel = np.array([state[7], state[8], state[9]]).flatten()
load_pos = np.array([state[10], state[11], state[12]]).flatten()
load_vel = np.array([state[13], state[14], state[15]]).flatten()
tether_vec = load_pos - pos
unit_tether_vec = tether_vec / linalg.norm(tether_vec)
if linalg.norm(tether_vec) >= self.tether_length:
#Quadrotor Dynamics
att_quaternion = Quaternion(att)
thrust_vec = thrust * att_quaternion.rotation_matrix.dot(
np.array([0.0, 0.0, 1.0]))
load_acceleration = np.inner(
unit_tether_vec, thrust_vec - self.mass * self.tether_length *
np.inner(load_vel, load_vel)) * unit_tether_vec
load_acceleration = (
1 / (self.mass + self.load_mass)) * load_acceleration + self.g
load_pos = load_pos + load_vel * self.dt + 0.5 * load_acceleration * self.dt * self.dt
load_vel = load_vel + load_acceleration * self.dt
T = self.load_mass * linalg.norm(
-self.g + load_acceleration) * unit_tether_vec
#Quadrotor Dynamics
acc = thrust / self.mass * att_quaternion.rotation_matrix.dot(
np.array([0.0, 0.0, 1.0])) + self.g + T / self.mass
pos = pos + vel * self.dt + 0.5 * acc * self.dt * self.dt
vel = vel + acc * self.dt
q_dot = att_quaternion.derivative(w)
att = att + q_dot.elements * self.dt
## Enforce kinematic constraints
load_direction = (load_pos - pos) / linalg.norm(load_pos - pos)
load_pos = pos + load_direction * self.tether_length
load_vel = load_vel - np.inner(load_vel - vel,
load_direction) * load_direction
else:
att_quaternion = Quaternion(att)
# Load dynamics
load_acceleration = self.g
load_pos = load_pos + load_vel * self.dt + 0.5 * load_acceleration * self.dt * self.dt
load_vel = load_vel + load_acceleration * self.dt
# Quadrotor Dynamics
acc = thrust / self.mass * att_quaternion.rotation_matrix.dot(
np.array([0.0, 0.0, 1.0])) + self.g
pos = pos + vel * self.dt + 0.5 * acc * self.dt * self.dt
vel = vel + acc * self.dt
q_dot = att_quaternion.derivative(w)
att = att + q_dot.elements * self.dt
self.state = (pos[0], pos[1], pos[2], att[0], att[1], att[2], att[3],
vel[0], vel[1], vel[2], load_pos[0], load_pos[1],
load_pos[2], load_vel[0], load_vel[1], load_vel[2])
done = linalg.norm(load_pos, 2) < -self.pos_threshold \
or linalg.norm(load_pos, 2) > self.pos_threshold \
or linalg.norm(vel, 2) < -self.vel_threshold \
or linalg.norm(vel, 2) > self.vel_threshold
done = bool(done)
if not done:
reward = (-linalg.norm(load_pos, 2))
elif self.steps_beyond_done is None:
# Pole just fell!
self.steps_beyond_done = 0
reward = 1.0
else:
if self.steps_beyond_done == 0:
logger.warn(
"You are calling 'step()' even though this environment has already returned done = True. You should always call 'reset()' once you receive 'done = True' -- any further steps are undefined behavior."
)
self.steps_beyond_done += 1
reward = 0.0
return np.array(self.state), reward, done, {}
def quadDynamics(x, Theta, rpm_now):
pos = x[0:3]
vel = x[3:6]
omega = x[10:13]
quat = x[6:10]
quat = Quaternion(quat)
quat = quat.normalised
try:
quat_inv = quat.inverse
except ZeroDivisionError:
quat_inv = Quaternion([1, 0, 0, 0])
quat = Quaternion([1, 0, 0, 0])
print('Warning: Quaternion could not be inverted')
theta = Theta['value']
m = theta['m']
Ixx = theta['Ixx']
Iyy = theta['Iyy']
Izz = theta['Izz']
Cq = theta['Cq']
Cdh = theta['Cdh']
Cdxy = theta['Cdxy']
Cdy = theta['Cdy']
Cdz = theta['Cdz']
deltaDx = theta['deltaDx']
deltaDy = theta['deltaDy']
Dx = theta['Dx']
Dy = theta['Dy']
# Get the thrust produced by each propeller in this state
thrusts, flagErr = getThrust(x, Theta, rpm_now)
# thrusts = thrusts + np.random.normal(0, 0.4, 4).reshape(np.shape(thrusts))
# The derivative of position is velocity
pos_dot = vel
# Get the rate of change of the quaterion
quat_dot = quat.derivative(omega)
quat_dot = np.array(quat_dot.elements)
# Thrust in the inertial frameor
thrust_BF = np.array([0, 0, -sum(thrusts)])
thrust_IF = quat.rotate(thrust_BF)
# Parasitic drag in the inertial frame
vel_BF = quat_inv.rotate(vel)
airframeDrag_BF = -1 * np.array([Cdxy, Cdxy, Cdz]) * vel_BF * abs(vel_BF)
airframeDrag_IF = quat.rotate(airframeDrag_BF)
# Blade drag in the inertial frame
bladeDrag_BF = -1 * abs(sum(thrusts)) * np.array(
[Cdh * vel_BF[0], Cdh * vel_BF[1], 0])
bladeDrag_IF = quat.rotate(bladeDrag_BF)
# Gravity in inertial frame
weight_IF = np.array([0, 0, 9.81 * m])
# Acceleration in inertial frame
vel_dot = (thrust_IF + weight_IF + airframeDrag_IF + bladeDrag_IF) / m
# Torque
torque = np.array(
[[
-thrusts[0] * (Dy - deltaDy) + thrusts[1] * (Dy + deltaDy) +
thrusts[2] * (Dy + deltaDy) - thrusts[3] * (Dy - deltaDy)
],
[
thrusts[0] * (Dx - deltaDx) - thrusts[1] * (Dx + deltaDx) +
thrusts[2] * (Dx - deltaDx) - thrusts[3] * (Dx + deltaDx)
],
[
Cq * rpm_now[0]**2 + Cq * rpm_now[1]**2 - Cq * rpm_now[2]**2 -
Cq * rpm_now[3]**2
]])
torque = np.squeeze(torque)
# Calc the angular accel
inertia_mat = np.diag([Ixx, Iyy, Izz])
omega_dot = np.dot(np.linalg.inv(inertia_mat),
(torque.reshape(3, 1) -
np.cross(omega, np.dot(inertia_mat, omega), axis=0)))
omega_dot = np.squeeze(omega_dot)
# Put in column vector
x_dot = np.vstack((pos_dot.reshape(3, 1), vel_dot.reshape(3, 1),
quat_dot.reshape(4, 1), omega_dot.reshape(3, 1)))
return x_dot, flagErr
