def Simulation(self,x0,u,params=None):
'''
A iterative application of the OneStepPrediction in order to perform a
simulation for a whole input trajectory
x0: Casadi MX, inital state a begin of simulation
u: Casadi MX, input trajectory
params: A dictionary of opti variables, if the parameters of the model
should be optimized, if None, then the current parameters of
the model are used
'''
x = []
y = []
# initial states
x.append(x0)
# Simulate Model
for k in range(u.shape[0]):
x_new,y_new = self.OneStepPrediction(x[k],u[[k],:],params)
x.append(x_new)
y.append(y_new)
# Concatenate list to casadiMX
y = cs.hcat(y).T
x = cs.hcat(x).T
return y
def Simulation(self,x0,u,params=None):
"""
Repeated call of self.OneStepPrediction() for a given input trajectory
Parameters
----------
x0 : array-like with dimension [self.dim_out, 1]
initial output
u : array-like with dimension [N,self.dim_u]
trajectory of input signal with length N
params : dictionary, optional
see self.OneStepPrediction()
Returns
-------
x : array-like with dimension [N+1,self.dim_out]
trajectory of output signal with length N+1
"""
x = []
# initial states
x.append(x0)
# Simulate Model
for k in range(u.shape[0]):
x.append(self.OneStepPrediction(x[k],u[[k],:],params))
# Concatenate list to casadiMX
x = cs.hcat(x).T
return x
def generate_model(self, configuration):
""" Logic to construct a model, and smooth representation on BSpline basis """
self.n = configuration['grid_size']
self.K = configuration['basis_number']
self.s = configuration['model_form']['state']
n_ps = configuration['model_form']['parameters']
# setup fine time grid
self.ts = ca.MX.sym("t", self.n, 1)
self.observation_times = np.linspace(*configuration['time_span'][:2],
self.n)
# determine knots and build basis functions
if configuration['knot_function'] is None:
knots = casbasis.choose_knots(self.observation_times, self.K - 2)
else:
knots = configuration['knot_function'](self.observation_times,
self.K - 2,
configuration['dataset'])
self.basis_fns = casbasis.basis_functions(knots)
self.basis = ca.vcat([b(self.ts) for b in self.basis_fns]).reshape(
(self.n, self.K))
self.tssx = ca.SX.sym("t", self.n, 1)
# define basis matrix and gradient matrix
phi = ca.Function('phi', [self.ts], [self.basis])
self.phi = np.array(phi(self.observation_times))
bjac = ca.vcat([
ca.diag(ca.jacobian(self.basis[:, i], self.ts))
for i in range(self.K)
]).reshape((self.n, self.K))
self.basis_jacobian = np.array(
ca.Function('bjac', [self.ts], [bjac])(self.observation_times))
# create the objects that define the smooth, model parameters
self.cs = [ca.SX.sym("c_" + str(i), self.K, 1) for i in range(self.s)]
self.xs = [self.phi @ ci for ci in self.cs]
self.xdash = self.basis_jacobian @ ca.hcat(self.cs)
self.ps = [ca.SX.sym("p_" + str(i)) for i in range(n_ps)]
# model function derived from input model function
self.model = ca.Function(
"model", [self.tssx, *self.cs, *self.ps],
[ca.hcat(configuration['model'](self.tssx, self.xs, self.ps))])
def Simulation(self, c0, u, params=None):
"""
Repeated call of self.OneStepPrediction() for a given input trajectory
Parameters
----------
c0 : array-like with dimension [self.dim_c, 1]
initial cell-state
u : array-like with dimension [N,self.dim_u]
trajectory of input signal with length N
params : dictionary, optional
see self.OneStepPrediction()
Returns
-------
x : array-like with dimension [N+1,self.dim_x]
trajectory of output signal with length N+1
"""
# Is that necessary?
print(
'GRU Simulation ignores given initial state, initial state is set to zero!'
)
c0 = np.zeros((self.dim_c, 1))
c = []
x = []
# initial cell state
c.append(c0)
# Simulate Model
for k in range(u.shape[0]):
c_new, x_new = self.OneStepPrediction(c[k], u[k, :], params)
c.append(c_new)
x.append(x_new)
# Concatenate list to casadiMX
c = cs.hcat(c).T
x = cs.hcat(x).T
return x[-1]
def get_link_relative_transform_casadi(self, qi):
""" Link transform according to the Denavit-Hartenberg convention.
Casadi compatible function.
"""
if self.joint_type == JointType.revolute:
a, alpha, d, theta = self.dh.a, self.dh.alpha, self.dh.d, qi
elif self.joint_type == JointType.prismatic:
a, alpha, d, theta = self.dh.a, self.dh.alpha, qi, self.dh.theta
c_t, s_t = ca.cos(theta), ca.sin(theta)
c_a, s_a = ca.cos(alpha), ca.sin(alpha)
row1 = ca.hcat([c_t, -s_t * c_a, s_t * s_a, a * c_t])
row2 = ca.hcat([s_t, c_t * c_a, -c_t * s_a, a * s_t])
row3 = ca.hcat([0, s_a, c_a, d])
row4 = ca.hcat([0, 0, 0, 1])
return ca.vcat([row1, row2, row3, row4])
def collocation_coeff(tau):
[C, D] = collocation_interpolators(tau)
d = len(tau)
tau = [0] + tau
F = [None] * (d + 1)
# Construct polynomial basis
for j in range(d + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(d + 1):
if r != j:
p *= np.poly1d([1, -tau[r]]) / (tau[j] - tau[r])
# Evaluate the integral of the polynomial to get the coefficients of the quadrature function
pint = np.polyint(p)
F[j] = pint(1.0)
return (hcat(C[1:]), vcat(D), hcat(F[1:]))
def discrete_system(self, stage):
f = stage._ode()
# Coefficient matrix from RK4 to reconstruct 4th order polynomial (k1,k2,k3,k4)
# nstates x (4 * M)
poly_coeffs = []
t0 = MX.sym('t0')
T = MX.sym('T')
DT = T / self.M
# Size of integrator interval
X0 = f.mx_in("x") # Initial state
U = f.mx_in("u") # Control
P = f.mx_in("p")
Z = f.mx_in("z")
X = [X0]
if hasattr(self, 'intg_'+ self.intg):
intg = getattr(self, "intg_" + self.intg)(f, X0, U, P, Z)
else:
intg = self.intg_builtin(f, X0, U, P, Z)
assert not intg.has_free()
# Compute local start time
t0_local = t0
quad = 0
for j in range(self.M):
intg_res = intg(x0=X[-1], u=U, t0=t0_local, DT=DT, p=P)
X.append(intg_res["xf"])
quad = quad + intg_res["qf"]
poly_coeffs.append(intg_res["poly_coeff"])
t0_local += DT
ret = Function('F', [X0, U, T, t0, P], [X[-1], hcat(X), hcat(poly_coeffs), quad],
['x0', 'u', 'T', 't0', 'p'], ['xf', 'Xi', 'poly_coeff', 'qf'])
assert not ret.has_free()
return ret
def _grid_integrator_roots(self, stage, expr, grid):
"""Evaluate expression at integrator roots."""
sub_expr = []
tr = []
for k in range(stage._method.N):
for l in range(stage._method.M):
for j in range(stage._method.xr[k][l].shape[1]):
sub_expr.append(
stage._method.eval_at_integrator_root(
stage, expr, k, l, j))
tr.extend(stage._method.tr[k][l])
res = [self.sol.value(elem) for elem in sub_expr]
time = self.sol.value(hcat(tr))
return time, np.array(res)
def cost_cas(self, goal=None, weights=None, var=True, state_cost_start=0):
"""
Creates a casadi cost function
:param goal: The goal. If none, the environment's default goal will be used
:param weights: The weights for the state and action dimensions.
If none, the environment's default goal will be used
:param var: Whether the input of the casadi function receives the variances
:param state_cost_start: The time step when the state cost is considered fist
:return:
"""
if weights is None:
weights = self.task_cost_weights
if goal is None:
goal = self.task_goal
assert(weights.size == self.x_dim)
assert(goal.size == self.o_dim)
nones = np.argwhere(np.isnan(goal.ravel()))
goal.ravel()[nones] = 0
weights[nones] = 0
goal = np.reshape(goal, (1, -1))
xi = cas.MX.sym("xi", 1, self.o_dim)
ui = cas.MX.sym("ui", 1, self.a_dim)
fun_in = [xi, ui]
if var:
vxi = cas.MX.sym("vxi", 1, 1)
fun_in += [vxi]
dist_goal = xi - goal
vals = cas.hcat((dist_goal, ui))
if state_cost_start is None or state_cost_start == 0:
state_cost_start = 0
elif state_cost_start < 0:
state_cost_start = self.task_t - state_cost_start
# make return time dependent cost function
def o(t):
if t < state_cost_start - 1:
pass
else:
return cas.Function("reward", fun_in, [cas.mtimes(vals, weights * vals.T)])
# o = lambda _: cas.Function("reward", fun_in, [cas.mtimes(vals, weights * vals.T)])
return o
def _grid_intg_fine(self, stage, expr, grid, refine):
"""Evaluate expression at extra fine integrator discretization points."""
if stage._method.poly_coeff is None:
msg = "No polynomal coefficients for the {} integration method".format(
stage._method.intg)
raise Exception(msg)
N, M, T = stage._method.N, stage._method.M, stage._method.T
expr_f = Function('expr', [stage.x, stage.z, stage.u], [expr])
sub_expr = []
time = self.sol.value(stage._method.control_grid)
total_time = []
for k in range(N):
t0 = time[k]
dt = (time[k + 1] - time[k]) / M
tlocal = np.linspace(0, dt, refine + 1)
ts = DM(tlocal[:-1]).T
for l in range(M):
total_time.append(t0 + tlocal[:-1])
coeff = stage._method.poly_coeff[k * M + l]
tpower = vcat([ts**i for i in range(coeff.shape[1])])
if stage._method.poly_coeff_z:
coeff_z = stage._method.poly_coeff_z[k * M + l]
tpower_z = vcat([ts**i for i in range(coeff_z.shape[1])])
z = coeff_z @ tpower_z
else:
z = nan
sub_expr.append(expr_f(coeff @ tpower, z, stage._method.U[k]))
t0 += dt
ts = tlocal[-1]
total_time.append(time[k + 1])
tpower = vcat([ts**i for i in range(coeff.shape[1])])
if stage._method.poly_coeff_z:
tpower_z = vcat([ts**i for i in range(coeff_z.shape[1])])
z = coeff_z @ tpower_z
else:
z = nan
sub_expr.append(
expr_f(stage._method.poly_coeff[-1] @ tpower, z,
stage._method.U[-1]))
res = self.sol.value(hcat(sub_expr))
time = np.hstack(total_time)
return time, res
def intg_rk(self, f, X, U, P, Z):
assert Z.is_empty()
DT = MX.sym("DT")
t0 = MX.sym("t0")
# A single Runge-Kutta 4 step
k1 = f(x=X, u=U, p=P, t=t0, z=Z)
k2 = f(x=X + DT / 2 * k1["ode"], u=U, p=P, t=t0+DT/2, z=Z)
k3 = f(x=X + DT / 2 * k2["ode"], u=U, p=P, t=t0+DT/2)
k4 = f(x=X + DT * k3["ode"], u=U, p=P, t=t0+DT)
f0 = k1["ode"]
f1 = 2/DT*(k2["ode"]-k1["ode"])/2
f2 = 4/DT**2*(k3["ode"]-k2["ode"])/6
f3 = 4*(k4["ode"]-2*k3["ode"]+k1["ode"])/DT**3/24
poly_coeff = hcat([X, f0, f1, f2, f3])
return Function('F', [X, U, t0, DT, P], [X + DT / 6 * (k1["ode"] + 2 * k2["ode"] + 2 * k3["ode"] + k4["ode"]), poly_coeff, DT / 6 * (k1["quad"] + 2 * k2["quad"] + 2 * k3["quad"] + k4["quad"])], ['x0', 'u', 't0', 'DT', 'p'], ['xf', 'poly_coeff', 'qf'])
varguess["x", 0, :] = x[t, :]
args = dict(x0=varguess,
lbx=varlb,
ubx=varub,
lbg=conlb,
ubg=conub,
p=parguess)
#<<ENDCHUNK>>
# Solve nlp.
sol = solver(**args)
status = "a" # solver.stats()["return_status"]
optvar = var(sol["x"])
outMap = jacMap(casadi.hcat(optvar['x', :-1]), casadi.hcat(optvar['u']))
# _Ac = casadi.blocksplit(outMap[0],Nx,Nx)[0]
# _Bc = casadi.blocksplit(outMap[1],Nx,Nu)[0]
# _fc = casadi.blocksplit(outMap[2],Nx,1)[0]
if t == 0:
parguess["Ad", :], parguess["Bd", :], parguess["fd", :] = zip(*map(
_calc_lin_disc_wrapper_for_mp_map_2,
zip(
casadi.blocksplit(outMap[0], Nx, Nx)[0],
casadi.blocksplit(outMap[1], Nx, Nu)[0],
casadi.blocksplit(outMap[2], Nx, 1)[0],
[Delta for _k in xrange(Nt)])))
else:
parguess["Ad", 0:-1] = parguess["Ad", 1:]
parguess["Bd", 0:-1] = parguess["Bd", 1:]
def add_constraints(self, stage, opti):
# Obtain the discretised system
f = stage._ode()
if stage.is_free_time():
opti.subject_to(self.T >= 0)
ps = []
tau_root = [0] + self.tau
# Construct polynomial basis
for j in range(self.degree + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p = np.poly1d([1])
for r in range(self.degree + 1):
if r != j:
p *= np.poly1d([1, -tau_root[r]
]) / (tau_root[j] - tau_root[r])
ps.append(hcat(p.coef[::-1]))
poly = vcat(ps)
ps_z = []
# Construct polynomial basis for z cfr "2.5 Continuous Output for Optimal Control" from Rien's thesis
for j in range(1, self.degree + 1):
# Construct Lagrange polynomials to get the polynomial basis at the collocation point
p_z = np.poly1d([1])
for r in range(1, self.degree + 1):
if r != j:
p_z *= np.poly1d([1, -tau_root[r]
]) / (tau_root[j] - tau_root[r])
ps_z.append(hcat(p_z.coef[::-1]))
poly_z = vcat(ps_z)
self.q = 0
# Make time-grid for roots
for k in range(self.N):
dt = (self.control_grid[k + 1] - self.control_grid[k]) / self.M
tr = []
for i in range(self.M):
tr.append([
self.integrator_grid[k][i] + dt * self.tau[j]
for j in range(self.degree)
])
self.tr.append(tr)
for k in range(self.N):
dt = (self.control_grid[k + 1] - self.control_grid[k]) / self.M
S = 1 / repmat(hcat([dt**i for i in range(self.degree + 1)]),
self.degree + 1, 1)
S_z = 1 / repmat(hcat([dt**i for i in range(self.degree)]),
self.degree, 1)
self.Z.append(self.Zc[k][0] @ poly_z[:, 0])
for i in range(self.M):
self.xk.append(self.Xc[k][i][:, 0])
self.poly_coeff.append(self.Xc[k][i] @ (poly * S))
self.poly_coeff_z.append(self.Zc[k][i] @ (poly_z * S_z))
self.zk.append(self.Zc[k][i] @ poly_z[:, 0])
for j in range(self.degree):
Pidot_j = self.Xc[k][i] @ self.C[:, j] / dt
res = f(x=self.Xc[k][i][:, j + 1],
u=self.U[k],
z=self.Zc[k][i][:, j],
p=self.P,
t=self.tr[k][i][j])
# Collocation constraints
opti.subject_to(Pidot_j == res["ode"])
self.q = self.q + res["quad"] * dt * self.B[j]
if stage.nz:
opti.subject_to(0 == res["alg"])
for c, meta, _ in stage._constraints["integrator_roots"]:
opti.subject_to(self.eval_at_integrator_root(
stage, c, k, i, j),
meta=meta)
# Continuity constraints
x_next = self.X[k + 1] if i == self.M - 1 else self.Xc[k][i +
1][:,
0]
opti.subject_to(self.Xc[k][i] @ self.D == x_next)
for c, meta, _ in stage._constraints["integrator"]:
opti.subject_to(self.eval_at_integrator(stage, c, k, i),
meta=meta)
for c, meta, _ in stage._constraints["inf"]:
self.add_inf_constraints(stage, opti, c, k, i, meta)
for c, meta, _ in stage._constraints[
"control"]: # for each constraint expression
# Add it to the optimizer, but first make x,u concrete.
opti.subject_to(self.eval_at_control(stage, c, k), meta=meta)
self.Z.append(self.Zc[-1][-1] @ sum2(poly_z))
for c, meta, _ in stage._constraints["control"] + stage._constraints[
"integrator"]: # for each constraint expression
# Add it to the optimizer, but first make x,u concrete.
opti.subject_to(self.eval_at_control(stage, c, -1), meta=meta)
for c, meta, _ in stage._constraints[
"point"]: # Append boundary conditions to the end
opti.subject_to(self.eval(stage, c), meta=meta)
