def attitude_limit(self):
"""
limit attitude
:return:
"""
att_overlimit = False
euler = quat2euler(self.state[6:10])
r = euler[0]
p = euler[1]
y = euler[2]
if np.abs(r) >= deg2rad(85):
attitude_limited = euler2quat(
np.array([np.sign(r) * deg2rad(85), p, y]))
att_overlimit = True
if np.abs(p) >= deg2rad(85):
attitude_limited = euler2quat(
np.array([r, np.sign(p) * deg2rad(85), y]))
att_overlimit = True
if np.abs(y) >= deg2rad(175):
attitude_limited = euler2quat(
np.array([r, p, np.sign(y) * deg2rad(175)]))
att_overlimit = True
if np.abs(r) <= deg2rad(85) and np.abs(p) <= deg2rad(85) and np.abs(
y) <= deg2rad(175):
attitude_limited = self.state[6:10]
att_overlimit = False
return att_overlimit, attitude_limited
def hover_controller(self, state_des, state_now):
acc_des = np.zeros(3)
e_pos = state_des[0:3] - state_now[0:3]
e_vel = state_des[3:6] - state_now[3:6]
acc_des[0] = self.kp_x * e_pos[0] + self.kd_x * e_vel[0]
acc_des[1] = self.kp_y * e_pos[1] + self.kd_y * e_vel[1]
acc_des[2] = self.kp_z * e_pos[2] + self.kd_z * e_vel[2]
F = self.mass * self.g + self.mass * acc_des[2]
# att_des =rot2euler(quat2rot(state_des[6:10]))
att_des = quat2euler(state_des[6:10])
psi_des = att_des[2]
phi_des = (acc_des[0] * np.sin(psi_des) -
acc_des[1] * np.cos(psi_des)) / self.g
theta_des = (acc_des[0] * np.cos(psi_des) +
acc_des[1] * np.sin(psi_des)) / self.g
roll_rate_des = 0
pitch_rate_des = 0
att_des[0] = phi_des
att_des[1] = theta_des
att_des[2] = psi_des
state_des[6:10] = euler2quat(att_des)
state_des[10] = roll_rate_des
state_des[11] = pitch_rate_des
return F, state_des
def __init__(self):
self.drone = Drone()
self.state = np.zeros(13)
self.steps_beyond_done = None
self.pos_threshold = 1
self.vel_threshold = 0.1
ini_pos = np.array([0.0, 0.0, 5.0]) + np.random.uniform(-1, 1, (3, ))
ini_att = euler2quat(
np.array([deg2rad(0), deg2rad(0), 0]) +
np.random.uniform(-0.2, 0.2, (3, )))
ini_angular_rate = np.array([0, deg2rad(0), 0])
self.ini_state = np.zeros(13)
self.ini_state[0:3] = ini_pos
self.ini_state[6:10] = ini_att
self.ini_state[10:] = ini_angular_rate
pos_des = np.array([0.0, 0.0, 5.0]) # [x, y, z]
att_des = euler2quat(np.array([deg2rad(0), deg2rad(0), deg2rad(0)]))
self.state_des = np.zeros(13)
self.state_des[0:3] = pos_des
self.state_des[6:10] = att_des
low = self.drone.state_lim_low
high = self.drone.state_lim_high
self.action_space = spaces.Box(low=np.array([0.0, 0.0, 0.0, 0.0]),
high=np.array([1.0, 1.0, 1.0, 1.0]))
self.observation_space = spaces.Box(low=low, high=high)
self.action_max = np.array([1.0, 1.0, 1.0, 1.0
]) * self.drone.mass * self.drone.gravity
self.seed()
def vel_controller(self, state_des, state_now, state_last):
acc_des = np.zeros(3)
# e_pos = state_des[0:3] - state_now[0:3]
e_vel = state_des[3:6] - state_now[3:6]
e_dv = state_now[3:6] - state_last[3:6]
acc_des[0] = self.kp_vx * e_vel[0] + self.kd_vx * e_dv[0]
acc_des[1] = self.kp_vy * e_vel[1] + self.kd_vy * e_dv[1]
acc_des[2] = self.kp_vz * e_vel[2] + self.kd_vz * e_dv[2]
F = self.mass * self.g + self.mass * acc_des[2]
# att_des =rot2euler(quat2rot(state_des[6:10]))
att_des = quat2euler(state_des[6:10])
psi_des = att_des[2]
phi_des = (acc_des[0] * np.sin(psi_des) -
acc_des[1] * np.cos(psi_des)) / self.g
theta_des = (acc_des[0] * np.cos(psi_des) +
acc_des[1] * np.sin(psi_des)) / self.g
roll_rate_des = 0
pitch_rate_des = 0
att_des[0] = phi_des
att_des[1] = theta_des
att_des[2] = psi_des
state_des[6:10] = euler2quat(att_des)
state_des[10] = roll_rate_des
state_des[11] = pitch_rate_des
M = self.attitude_controller(state_des, state_now)
output = np.zeros(4)
output[0] = F
output[1:] = M
return output
def __init__(self):
self.chaser = Drone()
self.target = Drone()
self.target_controller = controller(self.target.get_arm_length(), self.target.get_mass())
self.state_chaser = np.zeros(13)
self.state_target = np.zeros(13)
self.rel_state = np.zeros(12)
self.t = 0
self.done = False
self.reward = 0.0
self.shaping = 0.0
self.last_shaping = 0.0
# self.steps_beyond_done = None
# Chaser Initial State
chaser_ini_pos = np.array([8, -50, 5]) + np.random.uniform(-0.3, 0.3, (3,))
chaser_ini_vel = np.array([0, 0, 0])# + np.random.uniform(-0.1, 0.1, (3,))
chaser_ini_att = euler2quat(np.array([0.0, 0.0, 0.0]))# + np.random.uniform(-0.2, 0.2, (3,)))
chaser_ini_angular_rate = np.array([0.0, 0.0, 0.0]) #+ np.random.uniform(-0.1, 0.1, (3,))
self.chaser_dock_port = np.array([0.1, 0.0, 0.0])
self.chaser_ini_state = np.zeros(13)
self.chaser_ini_state[0:3] = chaser_ini_pos
self.chaser_ini_state[3:6] = chaser_ini_vel
self.chaser_ini_state[6:10] = chaser_ini_att
self.chaser_ini_state[10:] = chaser_ini_angular_rate
self.state_chaser = self.chaser.reset(self.chaser_ini_state, self.chaser_dock_port)
# Target Initial State
target_ini_pos = np.array([10, -50, 5])
target_ini_vel = np.array([0.0, 0.0, 0.0])
target_ini_att = euler2quat(np.array([0.0, 0.0, 0.0]))
target_ini_angular_rate = np.array([0.0, 0.0, 0.0])
self.target_dock_port = np.array([-0.1, 0, 0])
self.target_ini_state = np.zeros(13)
self.target_ini_state[0:3] = target_ini_pos
self.target_ini_state[3:6] = target_ini_vel
self.target_ini_state[6:10] = target_ini_att
self.target_ini_state[10:] = target_ini_angular_rate
self.state_target = self.target.reset(self.target_ini_state, self.target_dock_port)
# Target Final State
target_pos_des = np.array([10, -50, 5]) # [x, y, z]
target_att_des = euler2quat(np.array([0.0, 0.0, 0.0]))
self.target_state_des = np.zeros(13)
self.target_state_des[0:3] = target_pos_des
self.target_state_des[6:10] = target_att_des
# Final Relative Error
self.rel_pos_threshold = 1
self.rel_vel_threshold = 0.1
self.rel_att_threshold = np.array([deg2rad(0), deg2rad(0), deg2rad(0)])
self.rel_att_rate_threshold = np.array([deg2rad(0), deg2rad(0), deg2rad(0)])
# chaser_dp = self.chaser.get_dock_port_state() # drone A
# target_dp = self.target.get_dock_port_state() # drone B
self.rel_state = state2rel(self.state_chaser, self.state_target, self.chaser.get_dock_port_state(),
self.target.get_dock_port_state())
# State Limitation
chaser_low = self.chaser.state_lim_low
chaser_high = self.chaser.state_lim_high
target_low = self.target.state_lim_low
target_high = self.target.state_lim_high
# obs rel info: 12x1 [rel_pos, rel_vel, rel_rpy, rel_rpy_rate]
self.obs_low = np.array(
[-np.inf, -np.inf, -np.inf, -100, -100, -100, -np.pi, -np.pi / 2, -np.pi, -10 * np.pi, -10 * np.pi,
-10 * np.pi])
self.obs_high = np.array(
[np.inf, np.inf, np.inf, 100, 100, 100, np.pi, np.pi / 2, np.pi, 10 * np.pi, 10 * np.pi, 10 * np.pi])
# rel_low = np.array([60, 0, 100, 10, 10, 10, 1, 1, 1, 1, 10 * 2 * np.pi, 10 * 2 * np.pi, 10 * 2 * np.pi])
self.action_space = spaces.Box(low=np.array([-1.0, -1.0, -1.0, -1.0]), high=np.array([1.0, 1.0, 1.0, 1.0]),
dtype=np.float32)
self.observation_space = spaces.Box(low=self.obs_low, high=self.obs_high, dtype=np.float32)
# self.action_max = np.array([1.0, 1.0, 1.0, 1.0]) * self.chaser.mass * self.chaser.gravity
self.action_mean = np.array([1.0, 1.0, 1.0, 1.0]) * self.chaser.mass * self.chaser.gravity / 2.0
self.action_std = np.array([1.0, 1.0, 1.0, 1.0]) * self.chaser.mass * self.chaser.gravity / 2.0
self.seed()
from dynamics.quadrotor import Drone from controller.PIDController import controller from utils.transform import quat2rot, rot2euler, euler2rot, rot2quat, rad2deg, deg2rad, quat2euler, euler2quat import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # print(rot2quat(euler2rot(np.array([0, 0, 0])))) ini_pos = np.array([8, -50, 5]) ini_vel = np.array([0, 0, 0]) ini_att = euler2quat(np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])) ini_angular_rate = np.array([0, 0, 0]) ini_state = np.zeros(13) ini_state[0:3] = ini_pos ini_state[3:6] = ini_vel ini_state[6:10] = ini_att ini_state[10:] = ini_angular_rate pos_des = np.array([10, -50, 5]) # [x, y, z] vel_des = np.array([0, 0, 0]) att_des = euler2quat(np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])) state_des = np.zeros(13) state_des[0:3] = pos_des state_des[3:6] = vel_des state_des[6:10] = att_des # Initial a drone and set its initial state quad1 = Drone() quad1.reset(ini_state) control = controller(quad1.get_arm_length(), quad1.get_mass())
from dynamics.quadrotor import Drone from controller.PIDController import controller from utils.transform import quat2rot, rot2euler, euler2rot, rot2quat, rad2deg, deg2rad, quat2euler, euler2quat import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from controller.JoystickController import RCInput import threading # print(rot2quat(euler2rot(np.array([0, 0, 0])))) ini_pos = np.array([0, 0, 0]) ini_att = euler2quat(np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])) ini_angular_rate = np.array([0, 0, deg2rad(0)]) ini_state = np.zeros(13) ini_state[0:3] = ini_pos ini_state[6:10] = ini_att ini_state[10:] = ini_angular_rate pos_des = np.array([0, 0, 0]) # [x, y, z] att_des = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)]) angular_rate_des = np.array([0, 0, deg2rad(0)]) state_des = np.zeros(13) state_des[0:3] = pos_des state_des[6:10] = euler2quat(att_des) state_des[10:] = angular_rate_des # Initial a drone and set its initial state quad1 = Drone() quad1.reset(ini_state) control = controller(quad1.get_arm_length(), quad1.get_mass())
import gym
from gym import wrappers, logger
import numpy as np
from controller.PIDController import controller
from utils.transform import quat2rot, rot2euler, euler2rot, rot2quat, rad2deg, deg2rad, euler2quat
att_des = euler2quat(np.array([deg2rad(0), deg2rad(0), deg2rad(0)]))
pos_des = np.array([0, 0, 1]) # [x, y, z]
state_des = np.zeros(13)
state_des[0:3] = pos_des
state_des[6:10] = att_des
control = controller(0.086, 0.18)
env = gym.make('gym_docking:hovering-v0')
obs = env.reset()
for i in range(1000):
env.seed(0)
action = control.PID(state_des, obs)
obs, reward, dones, info = env.step(action)
# print('obs: ', obs)
print('reward: ', reward)
print('dones: ', dones)