def main():
pidcontrollers = (PositionHoldPidController(), PositionHoldPidController(),
DescentPidController())
demo3d('gym_copter:Lander3D-v0', heuristic, pidcontrollers,
ThreeDLanderRenderer)
def main():
pidcontrollers = (AngularVelocityPidController(),
AngularVelocityPidController(),
AngularVelocityPidController(),
PositionHoldPidController(), PositionHoldPidController(),
AltitudeHoldPidController())
demo3d('gym_copter:Hover3D-v0', heuristic, pidcontrollers,
ThreeDHoverRenderer)
def __init__(self, obs_size=10):
_Lander.__init__(self, obs_size, 4)
# Pre-convert max-angle degrees to radians
self.max_angle = np.radians(self.MAX_ANGLE)
# For generating CSV file
self.STATE_NAMES = ['X', 'dX', 'Y', 'dY', 'Z', 'dZ',
'Phi', 'dPhi', 'Theta', 'dTheta']
# Add PID controllers for heuristic demo
self.phi_rate_pid = AngularVelocityPidController()
self.theta_rate_pid = AngularVelocityPidController()
self.x_poshold_pid = PositionHoldPidController()
self.y_poshold_pid = PositionHoldPidController()
def demo_heuristic(env, seed=None, csvfilename=None):
env.seed(seed)
np.random.seed(seed)
rate_pid = AngularVelocityPidController()
pos_pid = PositionHoldPidController()
alt_pid = AltitudeHoldPidController()
total_reward = 0
steps = 0
state = env.reset()
dt = 1. / env.FRAMES_PER_SECOND
actsize = env.action_space.shape[0]
csvfile = None
if csvfilename is not None:
csvfile = open(csvfilename, 'w')
csvfile.write('t,' + ','.join([('m%d' % k)
for k in range(1, actsize + 1)]))
csvfile.write(',' + ','.join(env.STATE_NAMES) + '\n')
while True:
action = heuristic(state, rate_pid, pos_pid, alt_pid)
state, reward, done, _ = env.step(action)
total_reward += reward
if csvfile is not None:
csvfile.write('%f' % (dt * steps))
csvfile.write((',%f' * actsize) % tuple(action))
csvfile.write(((',%f' * len(state)) + '\n') % tuple(state))
steps += 1
if (steps % 20 == 0) or done:
print('time = %3.2f steps = %04d total_reward = %+0.2f' %
(time() - env.start, steps, total_reward))
if done:
break
sleep(1)
env.close()
if csvfile is not None:
csvfile.close()
return total_reward
def main():
phi_rate_pid = AngularVelocityPidController()
theta_rate_pid = AngularVelocityPidController()
x_poshold_pid = PositionHoldPidController()
y_poshold_pid = PositionHoldPidController()
descent_pid = DescentPidController()
env = gym.make('gym_copter:Lander3D-v0')
env = wrappers.Monitor(env, 'movie/', force=True)
state = env.reset()
x, dx, y, dy, z, dz, phi, dphi, theta, dtheta = state
while True:
phi_todo = 0
theta_todo = 0
phi_rate_todo = phi_rate_pid.getDemand(dphi)
y_pos_todo = x_poshold_pid.getDemand(y, dy)
phi_todo = phi_rate_todo + y_pos_todo
theta_rate_todo = theta_rate_pid.getDemand(-dtheta)
x_pos_todo = y_poshold_pid.getDemand(x, dx)
theta_todo = theta_rate_todo + x_pos_todo
descent_todo = descent_pid.getDemand(z, dz)
t, r, p = (descent_todo + 1) / 2, phi_todo, theta_todo
# Use mixer to set motors
action = t - r - p, t + r + p, t + r - p, t - r + p
state, _, done, _ = env.step(action)
if done:
break
x, dx, y, dy, z, dz, phi, dphi, theta, dtheta = state
env.close()
def main():
demo('gym_copter:Lander2D-v0', heuristic,
(PositionHoldPidController(), DescentPidController()))
class Hover3D(_Hover):
def __init__(self, obs_size=12):
_Hover.__init__(self, obs_size, 4)
# Pre-convert max-angle degrees to radians
self.max_angle = radians(self.MAX_ANGLE)
# For generating CSV file
self.STATE_NAMES = [
'X', 'dX', 'Y', 'dY', 'Z', 'dZ', 'Phi', 'dPhi', 'Theta', 'dTheta',
'Psi', 'dPsi'
]
# Add PID controllers for heuristic demo
self.roll_rate_pid = AngularVelocityPidController()
self.pitch_rate_pid = AngularVelocityPidController()
self.yaw_rate_pid = AngularVelocityPidController()
self.x_poshold_pid = PositionHoldPidController()
self.y_poshold_pid = PositionHoldPidController()
def reset(self):
return _Hover._reset(self)
def render(self, mode='human'):
'''
Returns None because we run viewer on a separate thread
'''
return None
def demo_pose(self, args):
x, y, z, phi, theta, viewer = args
while viewer.is_open():
self._reset(pose=(x, y, z, phi, theta), perturb=False)
self.render()
sleep(.01)
self.close()
def heuristic(self, state, nopid):
'''
PID controller
'''
x, dx, y, dy, z, dz, phi, dphi, theta, dtheta, _, dpsi = state
roll_todo = 0
pitch_todo = 0
yaw_todo = 0
if not nopid:
roll_rate_todo = self.roll_rate_pid.getDemand(dphi)
y_pos_todo = self.x_poshold_pid.getDemand(y, dy)
pitch_rate_todo = self.pitch_rate_pid.getDemand(-dtheta)
x_pos_todo = self.y_poshold_pid.getDemand(x, dx)
roll_todo = roll_rate_todo + y_pos_todo
pitch_todo = pitch_rate_todo + x_pos_todo
yaw_todo = self.yaw_rate_pid.getDemand(-dpsi)
hover_todo = self.altpid.getDemand(z, dz)
t, r, p, y = (hover_todo + 1) / 2, roll_todo, pitch_todo, yaw_todo
# Use mixer to set motors
return [t - r - p - y, t + r + p - y, t + r - p + y, t - r + p + y]
def _get_motors(self, motors):
return motors
def _get_state(self, state):
return state
class Lander3D(_Lander):
def __init__(self, obs_size=10):
_Lander.__init__(self, obs_size, 4)
# Pre-convert max-angle degrees to radians
self.max_angle = np.radians(self.MAX_ANGLE)
# For generating CSV file
self.STATE_NAMES = ['X', 'dX', 'Y', 'dY', 'Z', 'dZ',
'Phi', 'dPhi', 'Theta', 'dTheta']
# Add PID controllers for heuristic demo
self.phi_rate_pid = AngularVelocityPidController()
self.theta_rate_pid = AngularVelocityPidController()
self.x_poshold_pid = PositionHoldPidController()
self.y_poshold_pid = PositionHoldPidController()
def reset(self):
return _Lander._reset(self)
def render(self, mode='human'):
'''
Returns None because we run viewer on a separate thread
'''
return None
def demo_pose(self, args):
x, y, z, phi, theta, viewer = args
while viewer.is_open():
self._reset(pose=(x, y, z, phi, theta), perturb=False)
self.render()
sleep(.01)
self.close()
def heuristic(self, state, nopid):
'''
PID controller
'''
x, dx, y, dy, z, dz, phi, dphi, theta, dtheta = state
phi_todo = 0
theta_todo = 0
if not nopid:
phi_rate_todo = self.phi_rate_pid.getDemand(dphi)
y_pos_todo = self.x_poshold_pid.getDemand(y, dy)
phi_todo = phi_rate_todo + y_pos_todo
theta_rate_todo = self.theta_rate_pid.getDemand(-dtheta)
x_pos_todo = self.y_poshold_pid.getDemand(x, dx)
theta_todo = theta_rate_todo + x_pos_todo
descent_todo = self.descent_pid.getDemand(z, dz)
t, r, p = (descent_todo+1)/2, phi_todo, theta_todo
return [t-r-p, t+r+p, t+r-p, t-r+p] # use mixer to set motors
def _get_motors(self, motors):
return motors
def _get_state(self, state):
return state[:10]
MIT License
'''
from pidcontrollers import AltitudeHoldPidController
from pidcontrollers import AngularVelocityPidController
from pidcontrollers import PositionHoldPidController
from main import demo
def heuristic(state, pidcontrollers):
y, dy, z, dz, phi, dphi = state
rate_pid, pos_pid, alt_pid = pidcontrollers
rate_todo = rate_pid.getDemand(dphi)
pos_todo = pos_pid.getDemand(y, dy)
phi_todo = rate_todo + pos_todo
hover_todo = alt_pid.getDemand(z, dz)
return hover_todo-phi_todo, hover_todo+phi_todo
demo('gym_copter:Hover2D-v0', heuristic,
(AngularVelocityPidController(),
PositionHoldPidController(),
AltitudeHoldPidController()))