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