Cart Pole using Lyapunov and LQR control, OpenAI gym

We’re having a lot of trouble hacking together a reinforcement learning version of this, so we are taking an alternative approacg, inspired by wtaching the MIT underactuated robotics course.

http://underactuated.csail.mit.edu/underactuated.html?chapter=acrobot

It took some pen and paper to get the equations of motion (which are maybe right?).

openai gym has

We switch over to LQR when the y position of the pole is above a certain height

https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator

This scipy function solves the algebriac ricatti equation in the ocntinous time infite horizon section

https://docs.scipy.org/doc/scipy-0.14.0/reference/generated/scipy.linalg.solve_continuous_are.html

Things that helped: Trying to balance pole first from upright position then from downright.

Tuning weights for theta and thetadot. Thetadot was too small made it unstable

Hacked in the LQR control by adjusting force_mag variable. Nasty.

 

Put it some slight compensation for a delayed observation, which reflects our actual sensor system

 

from custom_cartpole_delay import CartPoleEnv

import gym

import numpy as np
import scipy.linalg as linalg
lqr = linalg.solve_continuous_are

env = CartPoleEnv()
env.buffer_size = 5
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)
	return 1.0 * (E(x)-Ed) * x[3] * np.cos(x[2])


H = np.array([
	[1,0,0,0],
	[0,total_mass, 0, - polemass_length],
	[0,0,1,0],
	[0,- polemass_length, 0, (2 * length)**2 * masspole /3]
	])

Hinv = np.linalg.inv(H)

A = Hinv @ np.array([
    [0,1,0,0],
    [0,0,0,0],
    [0,0,0,1],
    [0,0, - polemass_length * gravity, 0]
	])
B = Hinv @ np.array([0,1.0,0,0]).reshape((4,1))
Q = np.diag([0.1, 1.0, 100.0, 5.0])
R = np.array([[0.1]])

P = lqr(A,B,Q,R)
Rinv = np.linalg.inv(R)
K = Rinv @ B.T @ P
print(K)
def ulqr(x):
	x1 = np.copy(x)
	x1[2] = np.sin(x1[2])
	return np.dot(K, x1)

Ed = E([0,0,0,0])

observation = env.reset()
for _ in range(1000):
  observation = np.copy(observation)
  env.render()
  observation[0] += observation[1] * tau * env.buffer_size
  observation[2] += observation[3] * tau * env.buffer_size
  observation[3] += np.sin(observation[2]) * tau * env.buffer_size * gravity  / (length * 2) / 2
  #action = env.action_space.sample() # your agent here (this takes random actions)
  a = u(observation) - 0.3 * observation[0] -  0.1 * observation[1]
  if  abs(E(observation)-Ed) < 0.1 and np.cos(observation[2]) > 0.6: #abs(E(observation)-Ed) < 0.1 and
    
  	a = ulqr(observation)
  	env.force_mag = min(abs(a[0]), 10)
  	#print(a)
  else:
  	env.force_mag = 10.0
  if a < 0:
  	action = 0
  else:
  	action = 1



  observation, reward, done, info = env.step(action)


import math
import gym
from gym import spaces
from gym.utils import seeding
import numpy as np
import logging
logger = logging.getLogger(__name__)
 
