Esempi in Python per GridDynamicSystem

Esempio n. 1

sys  = vehicle.KinematicCarModelwithObstacles()

###############################################################################

# Planning

# Set domain
sys.x_ub = np.array([+35, +3, +3])
sys.x_lb = np.array([-5, -2, -3])

sys.u_ub = np.array([+3, +1])
sys.u_lb = np.array([-3, -1])

# Discrete world
grid_sys = discretizer.GridDynamicSystem(sys, (51, 51, 21), (3, 3), 0.1)

# Cost Function
xbar    = np.array( [30, 0, 0] ) 
cf      = costfunction.TimeCostFunction( xbar )
cf.INF  = 50
cf.EPS  = 0.5

# VI algo
vi = valueiteration.ValueIteration_ND( grid_sys , cf )

vi.uselookuptable = True
vi.initialize()

vi.load_data('car_vi_min_time')
#vi.compute_steps(20,False)

Esempio n. 2

File: longitudinal_car_braking_value_iteration.py Progetto: SherbyRobotics/pyro

###############################################################################

sys  = longitudinal_vehicule.LongitudinalFrontWheelDriveCarWithWheelSlipInput()

###############################################################################

# Planning

# Set domain
sys.x_ub = np.array([+50, 20,])
sys.x_lb = np.array([0, 0])
sys.u_ub = np.array([0.0])
sys.u_lb = np.array([-0.1])

# Discrete world
grid_sys = discretizer.GridDynamicSystem(sys, (101, 101), (11,), 0.1)

# Cost Function
cf = costfunction.QuadraticCostFunction.from_sys( sys )
cf.xbar = np.array( [0, 0] ) # target
cf.INF  = 1E3
cf.EPS  = 0.00
cf.R[0]   = 1
cf.Q[0,0] = 0
cf.Q[1,1] = 0.01

# VI algo
vi = valueiteration.ValueIteration_ND( grid_sys , cf )

vi.uselookuptable = True
vi.initialize()

Esempio n. 3

File: simple_pendulum_with_valueiteration_graphical_cost2go.py Progetto: auxym/pyro

##############################################################################
import numpy as np
##############################################################################
from pyro.dynamic import pendulum
from pyro.planning import discretizer
from pyro.analysis import costfunction
from pyro.planning import valueiteration
from pyro.control import controller
##############################################################################

sys = pendulum.SinglePendulum()

##############################################################################

# Discrete world
grid_sys = discretizer.GridDynamicSystem(sys)

# Cost Function
qcf = costfunction.QuadraticCostFunction.from_sys(sys)

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.plot_max_J = 10
vi.compute_steps(200, True)
vi.assign_interpol_controller()

Esempio n. 4

sys = TwoSpeedLinearActuator()

###############################################################################

# Planning

# Set domain
sys.x_ub = np.array([+3, +3])
sys.x_lb = np.array([-3, -3])

sys.u_ub = np.array([+1, +1])
sys.u_lb = np.array([-0, -1])

# Discrete world
grid_sys = discretizer.GridDynamicSystem(sys, (51, 51), (2, 11), 0.1)

# Cost Function
cf = costfunction.QuadraticCostFunction.from_sys(sys)
cf.xbar = np.array([1, 0])  # target
cf.INF = 1E9
cf.EPS = 0.2
cf.R = np.array([[0, 0], [0, 1]])

# VI algo
vi = valueiteration.ValueIteration_ND(grid_sys, cf)

vi.uselookuptable = True
vi.initialize()

vi.compute_steps(100)

Esempio n. 5

File: bicycle_parking_with_valueiteration.py Progetto: auxym/pyro

from pyro.dynamic import vehicle
from pyro.planning import discretizer
from pyro.analysis import costfunction
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
"""

Esempio n. 6

sys = vehicle.KinematicProtoCarModelwithObstacles()

###############################################################################

# 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()

Esempio n. 7

File: double_pendulum_with_valueiteration_nd.py Progetto: SherbyRobotics/pyro

import matplotlib.pyplot as plt

from pyro.planning import discretizer, valueiteration

# Continuous dynamic system
sys = pendulum.DoublePendulum()

sys.x_ub = np.array([0.2, 0.2, 0.2, 0.2])
sys.x_lb = np.array([-0.2, -0.2, -0.2, -0.2])

sys.u_ub = np.array([5, 2])
sys.u_lb = np.array([-5, -2])

# Discrete world
grid_sys = discretizer.GridDynamicSystem(sys, (11, 11, 11, 11), (3, 3), 0.05)

# Cost Function
qcf = sys.cost_function

qcf.xbar = np.array([0, 0, 0, 0])  # target
qcf.INF = 100

# VI algo
vi = valueiteration.ValueIteration_ND(grid_sys, qcf)

vi.initialize()
#vi.load_data('bbb')
vi.compute_steps(2, True)
#vi.load_data()
vi.assign_interpol_controller()

Esempio n. 8

File: optimal_control_demo.py Progetto: SherbyRobotics/pyro

sys_with_lqr = lqr_ctl + sys
sys_with_lqr.plot_trajectory('xuj')
sys_with_lqr.animate_simulation()

print('Trajectory cost: ', sys_with_lqr.traj.J[-1])

##############################################################################
# VI
##############################################################################

from pyro.planning import valueiteration
from pyro.planning import discretizer

# Value iteration algo
vi = valueiteration.ValueIteration_2D(discretizer.GridDynamicSystem(sys), cf)

vi.initialize()
#vi.compute_steps(200,True). # To compute from sratch instead of loading the solution
vi.load_data('simple_pendulum_vi')  # /content/pyro/examples/demo/
vi.assign_interpol_controller()

vi_ctl = vi.ctl

vi_ctl.plot_control_law(sys=sys, n=1000)

sys_with_vi = vi_ctl + sys
sys_with_vi.plot_trajectory('xuj')
sys_with_vi.animate_simulation()

print('Trajectory cost: ', sys_with_vi.traj.J[-1])