def __init__(self, cartport="/dev/ttyACM0", imageport=1):
self.analyzer = ImageAnalyzer(imageport)
self.cart = CartCommand(port=cartport)
self.action_space = spaces.Discrete(2)
self.observation_space = spaces.Box(
np.array([0., -50., 0., -50., -1., -50.]),
np.array([1., 50., 1., 50., 1., 50.]))
self.last_state = None
self.state = self._getState()
self.last_state = self._getState()
avg = np.array([0.5, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
var = np.array([
0.5, 1. / 50., 2., 1. / 5., 2.0, 1. / 5., 2048., 2048., 2048., 2048., 2048.
])
def learn(n):
cem.fit(env, nb_steps=n)
#####################
# testing functions #
#####################
analyzer = ImageAnalyzer(1)
cart = CartCommand(port="/dev/ttyACM0")
memory = Memory()
def test(n, random_action=False, eps=1.0):
global states, actions, next_states, command_queue
command_queue = Queue.Queue()
cart.toggleEnable()
current_states = []
current_actions = []
current_next_states = []
import cv2 import numpy as np import pickle from sabretooth_command import CartCommand from image_analyzer_pseye import ImageAnalyzer from time import time import Queue from keras.models import load_model import sys old_data = [] data = [] analyzer = ImageAnalyzer() cart = CartCommand(port="/dev/ttyACM0") commandqueue = Queue.Queue() for i in range(5): commandqueue.put(0) def reset(): x = 0 cart.enabled = True while not 0.4 < x < 0.6: x, dx, theta, dtheta = analyzer.analyzeFrame() command = 1000 * np.sign(x-0.5) command = min(max(command,-2046), 2046)
import cv2
from sabretooth_command import CartCommand
from image_analyzer_pseye import ImageAnalyzer
from time import time, sleep
import numpy as np
import scipy.linalg as linalg
analyzer = ImageAnalyzer(1)
cart = CartCommand(port="/dev/ttyACM0")
lqr = linalg.solve_continuous_are
gravity = 9.8
masscart = 1.0
masspole = 0.1
total_mass = (masspole + masscart)
length = 0.5 # actually half the pole's length
polemass_length = (masspole * length)
force_mag = 10.0
tau = 0.02
def E(x):
return 1 / 2 * masspole * (2 * length)**2 / 3 * x[3]**2 + np.cos(
x[2]) * polemass_length * gravity
def u(x):
#print(E(x)-Ed)
class CartPoleEnv(gym.Env):
def __init__(self, cartport="/dev/ttyACM0", imageport=1):
self.analyzer = ImageAnalyzer(imageport)
self.cart = CartCommand(port=cartport)
self.action_space = spaces.Discrete(2)
self.observation_space = spaces.Box(
np.array([0., -50., 0., -50., -1., -50.]),
np.array([1., 50., 1., 50., 1., 50.]))
self.last_state = None
self.state = self._getState()
self.last_state = self._getState()
def _step(self, action):
if action == self.action_space[0]:
d_command = 1.
else:
d_command = -1.
command += commandStep * d_command
command = min(max(command, -2046), 2046)
if x < 0.35:
command = min(command, -500)
if x > 0.65:
command = max(command, 500)
self.cart.setSpeed(command)
self.last_state = self.state
self.state = self._getState()
reward = self._getReward(self.state)
done = False
return np.array(self.state), reward, done, {}
def _reset(self):
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
self.cart.enabled = True
while not 0.4 < x < 0.6:
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
command = 1000 * np.sign(x - 0.5)
command = min(max(command, -2046), 2046)
self.cart.setSpeed(command)
cv2.waitKey(1)
self.cart.setSpeed(0)
sleep(0.3)
self.cart.enabled = False
def _getData(self):
x, dx, theta, dtheta = self.analyzer.analyzeFrame()
xpole = np.cos(theta)
ypole = np.sin(theta)
return x, xpole, ypole
def _getState(self):
x, xpole, ypole = self._getData()
if not self.last_state is None:
state = [
x, x - self.last_state[0], xpole, xpole - self.last_state[2],
ypole, ypole - self.last_state[4]
]
else:
state = [x, 0, xpole, 0, ypole, 0]
return state
def _getReward(self, state):
rewards_pole = 0.0 * (state[:, 4] + 0.5)**2 #ypole hieght
rewards_cart = -2.0 * np.power(state[:, 0], 2) #xcart pos
return rewards_cart + rewards_pole
def _render(self, mode='human', close=False):
pass
import cv2
import numpy as np
import pickle
from sabretooth_command import CartCommand
from image_analyzer_pseye import ImageAnalyzer
from time import time
import Queue
analyzer = ImageAnalyzer()
cart = CartCommand()
itheta = 0
start = 0
def nothing(x):
pass
cv2.namedWindow('PID', cv2.WINDOW_NORMAL)
cv2.resizeWindow('PID', 600,200)
cv2.createTrackbar('P','PID',0,200000,nothing)
cv2.setTrackbarPos('P', 'PID', 100000)
cv2.createTrackbar('I','PID',0,16000,nothing)
cv2.setTrackbarPos('I', 'PID', 8000)
cv2.createTrackbar('D','PID',0,200000,nothing)