def create_plan_fc(b0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=belief), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Objective function m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']] m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']] dm_bc = m_bN - m_cN J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0 # Regularize controls J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs g = [] for k in range(N_sim): # Multiple shooting [x_next] = BF([V['X',k], V['U',k]]) g.append(x_next - V['X',k+1]) # Penalize uncertainty J += 1e-1 * ca.trace(V['X',k,'S']) * dt g = ca.vertcat(g) # log-probability, doesn't work with full collocation #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']] #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \ # ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v <= v_max lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # m(t=0) = m0 lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m'] # S(t=0) = S0 lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S'] # Solve the NLP sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) return V(sol['x'])
def setup_oed(self, outputs, parameters, sigma, time_points, design="A"): """ Transforms an Optimization Problem into an Optimal Experimental Design problem. Parameters:: outputs -- List of names for outputs. Type: [string] parameters -- List of names for parameters to estimate. Type: [string] sigma -- Experiment variance matrix. Type: [[float]] time_points -- List of measurement time points. Type: [float] design -- Design criterion. Possible values: "A", "T" """ # Augment sensitivities and add timed variables self.augment_sensitivities(parameters) timed_sens = self.create_timed_sensitivities(outputs, parameters, time_points) # Create sensitivity and Fisher matrices Q = [] for j in xrange(len(outputs)): Q.append(casadi.vertcat([casadi.horzcat([s.getVar() for s in timed_sens[i][j]]) for i in xrange(len(time_points))])) Fisher = sum([sigma[i, j] * casadi.mul(Q[i].T, Q[j]) for i in xrange(len(outputs)) for j in xrange(len(outputs))]) # Define the objective if design == "A": b = casadi.MX.sym("b", Fisher.shape[1], 1) Fisher_inv = casadi.jacobian(casadi.solve(Fisher, b), b) obj = casadi.trace(Fisher_inv) elif design == "T": obj = -casadi.trace(Fisher) elif design == "D": obj = -casadi.det(Fisher) raise NotImplementedError("D-optimal design is not supported.") else: raise ValueError("Invalid design %s." % design) old_obj = self.getObjective() self.setObjective(old_obj + obj)
def loss_quadratic(m, z, v=None, W=None): """ Quadratic cost function Simple quadratic loss with W as weight matrix, ignoring variance Parameters ---------- m : dx1 ndarray[float | casadi.Sym] The mean of the input Gaussian v : dxd ndarray[float | casadi.Sym] z: dx1 ndarray[float | casadi.Sym] The target-state [optional] W: dxd ndarray[float | casadi.Sym] The weighting matrix factor for the cost-function (scaling) Returns ------- L: float The quadratic loss """ D = np.shape(m)[0] if W is None: W = SX.eye(D) l_var = 0 if not v is None: l_var = trace(mtimes(W, v)) return mtimes((m - z).T, mtimes(W, m - z)) + l_var
def FIM_t(xpdot, b, criterion): ''' computes FIM at a given time. b is the bonary variable which selects or not the time point ''' n_x = 2 n_theta = 4 FIM_sample = np.zeros([n_theta, n_theta]) FIM_0 = cad.inv((((10.0 - 0.001)**2) / 12) * cad.SX.eye(n_theta)) FIM_sample += FIM_0 for i in range(np.shape(xpdot)[0] - 1): xp_r = cad.reshape(xpdot[i + 1], (n_x, n_theta)) # vv = np.zeros([ntheta[0], ntheta[0], 1 + N]) # for i in range(0, 1 + N): FIM_sample += b[i] * xp_r.T @ np.linalg.inv( np.array([[0.01, 0], [0, 0.05]]) ) @ xp_r # + np.linalg.inv(np.array([[0.01, 0, 0, 0], [0, 0.05, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0.2]])) # FIM = solve(FIM1, SX.eye(FIM1.size1())) # [Q, R] = qr(FIM1.expand()) if criterion == 'D_optimal': # objective = -cad.log(cad.det(FIM_sample) + 1e-10) objective = -2 * cad.sum1(cad.log(cad.diag( cad.chol(FIM_sample)))) # by Cholesky factorisation # objective = -cad.log((cad.det(FIM_sample)**2)) # objective = -cad.det(FIM_sample) elif criterion == 'A_optimal': objective = -cad.log(cad.trace(FIM_sample) + 1e-10) return objective
def test(self): if tools.cross_building(self.settings): print("NOT RUN (cross-building)") return # swig_python if self.options["casadi"].swig_python: self.output.info("Try to load 'casadi' python module") try: import casadi A = casadi.SX.eye(2) if casadi.trace(A) == 2: self.output.info("Completed conanized casadi climax") except ModuleNotFoundError: try: import numpy except ModuleNotFoundError: self.output.error("Casadi requires 'numpy', but it was not found") self.output.error("Unable to proplerly load python casadi module") self.output.info("Run consumer tests for library interfaces") cmake = self._configure_cmake() if self.options["casadi"].hpmpc: self.output.warn("HPMPC plugin interface is not tested") if self.options["casadi"].dsdp: self.output.warn("DSDP interface is not tested(?)") env_build = RunEnvironment(self) with tools.environment_append(env_build.vars): cmake.test() self.output.info("Casadi OK!")
def asTwist(self): acosarg = (casadi.trace(self.R) - 1) / 2 theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg)) w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]), (self.R[0, 2] - self.R[2, 0]), (self.R[1, 0] - self.R[0, 1])) return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def get_stage_cost_func(x, u, p, Cs, Cu, x_target, dt): P = cs.reshape(p, 11, 11) cost = 0.5 * cs.trace(P @ Cs) cost += 0.5 * cs.dot(x[:6] - x_target, Cs[:6, :6] @ (x[:6] - x_target)) cost += 0.5 * cs.dot(u, Cu @ u) cost *= dt c_stage = cs.Function('c_stage', [x, u, p], [cost], ['x', 'u', 'p'], ['cost']) return c_stage
def __cost_lf(self, x, x_ref, covar_x, P, s=1): """ Terminal cost function: Expected Value of Quadratic Cost """ P_s = ca.SX.sym('Q', ca.MX.size(P)) x_s = ca.SX.sym('x', ca.MX.size(x)) covar_x_s = ca.SX.sym('covar_x', ca.MX.size(covar_x)) sqnorm_x = ca.Function('sqnorm_x', [x_s, P_s], [ca.mtimes(x_s.T, ca.mtimes(P_s, x_s))]) trace_x = ca.Function('trace_x', [P_s, covar_x_s], [s * ca.trace(ca.mtimes(P_s, covar_x_s))]) return sqnorm_x(x - x_ref, P) + trace_x(P, covar_x)
def __cost_l(self, x, x_ref, covar_x, u, covar_u, delta_u, Q, R, S, s=1): """ Stage cost function: Expected Value of Quadratic Cost """ Q_s = ca.SX.sym('Q', ca.MX.size(Q)) R_s = ca.SX.sym('R', ca.MX.size(R)) x_s = ca.SX.sym('x', ca.MX.size(x)) u_s = ca.SX.sym('u', ca.MX.size(u)) covar_x_s = ca.SX.sym('covar_x', ca.MX.size(covar_x)) covar_u_s = ca.SX.sym('covar_u', ca.MX.size(R)) sqnorm_x = ca.Function('sqnorm_x', [x_s, Q_s], [ca.mtimes(x_s.T, ca.mtimes(Q_s, x_s))]) sqnorm_u = ca.Function('sqnorm_u', [u_s, R_s], [ca.mtimes(u_s.T, ca.mtimes(R_s, u_s))]) trace_u = ca.Function('trace_u', [R_s, covar_u_s], [s * ca.trace(ca.mtimes(R_s, covar_u_s))]) trace_x = ca.Function('trace_x', [Q_s, covar_x_s], [s * ca.trace(ca.mtimes(Q_s, covar_x_s))]) return sqnorm_x(x - x_ref, Q) + sqnorm_u(u, R) + sqnorm_u(delta_u, S) \ + trace_x(Q, covar_x) + trace_u(R, covar_u)
def cost_func_BSP(self, robot, U, X0, cov_X0): cost = 0 U = c.reshape(U, robot.nu, self.T) X = c.MX(robot.nx, self.T + 1) X[:, 0] = X0 P = cov_X0 for i in range(self.T): X[:, i + 1] = robot.kinematics(X[:, i], U[0, i], U[1, i], U[2, i]) H, M = self.light_dark_MX(X[:, i + 1]) Sigma_w = (self.epsilon**2) * self.Sigma_w Sigma_nu = (self.epsilon**2) * self.Sigma_nu P = c.mtimes(c.mtimes(robot.A, P), robot.A.T) + c.mtimes( c.mtimes(robot.G, Sigma_w), robot.G.T) S = c.mtimes(c.mtimes(H, P), H.T) + c.mtimes( c.mtimes(M, Sigma_nu), M.T) K = c.mtimes(c.mtimes(P, H.T), c.inv(S)) P = c.mtimes(c.DM.eye(robot.nx) - c.mtimes(K, H), P) cost = cost + self.gamma * c.trace( c.mtimes(c.mtimes(self.Q, P), self.Q.T)) + c.mtimes( c.mtimes(U[:, i].T, self.R), U[:, i]) X_temp = X[0:2, i] if params.OBSTACLES: obstacle_cost = self.obstacle_cost_func(X_temp) cost = cost + obstacle_cost cost = cost + c.mtimes(c.mtimes((self.Xg - X[:, self.T]).T, self.Qf), (self.Xg - X[:, self.T])) return cost
def from_dcm(cls, R): assert R.shape == (3, 3) b1 = 0.5 * ca.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) b2 = 0.5 * ca.sqrt(1 + R[0, 0] - R[1, 1] - R[2, 2]) b3 = 0.5 * ca.sqrt(1 - R[0, 0] + R[1, 1] - R[2, 2]) b4 = 0.5 * ca.sqrt(1 - R[0, 0] - R[1, 1] + R[2, 2]) q1 = ca.SX(4, 1) q1[0] = b1 q1[1] = (R[2, 1] - R[1, 2]) / (4 * b1) q1[2] = (R[0, 2] - R[2, 0]) / (4 * b1) q1[3] = (R[1, 0] - R[0, 1]) / (4 * b1) q2 = ca.SX(4, 1) q2[0] = (R[2, 1] - R[1, 2]) / (4 * b2) q2[1] = b2 q2[2] = (R[0, 1] + R[1, 0]) / (4 * b2) q2[3] = (R[0, 2] + R[2, 0]) / (4 * b2) q3 = ca.SX(4, 1) q3[0] = (R[0, 2] - R[2, 0]) / (4 * b3) q3[1] = (R[0, 1] + R[1, 0]) / (4 * b3) q3[2] = b3 q3[3] = (R[1, 2] + R[2, 1]) / (4 * b3) q4 = ca.SX(4, 1) q4[0] = (R[1, 0] - R[0, 1]) / (4 * b4) q4[1] = (R[0, 2] + R[2, 0]) / (4 * b4) q4[2] = (R[1, 2] + R[2, 1]) / (4 * b4) q4[3] = b4 q = ca.if_else( ca.trace(R) > 0, q1, ca.if_else(ca.logic_and(R[0, 0] > R[1, 1], R[0, 0] > R[2, 2]), q2, ca.if_else(R[1, 1] > R[2, 2], q3, q4))) return q
def create_plan_pc(b0, BF, dt, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=state), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Final coordinate x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']] x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']] dx_bc = x_bN - x_cN # Final velocity v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']] v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']] dv_bc = v_bN - v_cN # Angle between gaze and ball velocity theta = V['X',N_sim,'theta'] phi = V['X',N_sim,'phi'] d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ]) r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']] r_unit = r / (ca.norm_2(r) + 1e-2) d_gaze = d - r_unit # Regularize controls J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs bk = cat.struct_SX(belief) bk['S'] = b0['S'] g, lbg, ubg = [], [], [] for k in range(N_sim): # Belief propagation bk['m'] = V['X',k] [bk_next] = BF([ bk, V['U',k] ]) bk_next = belief(bk_next) # simplify indexing # Multiple shooting g.append(bk_next['m'] - V['X',k+1]) lbg.append(ca.DMatrix.zeros(nx)) ubg.append(ca.DMatrix.zeros(nx)) # Control constraints g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi'])) lbg.append(-ca.inf) ubg.append(ca.DMatrix([0])) # Advance time bk = bk_next g = ca.vertcat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) # Simple cost J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity #J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity J += 1e1 * ca.trace(bk_next['S']) # uncertainty # log-probability cost #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']] #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # State constraints # catcher can look within the upper hemisphere lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max # Control constraints # 0 <= F lbx['U',:,'F'] = 0 # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # -pi <= psi <= pi lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi # m(t=0) = m0 lbx['X',0] = ubx['X',0] = b0['m'] # Take care of the time #lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf # Simulation ends when the ball touches the ground #lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0 # Solve the NLP sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) return V(sol['x'])
def test_mapaccum(self): x = SX.sym("x", 2) y = SX.sym("y") z = SX.sym("z", 2, 2) v = SX.sym("v", Sparsity.upper(3)) fun = Function("f", [x, y, z, v], [mtimes(z, x) + y, sin(y * x).T, v / y]) n = 2 X = MX.sym("x", x.sparsity()) Y = [MX.sym("y", y.sparsity()) for i in range(n)] Z = [MX.sym("z", z.sparsity()) for i in range(n)] V = [MX.sym("v", v.sparsity()) for i in range(n)] np.random.seed(0) X_ = DM(x.sparsity(), np.random.random(x.nnz())) Y_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Y] Z_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Z] V_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in V] for ad_weight in range(2): for ad_weight_sp in range(2): F = fun.mapaccum("map", n, [0], [0], {"ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight}) F.forward(2) XP = X Y0s = [] Y1s = [] Xps = [] for k in range(n): XP, Y0, Y1 = fun(XP, Y[k], Z[k], V[k]) Y0s.append(Y0) Y1s.append(Y1) Xps.append(XP) Fref = Function( "f", [X, horzcat(*Y), horzcat(*Z), horzcat(*V)], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Y1s)] ) inputs = [X_, horzcat(*Y_), horzcat(*Z_), horzcat(*V_)] for f in [F, toSX_fun(F)]: self.checkfunction(f, Fref, inputs=inputs) self.check_codegen(f, inputs=inputs) fun = Function("f", [y, x, z, v], [mtimes(z, x) + y + c.trace(v) ** 2, sin(y * x).T, v / y]) for ad_weight in range(2): for ad_weight_sp in range(2): F = fun.mapaccum("map", n, [1, 3], [0, 2], {"ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight}) XP = X VP = V[0] Y0s = [] Y1s = [] Xps = [] Vps = [] for k in range(n): XP, Y0, VP = fun(Y[k], XP, Z[k], VP) Y0s.append(Y0) Xps.append(XP) Vps.append(VP) Fref = Function("f", [horzcat(*Y), X, horzcat(*Z), V[0]], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Vps)]) inputs = [horzcat(*Y_), X_, horzcat(*Z_), V_[0]] for f in [F, toSX_fun(F)]: self.checkfunction(f, Fref, inputs=inputs) self.check_codegen(f, inputs=inputs)
def _create_running_uncertainty_cost(self, w_S): running_uncertainty_cost = 0.5 * w_S * ca.trace(self.b['S']) * self.dt op = {'input_scheme': ['b'], 'output_scheme': ['cS']} return ca.SXFunction('Running uncertainty cost', [self.b], [running_uncertainty_cost], op)
def _create_final_uncertainty_cost(self, w_Sl): final_uncertainty_cost = 0.5 * w_Sl * ca.trace(self.b['S']) op = {'input_scheme': ['b'], 'output_scheme': ['cSl']} return ca.SXFunction('Final uncertainty cost', [self.b], [final_uncertainty_cost], op)
def create_plan_pc(b0, N_sim): # Degrees of freedom for the optimizer V = cat.struct_symSX([ ( cat.entry('X',repeat=N_sim+1,struct=state), cat.entry('U',repeat=N_sim,struct=control) ) ]) # Final means m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']] m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']] dm_bc = m_bN - m_cN # Regularize controls J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and running costs bk = cat.struct_SX(belief) bk['S'] = b0['S'] g = [] lbg = [] ubg = [] for k in range(N_sim): # Belief propagation bk['m'] = V['X',k] [bk_next] = BF([ bk, V['U',k] ]) bk_next = belief(bk_next) # simplify indexing # Multiple shooting g.append(bk_next['m'] - V['X',k+1]) lbg.append(ca.DMatrix.zeros(nx)) ubg.append(ca.DMatrix.zeros(nx)) # Control constraints g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi'])) lbg.append(-ca.inf) ubg.append(ca.DMatrix([0])) # Advance time bk = bk_next g = ca.vertcat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) # Simple cost J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S'])) # log-probability cost #Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']] #J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb))) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) # Create solver opts = {} opts['linear_solver'] = 'ma97' #opts['hessian_approximation'] = 'limited-memory' solver = ca.NlpSolver('solver', 'ipopt', nlp, opts) # Define box constraints lbx = V(-ca.inf) ubx = V(ca.inf) # 0 <= v lbx['U',:,'v'] = 0 # -w_max <= w <= w_max lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max # -pi <= psi <= pi lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi # m(t=0) = m0 lbx['X',0] = ubx['X',0] = b0['m'] # Solve the NLP sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) return V(sol['x'])
def log(cls, R): # TODO theta = ca.arccos((ca.trace(R) - 1) / 2) return vee(cls.C3(theta) * (R - R.T))
def gp_exact_moment(invK, X, Y, hyper, inputmean, inputcov): """ Gaussian Process with Exact Moment Matching Copyright (c) 2018, Eric Bradford, Helge-André Langåker The first and second moments are used to compute the mean and covariance of the posterior distribution with a stochastic input distribution. This assumes a zero prior mean function and the squared exponential kernel. # Arguments invK: Array with the inverse covariance matrices of size (Ny x N x N), with Ny number of outputs from the GP and N number of training points. X: Training data matrix with inputs of size NxNx, with Nx number of inputs to the GP. Y: Training data matrix with outpyts of size (N x Ny). hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn]. inputmean: Mean from the last GP iteration of size (1 x Nx) inputcov: Covariance matrix from the last GP iteration of size (Nx x Nx) # Returns mean: Array of the output mean of size (Ny x 1). covariance: Covariance matrix of size (Ny x Ny). """ hyper = ca.log(hyper) Ny = len(invK) N, Nx = ca.MX.size(X) mean = ca.MX.zeros(Ny, 1) beta = ca.MX.zeros(N, Ny) log_k = ca.MX.zeros(N, Ny) v = X - ca.repmat(inputmean, N, 1) covariance = ca.MX.zeros(Ny, Ny) #TODO: Fix that LinsolQr don't work with the extended graph? A = ca.SX.sym('A', inputcov.shape) [Q, R2] = ca.qr(A) determinant = ca.Function('determinant', [A], [ca.exp(ca.trace(ca.log(R2)))]) for a in range(Ny): beta[:, a] = ca.mtimes(invK[a], Y[:, a]) iLambda = ca.diag(ca.exp(-2 * hyper[a, :Nx])) R = inputcov + ca.diag(ca.exp(2 * hyper[a, :Nx])) iR = ca.mtimes(iLambda, (ca.MX.eye(Nx) - ca.solve((ca.MX.eye(Nx) + ca.mtimes(inputcov, iLambda)), (ca.mtimes(inputcov, iLambda))))) T = ca.mtimes(v, iR) c = ca.exp(2 * hyper[a, Nx]) / ca.sqrt(determinant(R)) \ * ca.exp(ca.sum2(hyper[a, :Nx])) q2 = c * ca.exp(-ca.sum2(T * v) * 0.5) qb = q2 * beta[:, a] mean[a] = ca.sum1(qb) t = ca.repmat(ca.exp(hyper[a, :Nx]), N, 1) v1 = v / t log_k[:, a] = 2 * hyper[a, Nx] - ca.sum2(v1 * v1) * 0.5 # covariance with noisy input for a in range(Ny): ii = v / ca.repmat(ca.exp(2 * hyper[a, :Nx]), N, 1) for b in range(a + 1): R = ca.mtimes(inputcov, ca.diag(ca.exp(-2 * hyper[a, :Nx]) + ca.exp(-2 * hyper[b, :Nx]))) + ca.MX.eye(Nx) t = 1.0 / ca.sqrt(determinant(R)) ij = v / ca.repmat(ca.exp(2 * hyper[b, :Nx]), N, 1) Q = ca.exp(ca.repmat(log_k[:, a], 1, N) + ca.repmat(ca.transpose(log_k[:, b]), N, 1) + maha(ii, -ij, ca.solve(R, inputcov * 0.5), N)) A = ca.mtimes(beta[:, a], ca.transpose(beta[:, b])) if b == a: A = A - invK[a] A = A * Q covariance[a, b] = t * ca.sum2(ca.sum1(A)) covariance[b, a] = covariance[a, b] covariance[a, a] = covariance[a, a] + ca.exp(2 * hyper[a, Nx]) covariance = covariance - ca.mtimes(mean, ca.transpose(mean)) return [mean, covariance]
def rotationAngle(mx_R): acosarg = (casadi.trace(mx_R) - 1.) / 2. return casadi.if_else(casadi.fabs(acosarg) >= 1., 0., casadi.acos(acosarg))
def rotation_to_axis(R): vec = cs.vertcat(R[3, 2] - R[2, 3], R[1, 3] - R[3, 1], R[2, 1] - R[1, 2]) ang_core = (cs.trace(R) - 1.0) / 2.0 denom = 1.0 / (2 * (cs.sqrt(1 - ang_core))) return cs.if_else(ang_core == 1.0, cs.vertcat(1.0, 0.0, 0.0), vec * denom)
def trace(inputobj): return ca.trace(inputobj)
def cost_func_BSP(self, robot, U, X0, cov_X0): cost = 0 U = c.reshape(U, robot.nu * self.N, self.T) X = c.MX(robot.nx * self.N, self.T + 1) X[:, 0] = np.reshape(X0, (robot.nx * self.N, )) for i in range(self.T): for n in range(self.N): X[robot.nx * n:robot.nx * (n + 1), i + 1] = robot.kinematics( X[robot.nx * n:robot.nx * (n + 1), i], U[robot.nu * n, i], U[robot.nu * n + 1, i], U[robot.nu * n + 2, i]) X_temp = X[robot.nx * n:robot.nx * (n + 1), i + 1] if i == 0: P_temp = cov_X0[robot.nx * n:robot.nx * (n + 1), :] if n == 0: P = c.MX(robot.nx * self.N, robot.nx) else: P_temp = P[robot.nx * n:robot.nx * (n + 1), :] M = c.MX(robot.nx, robot.nx) M[0, 0] = (X_temp[0] - 3)**2 #1/(c.mtimes(X[0,i+1],X[0,i+1])+1); M[0, 1] = 0 M[0, 2] = 0 M[1, 0] = 0 M[1, 1] = 1 M[1, 2] = 0 M[2, 0] = 0 M[2, 1] = 0 M[2, 2] = 1 #M = c.DM.eye(robot.nx) Sigma_w = (self.epsilon**2) * self.Sigma_w Sigma_nu = (self.epsilon**2) * self.Sigma_nu P_temp = P_temp + c.mtimes(c.mtimes(robot.G, Sigma_w), robot.G.T) S = P_temp + c.mtimes(c.mtimes(M, Sigma_nu), M.T) K = c.mtimes(P_temp, c.inv(S)) P_temp = c.mtimes(c.DM.eye(robot.nx) - K, P_temp) P[robot.nx * n:robot.nx * (n + 1), :] = P_temp cost = cost + self.gamma*c.trace(c.mtimes(c.mtimes(self.Q,P_temp),self.Q.T)) + \ c.mtimes(c.mtimes(U[robot.nu*n:robot.nu*(n+1),i].T,self.R),U[robot.nu*n:robot.nu*(n+1),i]) if params.OBSTACLES: obstacle_cost = self.obstacle_cost_func(X_temp[0:2]) cost = cost + obstacle_cost if self.N > 1: inter_agent_cost = self.inter_agent_cost_func(robot, X[:, i]) cost = cost + inter_agent_cost for n in range(self.N): cost = cost + c.mtimes( c.mtimes((self.Xg[n, :] - X[robot.nx * n:robot.nx * (n + 1), self.T]).T, self.Qf), (self.Xg[n, :] - X[robot.nx * n:robot.nx * (n + 1), self.T])) return cost
def test_mapaccum(self): x = SX.sym("x", 2) y = SX.sym("y") z = SX.sym("z", 2, 2) v = SX.sym("v", Sparsity.upper(3)) fun = Function( "f", [x, y, z, v], [mtimes(z, x) + y, sin(y * x).T, v / y]) n = 2 X = MX.sym("x", x.sparsity()) Y = [MX.sym("y", y.sparsity()) for i in range(n)] Z = [MX.sym("z", z.sparsity()) for i in range(n)] V = [MX.sym("v", v.sparsity()) for i in range(n)] np.random.seed(0) X_ = DM(x.sparsity(), np.random.random(x.nnz())) Y_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Y] Z_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in Z] V_ = [DM(i.sparsity(), np.random.random(i.nnz())) for i in V] for ad_weight in range(2): for ad_weight_sp in range(2): F = fun.mapaccum("map", n, [0], [0], { "ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight }) F.forward(2) XP = X Y0s = [] Y1s = [] Xps = [] for k in range(n): XP, Y0, Y1 = fun(XP, Y[k], Z[k], V[k]) Y0s.append(Y0) Y1s.append(Y1) Xps.append(XP) Fref = Function("f", [ X, horzcat(*Y), horzcat(*Z), horzcat(*V) ], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Y1s)]) inputs = [X_, horzcat(*Y_), horzcat(*Z_), horzcat(*V_)] for f in [F, toSX_fun(F)]: self.checkfunction(f, Fref, inputs=inputs) self.check_codegen(f, inputs=inputs) fun = Function("f", [y, x, z, v], [mtimes(z, x) + y + c.trace(v)**2, sin(y * x).T, v / y]) for ad_weight in range(2): for ad_weight_sp in range(2): F = fun.mapaccum("map", n, [1, 3], [0, 2], { "ad_weight_sp": ad_weight_sp, "ad_weight": ad_weight }) XP = X VP = V[0] Y0s = [] Y1s = [] Xps = [] Vps = [] for k in range(n): XP, Y0, VP = fun(Y[k], XP, Z[k], VP) Y0s.append(Y0) Xps.append(XP) Vps.append(VP) Fref = Function( "f", [horzcat(*Y), X, horzcat(*Z), V[0]], [horzcat(*Xps), horzcat(*Y0s), horzcat(*Vps)]) inputs = [horzcat(*Y_), X_, horzcat(*Z_), V_[0]] for f in [F, toSX_fun(F)]: self.checkfunction(f, Fref, inputs=inputs) self.check_codegen(f, inputs=inputs)
for i in range(np.shape(xp_opt)[1] - 1): xp_r = np.reshape(xp_opt[:, i + 1], (2, 4)) # vv = np.zeros([ntheta[0], ntheta[0], 1 + N]) # for i in range(0, 1 + N): FIM_t += [xp_r.T @ np.linalg.inv(np.array([[0.01, 0], [0, 0.05]])) @ xp_r] obsFIM = [] for i in range(np.shape(FIM_t)[0]): obsFIM += [ np.linalg.inv((((10.0 - 0.001)**2) / 12) * np.identity(n_theta)) + sum(FIM_t[:i + 1]) ] for i in range(np.shape(xp_opt)[1] - 1): FIM1_det += [2 * cad.sum1(cad.log(cad.diag(cad.chol(obsFIM[i]))))] FIM1_trace += [cad.trace(obsFIM[i])] FIM_det += [2 * np.sum(np.log(np.diag(np.linalg.cholesky(obsFIM[i]))))] FIM_trace += [np.trace(obsFIM[i])] FIM1_mineigv += [min(np.linalg.eig(obsFIM[i])[0])] FIM1_maxeigv += [max(np.linalg.eig(obsFIM[i])[0])] V_theta = np.linalg.inv(sum(FIM_t[i] for i in range(len(FIM_t)))) theta1 = w_opt[n_x + n_x * n_theta:n_x + n_x * n_theta + n_theta] t_value = np.zeros(n_theta) for i in range(n_theta): t_value[i] = theta1[i] / np.sqrt(V_theta[i, i]) FIM1_trace_array = np.array(FIM1_trace) FIM1_det_array = np.array(FIM1_det) FIM1_maxeigv_array = np.array(FIM1_maxeigv)
def gp_taylor_approx(invK, X, Y, hyper, inputmean, inputcovar, meanFunc='zero', diag=False, log=False): """ Gaussian Process with Taylor Approximation Copyright (c) 2018, Helge-André Langåker This uses a first order taylor for the mean evaluation (a normal GP mean), and a second order taylor for estimating the variance. # Arguments invK: Array with the inverse covariance matrices of size (Ny x N x N), with Ny number of outputs from the GP and N number of training points. X: Training data matrix with inputs of size NxNx, with Nx number of inputs to the GP. Y: Training data matrix with outpyts of size (N x Ny). hyper: Array with hyperparameters [ell_1 .. ell_Nx sf sn]. inputmean: Mean from the last GP iteration of size (1 x Nx) inputvar: Variance from the last GP iteration of size (1 x Ny) # Returns mean: Array with estimated mean of size (Ny x 1). covariance: The estimated covariance matrix with the output variance in the diagonal of size (Ny x Ny). """ if log: X = ca.log(X) Y = ca.log(Y) inputmean = ca.log(inputmean) Ny = len(invK) N, Nx = ca.MX.size(X) mean = ca.MX.zeros(Ny, 1) var = ca.MX.zeros(Nx, 1) v = X - ca.repmat(inputmean, N, 1) covar_temp = ca.MX.zeros(Ny, Ny) covariance = ca.MX.zeros(Ny, Ny) d_mean = ca.MX.zeros(Ny, 1) dd_var = ca.MX.zeros(Ny, Ny) # Casadi symbols x_s = ca.SX.sym('x', Nx) z_s = ca.SX.sym('z', Nx) ell_s = ca.SX.sym('ell', Nx) sf2_s = ca.SX.sym('sf2') covSE = ca.Function('covSE', [x_s, z_s, ell_s, sf2_s], [covSEard(x_s, z_s, ell_s, sf2_s)]) for a in range(Ny): ell = hyper[a, :Nx] w = 1 / ell**2 sf2 = ca.MX(hyper[a, Nx]**2) m = get_mean_function(hyper[a, :], inputmean, func=meanFunc) iK = ca.MX(invK[a]) alpha = ca.mtimes(iK, Y[:, a] - m(inputmean)) + m(inputmean) kss = sf2 ks = ca.MX.zeros(N, 1) for i in range(N): ks[i] = covSE(X[i, :], inputmean, ell, sf2) invKks = ca.mtimes(iK, ks) mean[a] = ca.mtimes(ks.T, alpha) var[a] = kss - ca.mtimes(ks.T, invKks) d_mean[a] = ca.mtimes(ca.transpose(w[a] * v[:, a] * ks), alpha) #BUG: This don't take into account the covariance between states for d in range(Ny): for e in range(Ny): dd_var1a = ca.mtimes(ca.transpose(v[:, d] * ks), iK) dd_var1b = ca.mtimes(dd_var1a, v[e] * ks) dd_var2 = ca.mtimes(ca.transpose(v[d] * v[e] * ks), invKks) dd_var[d, e] = -2 * w[d] * w[e] * (dd_var1b + dd_var2) if d == e: dd_var[d, e] = dd_var[d, e] + 2 * w[d] * (kss - var[d]) mean_mat = ca.mtimes(d_mean, d_mean.T) covar_temp[0, 0] = inputcovar[a, a] covariance[a, a] = var[a] + ca.trace(ca.mtimes(covar_temp, .5 * dd_var + mean_mat)) return [mean, covariance]
def get_term_cost_func(x, p, Cs, x_target): P = cs.reshape(p, 11, 11) cost = 0.5 * cs.trace(P @ Cs) cost += 0.5 * cs.dot(x[:6] - x_target, Cs[:6, :6] @ (x[:6] - x_target)) c_term = cs.Function('c_term', [x, p], [cost], ['x', 'p'], ['cost']) return c_term
def rotation_to_angle(R): # Source: # https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation return cs.arccos((cs.trace(R) - 1.0) / 2.0)