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)