class CartPoleEnv(gym.Env):
    metadata = {
        'render.modes': ['human', 'rgb_array'],
        'video.frames_per_second' : 50
    }
 
    def __init__(self):
        self.gravity = 9.8
        self.masscart = 1.0
        self.masspole = 0.1
        self.total_mass = (self.masspole + self.masscart)
        self.length = 0.5 # actually half the pole's length
        self.polemass_length = (self.masspole * self.length)
        self.force_mag = 3.0
        self.tau = 0.02  # seconds between state updates
 
        # Angle at which to fail the episode
        # we expect full swings
        self.theta_threshold_radians =  np.pi  #12 * 2 * math.pi / 360
        self.x_threshold = 2.4
 
        # Angle limit set to 2 * theta_threshold_radians so failing observation is still within bounds
        high = np.array([
            self.x_threshold * 2,
            np.finfo(np.float32).max,
            self.theta_threshold_radians * 2,
            np.finfo(np.float32).max])
        high2 = np.array([1])
        self.action_space = spaces.Discrete(2) #spaces.Box(-high2, high2)# 
        self.observation_space = spaces.Box(-high, high)
 
        self._seed()
        self.viewer = None
        self.state = None
        self.buffer_size = 0

 
        self.steps_beyond_done = None
        self.steps = 0
        self.num_envs = 1
        self.viewer = None
    viewer = None
    def _seed(self, seed=None):
        self.np_random, seed = seeding.np_random(seed)
        return [seed]
 
    def _step(self, action):
        #assert self.action_space.contains(action), "%r (%s) invalid"%(action, type(action))
        #print(action)
        #action =action[0] # max(-1, min(action[0],1))
        state = self.state
        x, x_dot, theta, theta_dot = state
        force = self.force_mag if action==1 else -self.force_mag
        #force = self.force_mag * action
        #print(action)
        #print(state)
 
        x  = x + self.tau * x_dot
        theta = theta + self.tau * theta_dot
        costheta = math.cos(theta)
        sintheta = math.sin(theta)
        temp = (force + self.polemass_length * theta_dot * theta_dot * sintheta) / self.total_mass
        thetaacc = (self.gravity * sintheta - costheta* temp) / (self.length * (4.0/3.0 - self.masspole * costheta * costheta / self.total_mass))
        xacc  = temp - self.polemass_length * thetaacc * costheta / self.total_mass
        
        x_dot = x_dot + self.tau * xacc
        
        theta_dot = theta_dot + self.tau * thetaacc
        self.state = (x,x_dot,theta,theta_dot)
        
        done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -np.pi * 5 \
                or theta > np.pi * 5 \
                or self.steps > 1024
        done = bool(done)
        
        self.steps += 1
        limit = 200
 
        reward = 0.0
        if  x < -self.x_threshold or x > self.x_threshold:
            reward -= 1.0
        reward += (np.cos(theta)+1)**2 / 4
        #reward -= 0.1 * action**2 
        reward += -0.1 * x**2
        if np.cos(theta) > 0.95:
            reward += 1
        #reward = reward/2048
        '''
        if not done:
            reward = 1.0
        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.warning("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
        '''
        #print(np.array(self.state).reshape((1,-1)), reward, done, {})

        #obs = np.array([x,x_dot,np.cos(theta),np.sin(theta),theta_dot])# + self.action_buffer)
        self.buffer.append(np.copy(self.state))
        obs2 = self.buffer.pop(0)
        return obs2, reward, done, {}
 
    def _reset(self):
        #self.state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
        #x, xdot, theta, thetadot
        self.state = np.array([0, 0, np.pi+0.2 , 0]) # np.pi+0.2 + self.np_random.uniform(low=-1.0, high=1.0, size=(4,))
        self.steps_beyond_done = None
        self.steps = 0
        self.buffer = []
        for i in range(self.buffer_size):
            self.buffer.append(self.state)
        return np.array(self.state)
 
    def _render(self, mode='human', close=False):
        if close:
            if self.viewer is not None:
                self.viewer.close()
                self.viewer = None
            return
 
        screen_width = 600
        screen_height = 400
 
        world_width = self.x_threshold*2
        scale = screen_width/world_width
        carty = 100 # TOP OF CART
        polewidth = 10.0
        polelen = scale * 1.0
        cartwidth = 50.0
        cartheight = 30.0
 
        if self.viewer is None:
            from gym.envs.classic_control import rendering
            self.viewer = rendering.Viewer(screen_width, screen_height)
            l,r,t,b = -cartwidth/2, cartwidth/2, cartheight/2, -cartheight/2
            axleoffset =cartheight/4.0
            cart = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)])
            self.carttrans = rendering.Transform()
            cart.add_attr(self.carttrans)
            self.viewer.add_geom(cart)
            l,r,t,b = -polewidth/2,polewidth/2,polelen-polewidth/2,-polewidth/2
            pole = rendering.FilledPolygon([(l,b), (l,t), (r,t), (r,b)])
            pole.set_color(.8,.6,.4)
            self.poletrans = rendering.Transform(translation=(0, axleoffset))
            pole.add_attr(self.poletrans)
            pole.add_attr(self.carttrans)
            self.viewer.add_geom(pole)
            self.axle = rendering.make_circle(polewidth/2)
            self.axle.add_attr(self.poletrans)
            self.axle.add_attr(self.carttrans)
            self.axle.set_color(.5,.5,.8)
            self.viewer.add_geom(self.axle)
            self.track = rendering.Line((0,carty), (screen_width,carty))
            self.track.set_color(0,0,0)
            self.viewer.add_geom(self.track)
 
        if self.state is None: return None
 
        x = self.state
        cartx = x[0]*scale+screen_width/2.0 # MIDDLE OF CART
        self.carttrans.set_translation(cartx, carty)
        self.poletrans.set_rotation(-x[2])
 
        return self.viewer.render(return_rgb_array = mode=='rgb_array')

 

 

 

 

Leave a Reply

Your email address will not be published. Required fields are marked *