from run import run
from systems.cartpole import CartPole
from run_cartpole import CartpoleParam
from planning.rrt import rrt
from planning.scp import scp
import numpy as np
import sys, os
import matplotlib.pyplot as plt
from sklearn.cluster import KMeans
import networkx as nx
if __name__ == '__main__':
param = CartpoleParam()
param.env_case = 'Any90' #'SmallAngle','Swing90','Swing180', 'Any90'
env = CartPole(param)
G = nx.DiGraph()
# 0, 0, 0, 0
# 0.3, -0.75, 1, -4
x0 = np.array([0, 0, 0, 0])
states = []
states.append(x0)
idx = 0
G.add_node(0)
while True:
if idx >= len(states) or idx >= 100:
break
x0 = states[idx]
return sum(-K@(x - ref)) # sum ensures we return a scalar, not an array
else:
E_d = m_p*g*l
E_tilde = E - E_d
k_E, k_p, k_d = 2, 5, 1
return k_E*x[3]*c*E_tilde - k_p*x[0] - k_d*x[1]
"""
Setup cartpole environment
"""
T = 200 # number of timesteps to simulate
# Define initial conditions
x_0 = [0, 0, 0*np.pi, 0]
# Add some random perturbations
x_0[0] += np.random.uniform(low=-2.5, high=2.5) # initial x-axis location
x_0[2] += np.random.uniform(low=-np.pi/4, high=np.pi/4) # initial pole angle
S = CartPole(u, T, x_0=x_0)
X = S.simulate()
"""
Plot
"""
try:
# Results from ODESolve
goal = ref[0]
x = X[0]
theta = X[2]
animate(goal, x, theta, T, S.m_c, S.l)
except KeyboardInterrupt:
exit()
best_dist = None
best_file = None
best_x0 = None
for file in glob.glob(path):
data = np.loadtxt(file, delimiter=',', ndmin=2, max_rows=1)
dist = np.linalg.norm(x0 - data[0, 0:x0.shape[0]])
if best_dist is None or dist < best_dist:
best_dist = dist
best_file = file
best_x0 = data[0, 0:x0.shape[0]]
return best_file, best_x0
if __name__ == '__main__':
param = CartpoleParam()
env = CartPole(param)
# x0 = np.array([0.4, np.pi/2, 0.5, 0])
# x0 = np.array([0.07438156, 0.33501733, 0.50978889, 0.52446423])
# x0 = np.array([0,np.radians(180),0,0])
x0 = env.reset()
# scp_file = find_best_file(param.il_load_dataset, x0)
# print(scp_file)
if param.il_load_dataset_on:
scp_file, scp_x0 = find_best_file(param.il_load_dataset, x0)
print(scp_file, scp_x0)
controllers = {
from run import run
from systems.cartpole import CartPole
from run_cartpole import CartpoleParam
from planning.rrt import rrt
from planning.scp import scp
import numpy as np
import sys, os
if __name__ == '__main__':
param = CartpoleParam()
param.env_case = 'Any90' #'SmallAngle','Swing90','Swing180', 'Any90'
env = CartPole(param)
for i in range(1000):
k = 0
while True:
param.scp_fn = '../models/CartPole/dataset/scp_{}.csv'.format(k)
param.scp_pdf_fn = '../models/CartPole/dataset/scp_{}.pdf'.format(
k)
if not os.path.isfile(param.scp_fn):
break
k += 1
print("Running on ", param.scp_fn)
rrt(param, env)
# # fake rrt
# x0 = env.reset()
# xf = param.ref_trajectory[:,-1]
from systems.cartpole import CartPole
from systems.linearization import SystemLinearizer
from controllers import LQR
import numpy as np
import matplotlib.pyplot as plt
# Plant
plant = CartPole(m_c=1,
m_p=1,
l=1,
gravity=10,
x0=np.array([-1, np.pi / 6, 0, 0]),
underactuated=True)
# Energy shaping controller
m_c, m_p, l, g = plant.m_c, plant.m_p, plant.l, plant.gravity
k_e, k_p, k_d = 5, 10, 10 # gains after experimentation
def energy_shaping_controller(t, x):
d, theta, d_dot, theta_dot = x
T = (1 / 2) * m_p * l**2 * theta_dot**2
U = -m_p * g * l * np.cos(theta)
E = T + U
E_d = m_p * g * l
u_energy_shaping = k_e * theta_dot * np.cos(theta) * (E - E_d)
# Add extra terms to bring the cart in zero position
from systems.cartpole import CartPole
from systems.linearization import SystemLinearizer
from controllers import LQR
import numpy as np
import matplotlib.pyplot as plt
# Plant
plant = CartPole(m_c=1,
m_p=1,
l=1,
gravity=10,
x0=np.array([-2, np.pi * 3 / 4, 0, 0]),
underactuated=True)
# Linearize around fixed point (vertical position, zero velocity)
x_goal = np.array([0, np.pi, 0, 0])
m_p, l, g = plant.m_p, plant.l, plant.gravity
tau_g_dq = np.array([[0, 0], [0, m_p * l * g]])
sl = SystemLinearizer(plant, x0=x_goal, tau_g_dq=tau_g_dq)
# LQR controller
lqr = LQR(A=sl.A_lin, B=sl.B_lin, Q=10 * np.eye(4), R=np.eye(1))
plant.u = lambda t, x: lqr.controller(t, x - x_goal)
# Animate
fig, ax = plt.subplots(figsize=(5, 5))
plant.playback(fig=fig, ax=ax, T=10, save=True)
from systems.cartpole import CartPole
from controllers import FLC
import numpy as np
import matplotlib.pyplot as plt
# Plant
plant = CartPole(m_c=1,
m_p=1,
l=1,
gravity=10,
x0=np.array([-1, 0, 0, 0]),
underactuated=False)
# Feedback Linearization Controller
# Make system feedback-equivalent to a linear system
# controlled with a simple PD controller
plant.u = FLC(
plant, lambda t, x: np.array([[-(x[0] - 0) - x[2]],
[-(x[1] - np.pi) - x[3]]])).controller
# Animate
fig, ax = plt.subplots(figsize=(5, 5))
plant.playback(fig=fig, ax=ax, T=10, save=True)
reward += r
actions[step] = np.squeeze(action)
if done:
break
if param.sim_render_on: # and step%20==0:
env.render()
print('reward: ', reward)
env.close()
return states, actions, step
if __name__ == '__main__':
param = CartpoleParam()
param.env_case = 'Any90' #'SmallAngle','Swing90','Swing180', 'Any90'
env = CartPole(param)
# param = QuadrotorParam()
# env = Quadrotor(param)
# param.sim_rl_model_fn = '../models/quadrotor/rl_discrete.pt'
deeprl_controller = torch.load(param.sim_rl_model_fn)
times = param.sim_times
xf = param.ref_trajectory[:, -1]
for i in range(500):
k = 0
while True:
param.scp_fn = '../models/CartPole/dataset_rl/scp_{}.csv'.format(k)