def set_data(self, data, weights=None): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.boundary_species, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.boundary_species] # Rename columns with state indicies df.columns = np.arange(self.nx) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] if weights is None: weights = self.data.max() obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]] obj_resid = cs.sum_square(cs.vertcat(obj_list)) self.objective_sx += obj_resid
def prior_construct(self, prior_info): Pnlp = self._Pnlp if prior_info['type'] == 'Ridge': L2 = prior_info['L2'] prior = cas.mul(Pnlp.T, Pnlp) * L2 elif prior_info['type'] == 'normal': mean = prior_info['mean'] cov = prior_info['cov'] dev = Pnlp - mean prior = cas.mul(cas.mul(dev.T, np.linalg.inv(cov)), dev) elif prior_info['type'] == 'normal_std': mean = prior_info['mean'] std = prior_info['std'] dev = Pnlp - mean prior = cas.sum_square(dev) / (2 * std**2) elif prior_info['type'] == 'uniform': prior = 0 elif prior_info['type'] == 'GP': BEmean = prior_info['BEmean'] BEcov = prior_info['BEcov'] linear_BE2Ea = prior_info['BE2Ea'] Eacov = prior_info['Eacov'] _BE = Pnlp[self._NEa:] _Ea = Pnlp[:self._NEa] Eamean = cas.mul(linear_BE2Ea, _BE) dev_BE = _BE - BEmean dev_Ea = _Ea - Eamean prior = cas.mul(cas.mul(dev_BE.T, np.linalg.inv(BEcov)), dev_BE) + \ cas.mul(cas.mul(dev_Ea.T, np.linalg.inv(Eacov)), dev_Ea) self._prior_ = prior return prior
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 _set_objective_from_data(self, data, weights): obj_list = [] for ((ti, state), xi) in data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]] obj_resid = cs.sum_square(cs.vertcat(obj_list)) self.objective_sx += obj_resid
def create_simple_plan(x0, 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) ) ]) # Objective function 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 J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN # Regularize controls J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang # Multiple shooting constraints and non-linear control constraints g = [] for k in range(N_sim): # Multiple shooting [x_next] = F([V['X',k], V['U',k], dt]) g.append(x_next - V['X',k+1]) g = ca.vertcat(g) # Formulate the NLP nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g)) opts = {} #opts['hessian_approximation'] = 'limited-memory' opts['linear_solver'] = 'ma57' # Create a solver 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] = ubx['X',0] = x0 # Solve the NLP sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx) return V(sol['x'])
def set_data(self, data): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.state_names, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.state_names] # Rename columns with state indicies df.columns = np.arange(self.NEQ) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [ (self._get_interp(ti, [state]) - xi) / self.data[state].max() ] obj_resid = cs.sum_square(cs.vertcat(obj_list)) # ts = data.index # Define objective function # obj_resid = cs.sum_square(cs.vertcat( # [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0) # for ti, xi in zip(ts, xs)])) alpha = cs.SX.sym('alpha') obj_lasso = alpha * cs.sumRows(cs.fabs(self._P)) self._obj = obj_resid + obj_lasso # Create the solver object self._nlp = cs.SXFunction('nlp', cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
def set_data(self, data): """ Attach experimental measurement data. data : a pd.DataFrame object Data should have columns corresponding to the state labels in self.state_names, with an index corresponding to the measurement times. """ # Should raise an error if no state name is present df = data.loc[:, self.state_names] # Rename columns with state indicies df.columns = np.arange(self.NEQ) # Remove empty (nonmeasured) states self.data = df.loc[:, ~pd.isnull(df).all(0)] obj_list = [] for ((ti, state), xi) in self.data.stack().iteritems(): obj_list += [(self._get_interp(ti, [state]) - xi) / self.data[state].max()] obj_resid = cs.sum_square(cs.vertcat(obj_list)) # ts = data.index # Define objective function # obj_resid = cs.sum_square(cs.vertcat( # [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0) # for ti, xi in zip(ts, xs)])) alpha = cs.SX.sym("alpha") obj_lasso = alpha * cs.sumRows(cs.fabs(self._P)) self._obj = obj_resid + obj_lasso # Create the solver object self._nlp = cs.SXFunction("nlp", cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
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'])
# 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']] ????? delta = ca.arctan2(ca.norm_2(ca.cross())) r_unit = r / (ca.norm_2(r) + 1e-2) d_gaze = d - r_unit # Regularize controls J = 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # Multiple shooting constraints and non-linear control constraints g, lbg, ubg = [], [], [] for k in range(N_sim): # Multiple shooting [x_next] = F([V['X',k], V['U',k], dt]) g.append(x_next - 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])) g = ca.vertcat(g)
import numpy as np from optistack import * import casadi as C A = optivar(3, 3) # inverse B = np.array([[1, 0, -2], [3, 1, -1], [2, 1, 0]]) optisolve(C.sum_square(mtimes(A, B) - np.eye(3))) As = optival(A) assert (np.max(np.max(np.fabs(As - np.linalg.inv(B))) < 1e-7)) A = optivar(4, 3) # moore-penrose pseudo-inverse B = np.array([[1, 0, -2, 3], [3, 1, -1, 1], [2, 1, 0, 9]]) optisolve(C.sum_square(mtimes(A, B) - np.eye(4))) As = optival(A) assert (np.max(np.max(np.fabs(As - np.linalg.pinv(B))) < 1e-7))
import numpy as np from optistack import * import casadi as C A = optivar(3,3) # inverse B = np.array([[1, 0, -2],[3, 1, -1],[2, 1, 0]]) optisolve(C.sum_square(mtimes(A,B)-np.eye(3))) As = optival(A) assert(np.max(np.max(np.fabs(As-np.linalg.inv(B)))<1e-7)) A = optivar(4,3); # moore-penrose pseudo-inverse B = np.array([[1, 0, -2, 3],[ 3, 1, -1, 1],[2, 1, 0, 9]]) optisolve(C.sum_square(mtimes(A,B)-np.eye(4))) As = optival(A) assert(np.max(np.max(np.fabs(As-np.linalg.pinv(B)))<1e-7))
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'])