def _create_integrals(self):
_model = self._model
_parameters = self._parameters
def _force_expr(_mod, _pos):
# to offset for having optimum at the target velocity
steady_motor_force_coef = 2 * _mod.air_fric_coef * _mod.v_target ** 3 # THERE SHOULD BE A FACTOR 2 IN THERE BUT IF IT'S THERE IT MESSES IT UP ARGH. AAAAARGH i found it.
# add a constant to show cost departure from equilibrium rather than abs
cost_balancer = (_mod.air_fric_coef * _mod.v_target ** 2
- _mod.rolling_friction[_pos]
- _mod.gravity[_pos]
+ steady_motor_force_coef / _mod.v_target)
return (_mod.motor_force[_pos] + steady_motor_force_coef * _mod.v_inv[_pos]
- cost_balancer
if _pos < _parameters["episode_dist_end"] else 0)
# for soft constraint on velocity
_model.v_softlim = env.Var(_model.x_local, domain=env.Reals,
initialize=BaseModel._create_const_rule(0))
_model.v_softconstraint = env.Constraint(_model.x_local,
rule=BaseModel._create_soft_constraint_fn(
_model.v,
_model.v_softlim,
(_model.v_soft_min,
_model.v_soft_max)))
def _motor_smooth_expr(_mod, _pos):
return (_mod.d2m_dx2[_pos] ** 2
+ _mod.d2b_dx2[_pos] ** 2
if _pos < _parameters["episode_dist_end"] else 0)
def _vel_expr(_mod, _pos):
return (abs(_mod.v_softlim[_pos])**2 * _mod.v_inv[_pos]
if _pos < _parameters["episode_dist_end"] else 0)
def _v_2ndtimederiv_expr(_mod, _pos):
return (_mod.v[_pos]*(_mod.dv_dx[_pos]**2 +
_mod.v[_pos] * _mod.dv_dx[_pos].derivative(_mod.x_local)
)**2
if _pos < _parameters["episode_dist_end"] else 0)
_model.energy_integral = dae.Integral(_model.x_local, wrt=_model.x_local,
rule=_force_expr)
_model.velocity_integral = dae.Integral(_model.x_local, wrt=_model.x_local,
rule=_vel_expr)
_model.acc_smooth_integral = dae.Integral(_model.x_local, wrt=_model.x_local,
rule=_v_2ndtimederiv_expr)
_model.motor_smooth_integral= dae.Integral(_model.x_local, wrt=_model.x_local,
rule=_motor_smooth_expr)
_model.inst_cost = env.Expression(_model.x_local,
rule=lambda _mod, _pos: (
_mod.motor_cost_coef * _force_expr(_mod, _pos)
+ _mod.vel_cost_coef * _vel_expr(_mod, _pos)
+ _mod.acc_cost_coef * _v_2ndtimederiv_expr(_mod, _pos)
+ _mod.motor_smooth_coef * _motor_smooth_expr(_mod, _pos)
# to compensate for the cost compensation
- _mod.gravity[_pos]
- _mod.rolling_friction[_pos]))
def create_problem(begin, end):
m = aml.ConcreteModel()
m.t = dae.ContinuousSet(bounds=(begin, end))
m.x = aml.Var([1, 2], m.t, initialize=1.0)
m.u = aml.Var(m.t, bounds=(None, 0.8), initialize=0)
m.xdot = dae.DerivativeVar(m.x)
def _x1dot(M, i):
if i == M.t.first():
return aml.Constraint.Skip
return M.xdot[1,
i] == (1 - M.x[2, i]**2) * M.x[1, i] - M.x[2, i] + M.u[i]
m.x1dotcon = aml.Constraint(m.t, rule=_x1dot)
def _x2dot(M, i):
if i == M.t.first():
return aml.Constraint.Skip
return M.xdot[2, i] == M.x[1, i]
m.x2dotcon = aml.Constraint(m.t, rule=_x2dot)
def _init(M):
t0 = M.t.first()
yield M.x[1, t0] == 0
yield M.x[2, t0] == 1
yield aml.ConstraintList.End
m.init_conditions = aml.ConstraintList(rule=_init)
def _int_rule(M, i):
return M.x[1, i]**2 + M.x[2, i]**2 + M.u[i]**2
m.integral = dae.Integral(m.t, wrt=m.t, rule=_int_rule)
m.obj = aml.Objective(expr=m.integral)
m.init_condition_names = ['init_conditions']
return m
def build_burgers_model(self,
nfe_x=50,
nfe_t=50,
start_t=0,
end_t=1,
add_init_conditions=True):
dt = (end_t - start_t) / float(nfe_t)
start_x = 0
end_x = 1
dx = (end_x - start_x) / float(nfe_x)
m = pe.Block(concrete=True)
m.omega = pe.Param(initialize=0.02)
m.v = pe.Param(initialize=0.01)
m.r = pe.Param(initialize=0)
m.x = dae.ContinuousSet(bounds=(start_x, end_x))
m.t = dae.ContinuousSet(bounds=(start_t, end_t))
m.y = pe.Var(m.x, m.t)
m.dydt = dae.DerivativeVar(m.y, wrt=m.t)
m.dydx = dae.DerivativeVar(m.y, wrt=m.x)
m.dydx2 = dae.DerivativeVar(m.y, wrt=(m.x, m.x))
m.u = pe.Var(m.x, m.t)
def _y_init_rule(m, x):
if x <= 0.5 * end_x:
return 1
return 0
m.y0 = pe.Param(m.x, default=_y_init_rule)
def _upper_x_bound(m, t):
return m.y[end_x, t] == 0
m.upper_x_bound = pe.Constraint(m.t, rule=_upper_x_bound)
def _lower_x_bound(m, t):
return m.y[start_x, t] == 0
m.lower_x_bound = pe.Constraint(m.t, rule=_lower_x_bound)
def _upper_x_ubound(m, t):
return m.u[end_x, t] == 0
m.upper_x_ubound = pe.Constraint(m.t, rule=_upper_x_ubound)
def _lower_x_ubound(m, t):
return m.u[start_x, t] == 0
m.lower_x_ubound = pe.Constraint(m.t, rule=_lower_x_ubound)
def _lower_t_bound(m, x):
if x == start_x or x == end_x:
return pe.Constraint.Skip
return m.y[x, start_t] == m.y0[x]
def _lower_t_ubound(m, x):
if x == start_x or x == end_x:
return pe.Constraint.Skip
return m.u[x, start_t] == 0
if add_init_conditions:
m.lower_t_bound = pe.Constraint(m.x, rule=_lower_t_bound)
m.lower_t_ubound = pe.Constraint(m.x, rule=_lower_t_ubound)
# PDE
def _pde(m, x, t):
if t == start_t or x == end_x or x == start_x:
e = pe.Constraint.Skip
else:
# print(foo.last_t, t-dt, abs(foo.last_t - (t-dt)))
# assert math.isclose(foo.last_t, t - dt, abs_tol=1e-6)
e = m.dydt[x, t] - m.v * m.dydx2[x, t] + m.dydx[x, t] * m.y[
x, t] == m.r + m.u[x, self.last_t]
self.last_t = t
return e
m.pde = pe.Constraint(m.x, m.t, rule=_pde)
# Discretize Model
disc = pe.TransformationFactory('dae.finite_difference')
disc.apply_to(m, nfe=nfe_t, wrt=m.t, scheme='BACKWARD')
disc.apply_to(m, nfe=nfe_x, wrt=m.x, scheme='CENTRAL')
# Solve control problem using Pyomo.DAE Integrals
def _intX(m, x, t):
return (m.y[x, t] - m.y0[x])**2 + m.omega * m.u[x, t]**2
m.intX = dae.Integral(m.x, m.t, wrt=m.x, rule=_intX)
def _intT(m, t):
return m.intX[t]
m.intT = dae.Integral(m.t, wrt=m.t, rule=_intT)
def _obj(m):
e = 0.5 * m.intT
for x in sorted(m.x):
if x == start_x or x == end_x:
pass
else:
e += 0.5 * 0.5 * dx * dt * m.omega * m.u[x, start_t]**2
return e
m.obj = pe.Objective(rule=_obj)
return m
def motion_model(tf):
"""Motion model for car maneuvering task from given intial
conditions to final conditions.
Parameters
----------
tf : float
Final time of the maneuvering.
Returns
-------
m
A pyomo model with all the variables and constraints described.
"""
m = pyo.ConcreteModel()
m.time = pyod.ContinuousSet(bounds=(0, tf))
# Append dependent variables
dependent_variables = {
'x': (0, None),
'y': (0, None),
't': (None, None),
'u': (None, None),
'p': (-0.5, 0.5)
}
for key, value in dependent_variables.items():
m.add_component(key, pyo.Var(m.time, bounds=value))
# Append derivatives
derivative = {
'x': 'dxdt',
'y': 'dydt',
't': 'dtdt',
'u': 'dudt',
'p': 'dpdt'
}
for var in m.component_objects(pyo.Var, active=True):
m.add_component(derivative[str(var)],
pyod.DerivativeVar(var, wrt=m.time))
# Append control variables
control_inputs = {'a': (None, None), 'v': (-0.1, 0.1)}
for key, value in control_inputs.items():
m.add_component(key, pyo.Var(m.time, bounds=value))
# Append differential equations as constraints
L = 2
m.ode_x = pyo.Constraint(
m.time,
rule=lambda m, time: m.dxdt[time] == m.u[time] * pyo.cos(m.t[time]))
m.ode_y = pyo.Constraint(
m.time,
rule=lambda m, time: m.dydt[time] == m.u[time] * pyo.sin(m.t[time]))
m.ode_t = pyo.Constraint(m.time,
rule=lambda m, time: m.dtdt[time] == m.u[time] *
pyo.tan(m.p[time]) / L)
m.ode_u = pyo.Constraint(m.time,
rule=lambda m, time: m.dudt[time] == m.a[time])
m.ode_p = pyo.Constraint(m.time,
rule=lambda m, time: m.dpdt[time] == m.v[time])
# Add final and initial values
m.ic = pyo.ConstraintList()
intial_condition = {'x': 0, 'y': 0, 't': 0, 'u': 0, 'p': 0, 'a': 0, 'v': 0}
for i, var in enumerate(m.component_objects(pyo.Var, active=True)):
m.ic.add(var[0] == intial_condition[str(var)])
m.fc = pyo.ConstraintList()
final_condition = {'x': 0, 'y': 20, 't': 0, 'u': 0, 'p': 0, 'a': 0, 'v': 0}
for i, var in enumerate(m.component_objects(pyo.Var, active=True)):
m.fc.add(var[tf] == final_condition[str(var)])
# Objective function
m.integral = pyod.Integral(
m.time,
wrt=m.time,
rule=lambda m, time: 0.2 * m.p[time]**2 + m.a[time]**2 + m.v[time]**2)
m.obj = pyo.Objective(expr=m.integral)
return m
