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')
