def __init__(self, CLSystem , tf = 10 , n = 10001 , solver = 'ode' ): Simulation.__init__(self, CLSystem , tf, n, solver) self.sys = CLSystem.sys self.ctl = CLSystem.ctl # Cost computing self.cf = costfunction.QuadraticCostFunction( self.sys )
def __init__(self, ContinuousDynamicSystem, tf=10, n=10001, solver='ode'): self.cds = ContinuousDynamicSystem self.t0 = 0 self.tf = tf self.n = int(n) self.dt = ( tf + 0.0 - self.t0 ) / ( n - 1 ) self.x0 = np.zeros( self.cds.n ) self.solver = solver # Ploting self.fontsize = 5 self.figsize = (4,3) self.dpi = 300 # Cost computing self.cf = costfunction.QuadraticCostFunction( ContinuousDynamicSystem )
# -*- coding: utf-8 -*- """ Created on Fri Nov 16 10:17:36 2018 @author: Alexandre """ ############################################################################### import numpy as np ############################################################################### from pyro.dynamic import integrator from pyro.analysis import costfunction ################################### # Simple integrator sys = integrator.TripleIntegrator() # Simulation params sys.ubar = np.array([1]) sys.x0 = np.array([2, 0, 0]) # Include cost function computation sys.cost_function = costfunction.QuadraticCostFunction(3, 1, 1) # Compute and show results sys.plot_trajectory('xuj') sys.plot_phase_plane_trajectory(0, 1) sys.plot_phase_plane_trajectory_3d()
""" import numpy as np from pyro.dynamic import vehicle from pyro.planning import discretizer from pyro.analysis import costfunction from pyro.planning import valueiteration sys = vehicle.HolonomicMobileRobotwithObstacles() # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (51, 51), (3, 3)) # Cost Function cf = costfunction.QuadraticCostFunction(sys) cf.INF = 1E9 # VI algo vi = valueiteration.ValueIteration_2D(grid_sys, cf) vi.initialize() vi.load_data('holonomic_vi') vi.compute_steps(1) vi.plot_cost2go(40000) vi.assign_interpol_controller() vi.plot_policy(0) vi.plot_policy(1) #vi.save_data('holonomic_vi')
from pyro.planning import valueiteration from pyro.control import controller sys = vehicle.KinematicBicyleModel() sys.x_ub = np.array([+2, +2, +0.5]) sys.x_lb = np.array([-0, -0, -0.5]) sys.u_ub = np.array([+1, +1.0]) sys.u_lb = np.array([-1, -1.0]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (41, 41, 21), (3, 3), 0.05) # Cost Function cf = costfunction.QuadraticCostFunction(q=np.ones(sys.n), r=np.ones(sys.m), v=np.zeros(sys.p)) cf.xbar = np.array([1, 1, 0]) # target cf.INF = 1E4 cf.EPS = 0.2 cf.R = np.array([[0.01, 0], [0, 0]]) # VI algo """ EXPERIMENTAL TEST NOT WORKING """ vi = valueiteration.ValueIteration_ND(grid_sys, cf) vi.uselookuptable = True
############################################################################### import matplotlib.pyplot as plt from pyro.planning import discretizer, valueiteration sys = pendulum.DoublePendulum() # Discrete world grid_sys = discretizer.GridDynamicSystem( sys ) # Cost Function qcf = costfunction.QuadraticCostFunction( np.ones(sys.n), np.ones(sys.m), np.ones(sys.p) ) qcf.xbar = np.array([ -3.14 , 0 ]) # target qcf.INF = 10000 # VI algo vi = valueiteration.ValueIteration_ND( grid_sys , qcf ) vi.initialize() # vi.load_data('simple_pendulum_vi') vi.compute_steps() #vi.load_data() vi.assign_interpol_controller() vi.plot_policy(0)
sys.ubar = np.array([1]) # constant input = 1 ################################### # Analysis ################################### # Phase plane behavior test sys.plot_phase_plane() # Simulation sys.x0 = np.array([0,0]) traj = sys.compute_trajectory( tf = 10 ) sys.plot_trajectory() sys.plot_trajectory('y') sys.plot_phase_plane_trajectory() # Cost computing qcf = costfunction.QuadraticCostFunction(2, 1, 1) traj_with_qcf = qcf.trajectory_evaluation( traj ) sys.traj = traj_with_qcf sys.plot_trajectory('xuj') tcf = costfunction.TimeCostFunction( np.array([0,0]) ) traj_with_tcf = tcf.trajectory_evaluation( traj ) sys.traj = traj_with_tcf sys.plot_trajectory('xuj')
############################################################################### # Planning # Set domain sys.x_ub = np.array([+3.5, +1, +0.3]) sys.x_lb = np.array([-2, -1, -0.3]) sys.u_ub = np.array([+1, +1]) sys.u_lb = np.array([-1, -1]) # Discrete world grid_sys = discretizer.GridDynamicSystem(sys, (61, 61, 21), (3, 3), 0.025) # Cost Function cf = costfunction.QuadraticCostFunction(sys.n, sys.m, sys.p) cf.xbar = np.array([3, 0, 0]) # target cf.INF = 1E4 cf.EPS = 0.05 cf.R = np.array([[0.01, 0], [0, 0]]) # VI algo vi = valueiteration.ValueIteration_ND(grid_sys, cf) vi.uselookuptable = True vi.initialize() #if load_data: vi.load_data('udes_racecar')