def __init__(self, dae, N=None, ts=None, yxNames=None, yuNames=None): Ocp.__init__(self, dae, N=N, ts=ts) self.hashPrefix = 'mhe' if yxNames is None: yxNames = dae.xNames() if yuNames is None: yuNames = dae.uNames() if not isinstance(yxNames, list): raise Exception("If you decide to provide measurements, "+\ "you have to provide them as a list of strings") if not isinstance(yuNames, list): raise Exception("If you decide to provide end measurements, "+\ "you have to provide them as a list of strings") self._yxNames = yxNames self._yuNames = yuNames self._yx = C.veccat([self[n] for n in self.yxNames]) self._yu = C.veccat([self[n] for n in self.yuNames]) assert not C.dependsOn( self.yx, self.dae.uVec()), "error: x measurement depends on u" assert not C.dependsOn( self.yu, self.dae.xVec()), "error: u measurement depends on x" Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu])) Ocp.minimizeLsqEndTerm(self, self.yx)
def _get_subst_set(self, **kwargs): subst_from = [] subst_to = [] if "t" in kwargs: subst_from.append(self.t) subst_to.append(kwargs["t"]) if "x" in kwargs: subst_from.append(self.x) subst_to.append(kwargs["x"]) if "z" in kwargs: subst_from.append(self.z) subst_to.append(kwargs["z"]) if "xq" in kwargs: subst_from.append(self.xq) subst_to.append(kwargs["xq"]) if "u" in kwargs: subst_from.append(self.u) subst_to.append(kwargs["u"]) if "p" in kwargs and self.parameters['']: p = veccat(*self.parameters['']) subst_from.append(p) subst_to.append(kwargs["p"]) if "p_control" in kwargs and self.parameters['control']: p = veccat(*self.parameters['control']) subst_from.append(p) subst_to.append(kwargs["p_control"]) if "v" in kwargs and self.variables['']: v = veccat(*self.variables['']) subst_from.append(v) subst_to.append(kwargs["v"]) if "v_control" in kwargs and self.variables['control']: v = veccat(*self.variables['control']) subst_from.append(v) subst_to.append(kwargs["v_control"]) return (subst_from, subst_to)
def initial_guess(self, t, tf_init, params): x_guess = self.xd() inclination = 30.0 * ca.pi / 180.0 # the other angle than the one you're thinking of dcmInclination = np.array( [[ca.cos(inclination), 0.0, -ca.sin(inclination)], [0.0, 1.0, 0.0], [ca.sin(inclination), 0.0, ca.cos(inclination)]]) dtheta = 2.0 * ca.pi / tf_init theta = t * dtheta r = 0.25 * params['l'] angle = ca.SX.sym('angle') x_cir = ca.sqrt(params['l']**2 - r**2) y_cir = r * ca.cos(angle) z_cir = r * ca.sin(angle) init_pos_fun = ca.Function( 'init_pos', [angle], [ca.mtimes(dcmInclination, ca.veccat(x_cir, y_cir, z_cir))]) init_vel_fun = init_pos_fun.jacobian() ret = {} ret['q'] = init_pos_fun(theta) ret['dq'] = init_vel_fun(theta, 0) * dtheta ret['w'] = ca.veccat(0.0, 0.0, dtheta) norm_vel = ca.norm_2(ret['dq']) norm_pos = ca.norm_2(ret['q']) R0 = ret['dq'] / norm_vel R2 = ret['q'] / norm_pos R1 = ca.cross(ret['q'] / norm_pos, ret['dq'] / norm_vel) ret['R'] = ca.vertcat(R0.T, R1.T, R2.T).T return ret
def _ode(self): ode = veccat(*[self._state_der[k] for k in self.states]) quad = veccat(*[self._state_der[k] for k in self.qstates]) alg = veccat(*self._alg) return Function('ode', [self.x, self.u, self.z, self.p, self.t], [ode, alg, quad], ["x", "u", "z", "p", "t"], ["ode", "alg", "quad"])
def _create_belief_nonlinear_constraints(model, V): """Non-linear constraints for planning""" bk = cat.struct_SX(model.b) bk['S'] = model.b0['S'] g, lbg, ubg = [], [], [] for k in range(model.n): # Belief propagation bk['m'] = V['X', k] [bk_next] = model.BF([bk, V['U', k]]) bk_next = model.b(bk_next) # Multiple shooting g.append(bk_next['m'] - V['X', k+1]) lbg.append(ca.DMatrix.zeros(model.nx)) ubg.append(ca.DMatrix.zeros(model.nx)) # Control constraints constraint_k = model._set_constraint(V, k) g.append(constraint_k) lbg.append(-ca.inf) ubg.append(0) # Advance time bk = bk_next g = ca.veccat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) return [g, lbg, ubg]
def getSteadyStateNlpFunctions(self, dae, ref_dict = {}): # make steady state model g = Constraints() g.add(dae.getResidual(), '==', 0, tag = ('dae residual', None)) def constrainInvariantErrs(): R_c2b = dae['R_c2b'] self.makeOrthonormal(g, R_c2b) g.add(dae['c'], '==', 0, tag = ('c(0) == 0', None)) g.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None)) constrainInvariantErrs() # Rotational velocity time derivative # NOTE: In the following constraint omega0 was hard-coded. g.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag = ("Rotational velocities", None)) for name in ['alpha_deg', 'beta_deg', 'cL']: if name in ref_dict: g.addBnds(dae[name], ref_dict[name], tag = (name, None)) dvs = C.veccat([dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec(), dae.xDotVec()]) obj = 0 # for name in ['aileron', 'elevator', 'rudder', 'flaps']: for name in dae.uNames(): if name in dae: obj += dae[ name ] ** 2 return dvs, obj, g.getG(), g.getLb(), g.getUb()
def __init__(self, dae, N=None, ts=None, yxNames=None, yuNames=None): Ocp.__init__(self, dae, N=N, ts=ts) self.hashPrefix = "mhe" if yxNames is None: yxNames = dae.xNames() if yuNames is None: yuNames = dae.uNames() if not isinstance(yxNames, list): raise Exception("If you decide to provide measurements, " + "you have to provide them as a list of strings") if not isinstance(yuNames, list): raise Exception( "If you decide to provide end measurements, " + "you have to provide them as a list of strings" ) self._yxNames = yxNames self._yuNames = yuNames self._yx = C.veccat([self[n] for n in self.yxNames]) self._yu = C.veccat([self[n] for n in self.yuNames]) assert not C.dependsOn(self.yx, self.dae.uVec()), "error: x measurement depends on u" assert not C.dependsOn(self.yu, self.dae.xVec()), "error: u measurement depends on x" Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu])) Ocp.minimizeLsqEndTerm(self, self.yx)
def get_chain_mass_ode(num_masses): x = casadi.SX.sym('x', 3, num_masses) dot_x = casadi.SX.sym('dot_x', 3, num_masses) x_end = casadi.SX.sym('x_end', 3) u = casadi.SX.sym('u', 3) y = casadi.veccat(x, dot_x, x_end) l = 0.033 d = 1.0 m = 0.03 g = [0, 0, -9.81] def get_f(x_i, x_ip1): x_diff = x_ip1 - x_i return d * (1 - l / casadi.norm_2(x_diff)) * x_diff ddot_x = casadi.SX.zeros(3, num_masses) for i in range(0, num_masses): if i == 0: x_real_end = x[:, i + 1] if num_masses > 1 else x_end ddot_x[:, i] = 1. / m * (get_f(x[:, i], x_real_end) - get_f(CHAIN_X0, x[:, i])) + g elif i < num_masses - 1: ddot_x[:, i] = 1. / m * (get_f(x[:, i], x[:, i + 1]) - get_f(x[:, i - 1], x[:, i])) + g else: ddot_x[:, i] = 1. / m * (get_f(x[:, i], x_end) - get_f(x[:, i - 1], x[:, i])) + g dot_y = casadi.veccat(dot_x, ddot_x, u) assert y.shape == dot_y.shape return u, y, dot_y
def initial_residual_function(self): if hasattr(self, '_states_vector'): return ca.Function('initial_residual', [self.time, self._states_vector, self._der_states_vector, self._alg_states_vector, self._inputs_vector, ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters))], [ca.veccat(*self.initial_equations)] if len(self.initial_equations) > 0 else []) else: return ca.Function('initial_residual', [self.time, ca.veccat(*self._symbols(self.states)), ca.veccat(*self._symbols(self.der_states)), ca.veccat(*self._symbols(self.alg_states)), ca.veccat(*self._symbols(self.inputs)), ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters))], [ca.veccat(*self.initial_equations)] if len(self.initial_equations) > 0 else [])
def variable_metadata_function(self): in_var = ca.veccat(*self._symbols(self.parameters)) out = [] is_affine = True zero, one = ca.MX(0), ca.MX( 1) # Recycle these common nodes as much as possible. for variable_list in [ self.states, self.alg_states, self.inputs, self.parameters, self.constants ]: attribute_lists = [[] for i in range(len(ast.Symbol.ATTRIBUTES))] for variable in variable_list: for attribute_list_index, attribute in enumerate( ast.Symbol.ATTRIBUTES): value = ca.MX(getattr(variable, attribute)) if value.is_zero(): value = zero elif value.is_one(): value = one value = value if value.numel() != 1 else ca.repmat( value, *variable.symbol.size()) attribute_lists[attribute_list_index].append(value) expr = ca.horzcat(*[ ca.veccat(*attribute_list) for attribute_list in attribute_lists ]) if len(self.parameters) > 0 and isinstance(expr, ca.MX): f = ca.Function('f', [in_var], [expr]) contains_if_else = ca.OP_IF_ELSE_ZERO in [ f.instruction_id(k) for k in range(f.n_instructions()) ] zero_hessian = ca.jacobian(ca.jacobian(expr, in_var), in_var).is_zero() if contains_if_else or not zero_hessian: is_affine = False out.append(expr) if len(self.parameters) > 0 and is_affine: # Rebuild variable metadata as a single affine expression, if all # subexpressions are affine. in_var_ = ca.MX.sym('in_var', in_var.shape) out_ = [] for o in out: Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)]) bf = ca.Function('bf', [in_var], [o]) A = Af(0) A = ca.sparsify(A) b = bf(0) b = ca.sparsify(b) o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b out_.append(o_) out = out_ in_var = in_var_ return ca.Function('variable_metadata', [in_var], out)
def generateCModel(dae,ag): writer = AlgorithmWriter() inputs = C.veccat([ag['x'], ag['z'], ag['u'], ag['p'], ag['xdot']]) # dae residual f = ag['f'] rhs = C.SXFunction( [inputs], [f] ) rhs.init() # handle time scaling [f] = rhs.eval([C.veccat([ag['x'], ag['z'], ag['u'], ag['p'], ag['xdot']/ag['timeScaling']])]) rhs = C.SXFunction( [inputs], [f] ) rhs.init() rhs_string = [writer.writePrototype('rhs')] rhs_string.extend(writer.convertAlgorithm(rhs)) rhs_string.append('}') # dae residual jacobian jf = C.veccat( [ C.jacobian(f,inputs).T ] ) rhs_jacob = C.SXFunction( [inputs], [jf] ) rhs_jacob.init() rhs_jacob_string = [writer.writePrototype('rhs_jac')] rhs_jacob_string.extend(writer.convertAlgorithm(rhs_jacob)) rhs_jacob_string.append('}') # outputs o = C.veccat( [dae[outname] for outname in dae.outputNames()] ) outputs = C.SXFunction( [inputs], [o] ) outputs.init() outputs_string = [writer.writePrototype('out')] outputs_string.extend(writer.convertAlgorithm(outputs)) outputs_string.append('}') # outputs jacobian jo = C.veccat( [ C.jacobian(o,inputs).T ] ) outputs_jacob = C.SXFunction( [inputs], [jo] ) outputs_jacob.init() outputs_jacob_string = [writer.writePrototype('out_jac')] outputs_jacob_string.extend(writer.convertAlgorithm(outputs_jacob)) outputs_jacob_string.append('}') # model file modelFile = ['#include "acado.h"'] modelFile.append('') modelFile.extend(rhs_string) modelFile.append('') modelFile.extend(rhs_jacob_string) modelFile.append('') modelFile.append('') modelFile.extend(outputs_string) modelFile.append('') modelFile.extend(outputs_jacob_string) return {'modelFile':'\n'.join(modelFile), 'rhs':rhs, 'rhsJacob':rhs_jacob}
def dynamics(self, aero, params): ScalePower = params['ScalePower'] scale = 1000. xd = self.xd u = self.u xa = self.xa p = self.p puni = self.puni l = self.l m = params['mK'] + 1. / 3 * params['mT'] #mass of kite and tether g = params['g'] # gravity A = params['sref'] # area of Kite J = params['J'] # Kite Inertia xddot = ca.struct_symSX( [ca.entry('d' + name, shape=xd[name].shape) for name in xd.keys()]) [q, dq, R, w, coeff, E, Drag] = xd[...] (Fa, M, F_tether_scaled, F_drag, F_gravity, Tether_drag), outputs = aero(xd, xa, p, puni, l, params) wx = skew(w[0], w[1], w[2]) # creating a skrew matrix of the angular velocities # Rdot = R*w (whereas w is in the skrew symmetric form) Rconstraint = ca.reshape(xddot['dR'] - ca.mtimes(xd['R'], wx), 9, 1) # J*wdot +w x J*w = T TorqueEq = ca.mtimes( J, xddot['dw']) + (ca.cross(w.T, ca.mtimes(J, w).T).T - scale * (1 - puni['gam']) * u['T']) - puni['gam'] * M DragForce = -Drag * R[0:3] # ------------------------------------------------------------------------------------ # DYNAMICS of the system - Create a structure for the Differential-Algebraic Equation # ------------------------------------------------------------------------------------ res = ca.vertcat( xddot['dq'] - xd['dq'],\ xddot['dcoeff'] - u['dcoeff'],\ xddot['dDrag'] - u['dDrag'],\ m*(xddot['ddq'][0] + F_tether_scaled[0] - A*(1-puni['gam'])*u['u',0]) - puni['gam']*Fa[0] - F_drag[0] - Tether_drag[0], \ m*(xddot['ddq'][1] + F_tether_scaled[1] - A*(1-puni['gam'])*u['u',1]) - puni['gam']*Fa[1] - F_drag[1] - Tether_drag[1], \ m*(xddot['ddq'][2] + F_tether_scaled[2] - A*(1-puni['gam'])*u['u',2]) - puni['gam']*Fa[2] - F_drag[2] - Tether_drag[2]+ F_gravity , \ xddot['dE'] - ScalePower*ca.mtimes(F_drag.T,dq) ) res = ca.veccat(res, Rconstraint, TorqueEq) res = ca.veccat(res, ca.sum1(xd['q'] * xddot['ddq']) + ca.sum1(xd['dq']**2)) # --- System dynamics function (implicit formulation) dynamics = ca.Function('dynamics', [xd, xddot, xa, u, p, puni, l], [res]) return dynamics
def __init__(self, dae, N=None, ts=None): Ocp.__init__(self, dae, N=N, ts=ts) self.hashPrefix = "mpc" self._yxNames = dae.xNames() self._yuNames = dae.uNames() self._yx = C.veccat([self[n] for n in self.yxNames]) self._yu = C.veccat([self[n] for n in self.yuNames]) Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu])) Ocp.minimizeLsqEndTerm(self, self.yx)
def __init__(self, dae, N=None, ts=None): Ocp.__init__(self, dae, N=N, ts=ts) self.hashPrefix = 'mpc' self._yxNames = dae.xNames() self._yuNames = dae.uNames() self._yx = C.veccat([self[n] for n in self.yxNames]) self._yu = C.veccat([self[n] for n in self.yuNames]) Ocp.minimizeLsq(self, C.veccat([self.yx, self.yu])) Ocp.minimizeLsqEndTerm(self, self.yx)
def getOutputs(self, x, u, p): xVec = C.veccat([getitemMsg(x,name,"the states") for name in self.dae.xNames()]) uVec = C.veccat([getitemMsg(u,name,"the controls") for name in self.dae.uNames()]) pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in self.dae.pNames()]) self.outputsFun0.setInput(xVec, 0) self.outputsFun0.setInput(uVec, 1) self.outputsFun0.setInput(pVec, 2) self.outputsFun0.evaluate() ret = {} for k,name in enumerate(self.outputs0names): ret[name] = maybeToScalar(C.DMatrix(self.outputsFun0.output(k))) return ret
def step(self, x, u, p): xVec = C.veccat([getitemMsg(x,name,"the states") for name in self.dae.xNames()]) uVec = C.veccat([getitemMsg(u,name,"the controls") for name in self.dae.uNames()]) pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in self.dae.pNames()]) self.integrator.setInput(xVec,C.INTEGRATOR_X0) self.integrator.setInput(C.veccat([uVec,pVec]),C.INTEGRATOR_P) self.integrator.evaluate() xNext = C.DMatrix(self.integrator.output()) ret = {} for k,name in enumerate(self.dae.xNames()): ret[name] = xNext[k].at(0) return ret
def pendulum_model(nSteps=None): dae = Dae() dae.addZ( [ "ddx" , "ddz" , "tau" ] ) dae.addX( [ "x" , "z" , "dx" , "dz" ] ) dae.addU( [ "torque" ] ) dae.addP( [ "m" ] ) r = 0.3 dae.stateDotDummy = C.veccat( [C.ssym(name+"DotDummy") for name in dae._xNames] ) scaledStateDotDummy = dae.stateDotDummy if nSteps is not None: endTime = dae.addP('endTime') scaledStateDotDummy = dae.stateDotDummy/(endTime/(nSteps-1)) xDotDummy = scaledStateDotDummy[0] zDotDummy = scaledStateDotDummy[1] dxDotDummy = scaledStateDotDummy[2] dzDotDummy = scaledStateDotDummy[3] ode = [ dae.x('dx') - xDotDummy , dae.x('dz') - zDotDummy , dae.z('ddx') - dxDotDummy , dae.z('ddz') - dzDotDummy ] fx = dae.u('torque')*dae.x('z') fz = -dae.u('torque')*dae.x('x') + dae.p('m')*9.8 alg = [ dae.p('m')*dae.z('ddx') + dae.x('x')*dae.z('tau') - fx , dae.p('m')*dae.z('ddz') + dae.x('z')*dae.z('tau') - fz , dae.x('x')*dae.z('ddx') + dae.x('z')*dae.z('ddz') + (dae.x('dx')*dae.x('dx') + dae.x('dz')*dae.x('dz')) ] c = [ dae.x('x')*dae.x('x') + dae.x('z')*dae.x('z') - r*r , dae.x('dx')*dae.x('x')* + dae.x('dz')*dae.x('z') ] dae.addOutput('invariants',C.veccat(c)) dae.setAlgRes( alg ) dae.setOdeRes( ode ) return dae
def variable_metadata_function(self): in_var = ca.veccat(*self._symbols(self.parameters)) out = [] is_affine = True zero, one = ca.MX(0), ca.MX(1) # Recycle these common nodes as much as possible. for variable_list in [self.states, self.alg_states, self.inputs, self.parameters, self.constants]: attribute_lists = [[] for i in range(len(CASADI_ATTRIBUTES))] for variable in variable_list: for attribute_list_index, attribute in enumerate(CASADI_ATTRIBUTES): value = ca.MX(getattr(variable, attribute)) if value.is_zero(): value = zero elif value.is_one(): value = one value = value if value.numel() != 1 else ca.repmat(value, *variable.symbol.size()) attribute_lists[attribute_list_index].append(value) expr = ca.horzcat(*[ca.veccat(*attribute_list) for attribute_list in attribute_lists]) if len(self.parameters) > 0 and isinstance(expr, ca.MX): f = ca.Function('f', [in_var], [expr]) # NOTE: This is not a complete list of operations that can be # handled in an affine expression. That said, it should # capture the most common ways variable attributes are # expressed as a function of parameters. allowed_ops = {ca.OP_INPUT, ca.OP_OUTPUT, ca.OP_CONST, ca.OP_SUB, ca.OP_ADD, ca.OP_SUB, ca.OP_MUL, ca.OP_DIV, ca.OP_NEG} f_ops = {f.instruction_id(k) for k in range(f.n_instructions())} contains_unallowed_ops = not f_ops.issubset(allowed_ops) zero_hessian = ca.jacobian(ca.jacobian(expr, in_var), in_var).is_zero() if contains_unallowed_ops or not zero_hessian: is_affine = False out.append(expr) if len(self.parameters) > 0 and is_affine: # Rebuild variable metadata as a single affine expression, if all # subexpressions are affine. in_var_ = ca.MX.sym('in_var', in_var.shape) out_ = [] for o in out: Af = ca.Function('Af', [in_var], [ca.jacobian(o, in_var)]) bf = ca.Function('bf', [in_var], [o]) A = Af(0) A = ca.sparsify(A) b = bf(0) b = ca.sparsify(b) o_ = ca.reshape(ca.mtimes(A, in_var_), o.shape) + b out_.append(o_) out = out_ in_var = in_var_ return self._expand_mx_func(ca.Function('variable_metadata', [in_var], out))
def invariantErrs(): dcm = C.horzcat( [ C.veccat([dae.x('e11'), dae.x('e21'), dae.x('e31')]) , C.veccat([dae.x('e12'), dae.x('e22'), dae.x('e32')]) , C.veccat([dae.x('e13'), dae.x('e23'), dae.x('e33')]) ] ).trans() err = C.mul(dcm.trans(), dcm) dcmErr = C.veccat([ err[0,0]-1, err[1,1]-1, err[2,2]-1, err[0,1], err[0,2], err[1,2] ]) f = C.SXFunction( [dae.xVec(),dae.uVec(),dae.pVec()] , [dae.output('c'),dae.output('cdot'),dcmErr] ) f.setOption('name','invariant errors') f.init() return f
def _plot_arrows_3D(name, ax, x, y, phi, psi): x = ca.veccat(x) y = ca.veccat(y) z = ca.DMatrix.zeros(x.size()) phi = ca.veccat(phi) psi = ca.veccat(psi) x_vec = ca.cos(psi) * ca.cos(phi) y_vec = ca.cos(psi) * ca.sin(phi) z_vec = ca.sin(psi) ax.quiver(x + x_vec, y + y_vec, z + z_vec, x_vec, y_vec, z_vec, color='r', alpha=0.8) return [Patch(color='red', label=name)]
def setQuadratureDdt(self,quadratureStateName,quadratureStateDotName,lookup,lagrangePoly,h,symbolicDvs): if quadratureStateName in self._quadratures: raise ValueError(quadratureStateName+" is not unique") qStates = np.resize(np.array([None],dtype=C.MX),(self._nk+1,self._nicp,self._deg+1)) # set the dimension of the initial quadrature state correctly, and make sure the derivative name is valid try: qStates[0,0,0] = 0*lookup(quadratureStateDotName,timestep=0,nicpIdx=0,degIdx=1) except ValueError: raise ValueError('quadrature derivative name \"'+quadratureStateDotName+ '\" is not a valid x/z/u/p/output') ldInv = np.linalg.inv(lagrangePoly.lDotAtTauRoot[1:,1:]) ld0 = lagrangePoly.lDotAtTauRoot[1:,0] l1 = lagrangePoly.lAtOne # print -C.mul(C.mul(ldInv, ld0).T, l1[1:]) + l1[0] breakQuadratureIntervals = True if breakQuadratureIntervals: for k in range(self._nk): for i in range(self._nicp): dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j) for j in range(1,self._deg+1)]) Qs = C.mul( ldInv, dQs) for j in range(1,self._deg+1): qStates[k,i,j] = qStates[k,i,0] + Qs[j-1] m = C.mul( Qs.T, l1[1:]) if i < self._nicp - 1: qStates[k,i+1,0] = qStates[k,i,0] + m else: qStates[k+1,0,0] = qStates[k,i,0] + m else: for k in range(self._nk): for i in range(self._nicp): dQs = h*C.veccat([lookup(quadratureStateDotName,timestep=k,nicpIdx=i,degIdx=j) for j in range(1,self._deg+1)]) Qs = C.mul( ldInv, dQs - ld0*qStates[k,i,0] ) for j in range(1,self._deg+1): qStates[k,i,j] = Qs[j-1] m = C.veccat( [qStates[k,i,0], Qs] ) m = C.mul( m.T, l1) if i < self._nicp - 1: qStates[k,i+1,0] = m else: qStates[k+1,0,0] = m self._quadratures[quadratureStateName] = qStates self._setupQuadratureFunctions(symbolicDvs)
def plot_arrows3D(name, ax, x, y, phi, theta): x = ca.veccat(x) y = ca.veccat(y) z = ca.DMatrix.zeros(x.size()) phi = ca.veccat(phi) theta = ca.veccat(theta) x_vec = ca.cos(theta)*ca.cos(phi) y_vec = ca.cos(theta)*ca.sin(phi) z_vec = ca.sin(theta) length = 2.0 ax.quiver(x+length*x_vec, y+length*y_vec, z+length*z_vec, x_vec, y_vec, z_vec, length=length, arrow_length_ratio=0.5) return [Patch(color='red', label=name)]
def delay_arguments_function(self): # We cannot assume that we can ca.horzcat/vertcat all delay arguments # and expressions due to shape differences, so instead we flatten our # delay expressions and durations into list, i.e. [delay_expr_1, # delay_duration_1, delay_expr_2, delay_duration_2, ...]. out_arguments = list(itertools.chain.from_iterable(self.delay_arguments)) if hasattr(self, '_states_vector'): return self._expand_mx_func(ca.Function( 'delay_arguments', [self.time, self._states_vector, self._der_states_vector, self._alg_states_vector, self._inputs_vector, ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters))], out_arguments)) else: return self._expand_mx_func(ca.Function( 'delay_arguments', [self.time, ca.veccat(*self._symbols(self.states)), ca.veccat(*self._symbols(self.der_states)), ca.veccat(*self._symbols(self.alg_states)), ca.veccat(*self._symbols(self.inputs)), ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters))], out_arguments))
def observation_covariance(state, observation): d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])]) r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]]) r_cos_theta = ca.mul(d.T, r) cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2) # Look at the ball and be close to the ball nz = observation.size R = observation.squared(ca.SX.zeros(nz, nz)) R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2 R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2 # state -> R return ca.SXFunction("Observation covariance", [state], [R])
def delay_arguments_function(self): if hasattr(self, '_states_vector'): return self._expand_mx_func( ca.Function('delay_arguments', [ self.time, self._states_vector, self._der_states_vector, self._alg_states_vector, self._inputs_vector, ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters)) ], [ ca.horzcat(delay_argument.expr, delay_argument.duration) for delay_argument in self.delay_arguments ] if len(self.delay_arguments) > 0 else [])) else: return self._expand_mx_func( ca.Function('delay_arguments', [ self.time, ca.veccat(*self._symbols(self.states)), ca.veccat(*self._symbols(self.der_states)), ca.veccat(*self._symbols(self.alg_states)), ca.veccat(*self._symbols(self.inputs)), ca.veccat(*self._symbols(self.constants)), ca.veccat(*self._symbols(self.parameters)) ], [ ca.horzcat(delay_argument.expr, delay_argument.duration) for delay_argument in self.delay_arguments ] if len(self.delay_arguments) > 0 else []))
def vectorizeXUP(x,u,p,dae): if type(x) == dict: xVec = C.veccat([getitemMsg(x,name,"the states") for name in dae.xNames()]) else: xVec = x if type(u) == dict: uVec = C.veccat([getitemMsg(u,name,"the controls") for name in dae.uNames()]) else: uVec = u if type(p) == dict: pVec = C.veccat([getitemMsg(p,name,"the parameters") for name in dae.pNames()]) else: pVec = p return (xVec,uVec,pVec)
def sxFun(self): self._freeze('sxFun()') algRes = None if hasattr(self,'_algRes'): algRes = self._algRes odeRes = self._odeRes if isinstance(odeRes,list): odeRes = C.veccat(odeRes) if isinstance(algRes,list): algRes = C.veccat(algRes) return C.SXFunction( C.daeIn( x=self.xVec(), z=self.zVec(), p=C.veccat([self.uVec(),self.pVec()]), xdot=self.stateDotDummy ), C.daeOut( alg=algRes, ode=odeRes) )
def observation_covariance(state, observation): d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])]) r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']]) r_cos_theta = ca.mul(d.T,r) cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2) # Look at the ball and be close to the ball nz = observation.size R = observation.squared(ca.SX.zeros(nz,nz)) R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2 R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2 # state -> R return ca.SXFunction('Observation covariance',[state],[R])
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3): ''' take the dae that has x/z/u/p added to it already and return the states added to it and return mass matrix and rhs of the dae residual ''' R_b2n = dae['R_n2b'].T r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']]) r_b2bridle_b = C.veccat([0,0,conf['zt']]) v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']]) r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b) mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0) mm01 = C.SXMatrix(3,3) mm10 = mm01.T mm02 = r_n2bridle_n mm20 = mm02.T J = C.blockcat([[ conf['j1'], 0, conf['j31']], [ 0, conf['j2'], 0], [conf['j31'], 0, conf['j3']]]) mm11 = J mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n)) mm21 = mm12.T mm22 = C.SXMatrix(1,1) mm = C.blockcat([[mm00,mm01,mm02], [mm10,mm11,mm12], [mm20,mm21,mm22]]) # right hand side rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)]) rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b'])) # last element of RHS R_n2b = dae['R_n2b'] w_bn_b = dae['w_bn_b'] grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b)) tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \ C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b) rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr'] rhs = C.veccat([rhs0,rhs1,rhs2]) c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2) v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b)) cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr'] a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']]) dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']]) a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b))) cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \ dae['dr']**2 - dae['r']*dae['ddr'] dae['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot return (mm, rhs)
def compute_mass_matrix(dae, conf, f1, f2, f3, t1, t2, t3): ''' take the dae that has x/z/u/p added to it already and return the states added to it and return mass matrix and rhs of the dae residual ''' R_b2n = dae['R_n2b'].T r_n2b_n = C.veccat([dae['r_n2b_n_x'], dae['r_n2b_n_y'], dae['r_n2b_n_z']]) r_b2bridle_b = C.veccat([0,0,conf['zt']]) v_bn_n = C.veccat([dae['v_bn_n_x'],dae['v_bn_n_y'],dae['v_bn_n_z']]) r_n2bridle_n = r_n2b_n + C.mul(R_b2n, r_b2bridle_b) mm00 = C.diag([1,1,1]) * (conf['mass'] + conf['tether_mass']/3.0) mm01 = C.SXMatrix(3,3) mm10 = mm01.T mm02 = r_n2bridle_n mm20 = mm02.T J = C.vertcat([C.horzcat([ conf['j1'], 0, conf['j31']]), C.horzcat([ 0, conf['j2'], 0]), C.horzcat([conf['j31'], 0, conf['j3']])]) mm11 = J mm12 = C.cross(r_b2bridle_b, C.mul(dae['R_n2b'], r_n2b_n)) mm21 = mm12.T mm22 = C.SXMatrix(1,1) mm = C.vertcat([C.horzcat([mm00,mm01,mm02]), C.horzcat([mm10,mm11,mm12]), C.horzcat([mm20,mm21,mm22])]) # right hand side rhs0 = C.veccat([f1,f2,f3 + conf['g']*(conf['mass'] + conf['tether_mass']*0.5)]) rhs1 = C.veccat([t1,t2,t3]) - C.cross(dae['w_bn_b'], C.mul(J, dae['w_bn_b'])) # last element of RHS R_n2b = dae['R_n2b'] w_bn_b = dae['w_bn_b'] grad_r_cdot = v_bn_n + C.mul(R_b2n, C.cross(dae['w_bn_b'], r_b2bridle_b)) tPR = - C.cross(C.mul(R_n2b, r_n2b_n), C.cross(w_bn_b, r_b2bridle_b)) - \ C.cross(C.mul(R_n2b, v_bn_n), r_b2bridle_b) rhs2 = -C.mul(grad_r_cdot.T, v_bn_n) - C.mul(tPR.T, w_bn_b) + dae['dr']**2 + dae['r']*dae['ddr'] rhs = C.veccat([rhs0,rhs1,rhs2]) c = 0.5*(C.mul(r_n2bridle_n.T, r_n2bridle_n) - dae['r']**2) v_bridlen_n = v_bn_n + C.mul(R_b2n, C.cross(w_bn_b, r_b2bridle_b)) cdot = C.mul(r_n2bridle_n.T, v_bridlen_n) - dae['r']*dae['dr'] a_bn_n = C.veccat([dae.ddt(name) for name in ['v_bn_n_x','v_bn_n_y','v_bn_n_z']]) dw_bn_b = C.veccat([dae.ddt(name) for name in ['w_bn_b_x','w_bn_b_y','w_bn_b_z']]) a_bridlen_n = a_bn_n + C.mul(R_b2n, C.cross(dw_bn_b, r_b2bridle_b) + C.cross(w_bn_b, C.cross(w_bn_b, r_b2bridle_b))) cddot = C.mul(v_bridlen_n.T, v_bridlen_n) + C.mul(r_n2bridle_n.T, a_bridlen_n) - \ dae['dr']**2 - dae['r']*dae['ddr'] dae['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot return (mm, rhs)
def computeOutputs(self, x, u): self._outputsFun.setInput(x,0) self._outputsFun.setInput(u,1) self._outputsFun.evaluate() outs = [C.DMatrix(self._outputsFun.output(k)) for k in range(self._outputsFun.getNumOutputs())] return numpy.squeeze(numpy.array(C.veccat(outs)))
def __init__(self, dae, ts, measurements=None, options=RtIntegratorOptions()): self._dae = dae self._ts = ts if measurements is None: self._measurements = measurements else: if isinstance(measurements,list): measurements = C.veccat(measurements) self._measurements = measurements # setup outputs function self._outputsFun = self._dae.outputsFunWithSolve() (integratorLib, modelLib, rtModelGen) = exportIntegrator(self._dae, ts, options, self._measurements) self._integratorLib = integratorLib self._modelLib = modelLib self._rtModelGen = rtModelGen self._initIntegrator = 1 nx = len( self._dae.xNames() ) nz = len( self._dae.zNames() ) nu = len( self._dae.uNames() ) np = len( self._dae.pNames() ) print "creating outputs function" (fAll, (f0,outputs0names)) = self._dae.outputsFun() self.outputsFunAll = fAll self.outputsFun0 = f0 self.outputs0names = outputs0names self.xNames = self._dae.xNames() self.uNames = self._dae.uNames() self.outputNames = self._dae.outputNames() # self.uNames = dae.uNames() listOut=[] for n in self.outputNames: listOut.append([]) self._log = {'x':[],'u':[],'y':[],'yN':[],'outputs':dict(zip(self.outputNames,listOut))} # [ x z d(x,z)/dx d(x,z)/d(u,p) u p] self.x = numpy.zeros( nx ) self.z = numpy.zeros( nz ) self.u = numpy.zeros( nu ) self.p = numpy.zeros( np ) self.dx1_dx0 = numpy.zeros( (nx, nx) ) self.dz0_dx0 = numpy.zeros( (nz, nx) ) self.dx1_du = numpy.zeros( (nx, nu) ) self.dx1_dp = numpy.zeros( (nx, np) ) self.dz0_du = numpy.zeros( (nz, nu) ) self.dz0_dp = numpy.zeros( (nz, np) ) self._dx1z0_dx0 = numpy.zeros( (nx+nz, nx) ) self._dx1z0_dup = numpy.zeros( (nx+nz, nu+np) ) if self._measurements is not None: nh = self._measurements.size() self.h = numpy.zeros( nh ) self.dh_dx0 = numpy.zeros( (nh, nx) ) self.dh_du = numpy.zeros( (nh, nu) ) self.dh_dp = numpy.zeros( (nh, np) )
def constrainInvariantErrs(): dcm = ocp.lookup('dcm',timestep=0) err = C.mul(dcm.T,dcm) ocp.constrain( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag= ("initial dcm orthonormal",None)) ocp.constrain(ocp.lookup('c',timestep=0), '==', 0, tag=('c(0)==0',None)) ocp.constrain(ocp.lookup('cdot',timestep=0), '==', 0, tag=('cdot(0)==0',None))
def get_primitives(el, *args): """ Out of a list of expression, retrieves all primitive expressions The result is sorted into a dictionary with the key originating from the 'shorthand' class attribute of OptimzationObject subclasses """ if len(args) == 0: dep = True else: dep = args[0] try: vars = C.symvar(C.veccat(*el)) except: vars = {} syms = dict() for i, v in enumerate(vars): mymapping = OptimizationObject.mapping vobj = mymapping[hash(v)] symkey = vobj.shorthand if symkey in syms: syms[symkey] = syms[symkey] + [vobj] else: syms[symkey] = [vobj] return syms
def _setupDynamicsConstraints(self,endTime,traj): # Todo: add parallelization # Todo: get endTime right g = [] nicp = 1 deg = 4 p = self._dvMap.pVec() for k in range(self.nk): newton = Newton(LagrangePoly,self.dae,1,nicp,deg,'RADAU') newton.setupStuff(endTime) X0_i = self._dvMap.xVec(k) U_i = self._U[k,:].T # guess if traj is None: newton.isolver.setOutput(1,0) else: X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._xNames]) Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._zNames]) newton.isolver.setOutput(C.veccat([X,Z]),0) _, Xf_i = newton.isolver.call([X0_i,U_i,p]) X0_i_plus = self._dvMap.xVec(k+1) g.append(Xf_i-X0_i_plus) return g
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 _setupDynamicsConstraints(self, endTime, traj): # Todo: add parallelization # Todo: get endTime right g = [] nicp = 1 deg = 4 p = self._dvMap.pVec() for k in range(self.nk): newton = Newton(LagrangePoly, self.dae, 1, nicp, deg, 'RADAU') newton.setupStuff(endTime) X0_i = self._dvMap.xVec(k) U_i = self._U[k, :].T # guess if traj is None: newton.isolver.setOutput(1, 0) else: X = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._xNames]) Z = C.DMatrix([[traj.lookup(name,timestep=k,degIdx=j) for j in range(1,traj.dvMap._deg+1)] \ for name in traj.dvMap._zNames]) newton.isolver.setOutput(C.veccat([X, Z]), 0) _, Xf_i = newton.isolver.call([X0_i, U_i, p]) X0_i_plus = self._dvMap.xVec(k + 1) g.append(Xf_i - X0_i_plus) return g
def setupImplicitFunction(operAlpha, An, geom): #setup function to obtain B and RHS for the Fourier series calc def getBandRHS(alphaiLoc): alphaLoc = geom.alphaGeometric(operAlpha) - alphaiLoc clLoc = geom.clLoc(alphaLoc) RHS = clLoc*geom.chordLoc B = numpy.sin(numpy.outer(geom.thetaLoc, geom.sumN))*(4.0*geom.bref) return (B,RHS) alphaiLoc = [] for i in range(geom.n): alphaiLoc.append( C.inner_prod(geom.sumN*numpy.sin(geom.sumN*geom.thetaLoc[i])/numpy.sin(geom.thetaLoc[i]), An)) alphaiLoc = C.veccat(alphaiLoc) (B,RHS) = getBandRHS(alphaiLoc) makeMeZero = RHS - C.mul(B, An) CL = An[0]*numpy.pi*geom.AR CDi = 0.0 for i in range(geom.n): k = geom.sumN[i] CDi += k * An[i]**2 / An[0]**2 CDi *= numpy.pi*geom.AR*An[0]**2 return (makeMeZero, alphaiLoc, CL, CDi)
def constrainInvariantErrs(): dcm = dae['dcm'] err = C.mul(dcm.T,dcm) g.add( C.veccat([err[0,0] - 1, err[1,1]-1, err[2,2] - 1, err[0,1], err[0,2], err[1,2]]), '==', 0, tag= ("initial dcm orthonormal",None)) g.add(dae['c'], '==', 0, tag=('c(0)==0',None)) g.add(dae['cdot'], '==', 0, tag=('cdot(0)==0',None))
def maybeAddBoxConstraint(): inputs = C.veccat([self.dae.xVec(), self.dae.uVec()]) # make sure only x and u are in rhs,lhs rml = rhs - lhs f = C.SXFunction([inputs], [rml]) f.init() if len(f.getFree()) != 0: return # take jacobian of rhs-lhs jac = C.jacobian(rml, inputs) # fail if any jacobian element is not constant coeffs = {} for j in range(inputs.size()): if not jac[0, j].toScalar().isZero(): if not jac[0, j].toScalar().isConstant(): return coeffs[j] = jac[0, j] if len(coeffs) == 0: raise Exception("constraint has no design variables in it") if len(coeffs) > 1: self.debug( "found linear constraint that is not box constraint") return # alright, we've found a box constraint! j = coeffs.keys()[0] coeff = coeffs[j] name = (self.dae.xNames() + self.dae.uNames())[j] [f0] = f.eval([0 * inputs]) # if we just divided by a negative number (coeff), flip the comparison if not coeff.toScalar().isNonNegative(): # lhs `cmp` rhs # 0 `cmp` rhs - lhs # 0 `cmp` coeff*x + f0 # -f0 `cmp` coeff*x # -f0/coeff `FLIP(cmp)` x if comparison == '>=': newComparison = '<=' elif comparison == '<=': newComparison = '>=' else: newComparison = comparison else: newComparison = comparison c = -f0 / coeff self.debug('found linear constraint: ' + str(c) + ' ' + newComparison + ' ' + name) if newComparison == '==': self._bound(name, c, 'equality', when=when) elif newComparison == '<=': self._bound(name, c, 'lower', when=when) elif newComparison == '>=': self._bound(name, c, 'upper', when=when) else: raise Exception('the "impossible" happened, comparison "' + str(comparison) + "\" not in ['==','>=','<=']") return 'found box constraint'
def minimizeLsqEndTerm(self, obj): if isinstance(obj, list): obj = C.veccat(obj) C.makeDense(obj) shape = obj.shape assert shape[0] == 1 or shape[1] == 1, "objective cannot be matrix, got shape: " + str(shape) assert not hasattr(self, "_minLsqEndTerm"), "you can only call minimizeLsqEndTerm once" self._minLsqEndTerm = obj
def __minimizeLsqEndTerm(self, obj): if isinstance(obj, list): obj = C.veccat(obj) obj.makeDense() shape = obj.shape assert shape[0] == 1 or shape[1] == 1, 'objective cannot be matrix, got shape: '+str(shape) assert not hasattr(self, '_minLsqEndTerm'), 'you can only call __minimizeLsqEndTerm once' self._minLsqEndTerm = obj
def maybeAddBoxConstraint(): inputs = C.veccat([self.dae.xVec(),self.dae.uVec()]) # make sure only x and u are in rhs,lhs rml = rhs-lhs f = C.SXFunction([inputs],[rml]) f.init() if f.getFree().shape[0] != 0: return # take jacobian of rhs-lhs jac = C.jacobian(rml,inputs) # fail if any jacobian element is not constant coeffs = {} for j in range(inputs.size()): if jac.hasNZ(0,j): if not jac[0,j].isConstant(): return coeffs[j] = jac[0,j] if len(coeffs) == 0: raise Exception("constraint has no design variables in it") if len(coeffs) > 1: self.debug("found linear constraint that is not box constraint") return # alright, we've found a box constraint! j = coeffs.keys()[0] coeff = coeffs[j] name = (self.dae.xNames()+self.dae.uNames())[j] [f0] = f([0*inputs]) # if we just divided by a negative number (coeff), flip the comparison #if not coeff.toScalar().isNonNegative(): if not coeff>=0.0: # lhs `cmp` rhs # 0 `cmp` rhs - lhs # 0 `cmp` coeff*x + f0 # -f0 `cmp` coeff*x # -f0/coeff `FLIP(cmp)` x if comparison == '>=': newComparison = '<=' elif comparison == '<=': newComparison = '>=' else: newComparison = comparison else: newComparison = comparison c = -f0/coeff self.debug('found linear constraint: '+str(c)+' '+newComparison+' '+name) if newComparison == '==': self._bound( name, c, 'equality', when=when) elif newComparison == '<=': self._bound( name, c, 'lower', when=when) elif newComparison == '>=': self._bound( name, c, 'upper', when=when) else: raise Exception('the "impossible" happened, comparison "'+str(comparison)+ "\" not in ['==','>=','<=']") return 'found box constraint'
def writeObjective(ocp, out0, exportName): dae = ocp.dae # first make out not a function of xDot or z inputs0 = [dae.xDotVec(), dae.xVec(), dae.zVec(), dae.uVec(), dae.pVec()] outputFun0 = C.SXFunction(inputs0, [out0]) (xDotDict, zDict) = dae.solveForXDotAndZ() xDot = C.veccat([xDotDict[name] for name in dae.xNames()]) z = C.veccat([zDict[name] for name in dae.zNames()]) # plug in xdot, z solution to outputs fun outputFun0.init() [out] = outputFun0.eval([xDot, dae.xVec(), z, dae.uVec(), dae.pVec()]) assert len(dae.pNames( )) == 0, "parameters not supported right now in ocp export, sorry" # make sure each element in the output is only a function of x or u, not both testSeparation(dae, out, exportName) # make new SXFunction that is only fcn of [x, u, p] if exportName == 'lsqExtern': inputs = C.veccat([dae.xVec(), dae.uVec(), dae.pVec()]) outs = C.veccat([ out, C.jacobian(out, dae.xVec()).T, C.jacobian(out, dae.uVec()).T ]) outputFun = C.SXFunction([inputs], [C.densify(outs)]) outputFun.init() assert len(outputFun.getFree()) == 0, 'the "impossible" happened >_<' elif exportName == 'lsqEndTermExtern': inputs = dae.xVec() outs = C.veccat([out, C.jacobian(out, dae.xVec()).T]) outputFun = C.SXFunction([inputs], [C.densify(outs)]) outputFun.init() assert len( outputFun.getFree() ) == 0, 'lsqEndTermExtern cannot be a function of controls u, saw: ' + str( outputFun.getFree()) else: raise Exception('unrecognized name "' + exportName + '"') return codegen.writeCCode(outputFun, exportName)
def computeOutputs(self, x, u): self._outputsFun.setInput(x, 0) self._outputsFun.setInput(u, 1) self._outputsFun.evaluate() outs = [ C.DMatrix(self._outputsFun.output(k)) for k in range(self._outputsFun.getNumOutputs()) ] return numpy.squeeze(numpy.array(C.veccat(outs)))
def _create_nonlinear_constraints(model, V): g, lbg, ubg = [], [], [] for k in range(model.n): # Multiple shooting [xk_next] = model.F([V['X', k], V['U', k]]) g.append(xk_next - V['X', k+1]) lbg.append(ca.DMatrix.zeros(model.nx)) ubg.append(ca.DMatrix.zeros(model.nx)) # Control constraints constraint_k = model._set_constraint(V, k) g.append(constraint_k) lbg.append(-ca.inf) ubg.append(0) g = ca.veccat(g) lbg = ca.veccat(lbg) ubg = ca.veccat(ubg) return [g, lbg, ubg]
def add_constraints(self, stage, opti): # Obtain the discretised system F = self.discrete_system(stage) if stage.is_free_time(): opti.subject_to(self.T >= 0) self.xk = [] self.q = 0 # we only save polynomal coeffs for runge-kutta4 if stage._method.intg == 'rk': self.poly_coeff = [] else: self.poly_coeff = None for k in range(self.N): FF = F(x0=self.X[k], u=self.U[k], t0=self.control_grid[k], T=self.control_grid[k + 1] - self.control_grid[k], p=vertcat(veccat(*self.P), self.get_p_control_at(stage, k))) # Dynamic constraints a.k.a. gap-closing constraints opti.subject_to(self.X[k + 1] == FF["xf"]) # Save intermediate info poly_coeff_temp = FF["poly_coeff"] xk_temp = FF["Xi"] self.q = self.q + FF["qf"] # we cannot return a list from a casadi function self.xk.extend([xk_temp[:, i] for i in range(self.M)]) if self.poly_coeff is not None: self.poly_coeff.extend( horzsplit(poly_coeff_temp, poly_coeff_temp.shape[1] // self.M)) for l in range(self.M): for c, meta, _ in stage._constraints["integrator"]: opti.subject_to(self.eval_at_integrator(stage, c, k, l), meta=meta) for c, meta, _ in stage._constraints["inf"]: self.add_inf_constraints(stage, opti, c, k, l, meta) for c, meta, _ in stage._constraints[ "control"]: # for each constraint expression opti.subject_to(self.eval_at_control(stage, c, k), meta=meta) 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) self.xk.append(self.X[-1]) for c, meta, _ in stage._constraints[ "point"]: # Append boundary conditions to the end opti.subject_to(self.eval(stage, c), meta=meta)
def setResidual(self,res): ''' set the dae implicit residual vector f where f(x,xdot,z,u,p) == 0 ''' if hasattr(self,'_residual'): raise ValueError('residual already set') if isinstance(res,list): res = C.veccat(res) self._residual = res
def constrainInvariantErrs(): R_c2b = dae['R_c2b'] self.makeOrthonormal(ginv, R_c2b) ginv.add(dae['c'], '==', 0, tag = ('c(0) == 0', None)) ginv.add(dae['cdot'], '==', 0, tag = ('cdot( 0 ) == 0', None)) di = dae['cos_delta'] ** 2 + dae['sin_delta'] ** 2 - 1 ginv.add(di, '==', 0, tag = ('delta invariant', None)) ginv.add(C.mul(dae['R_c2b'].T, dae['w_bn_b']) - C.veccat([0, 0, dae['ddelta']]) , '==', 0, tag = ("Rotational velocities", None))
def minimizeLsq(self, obj): if isinstance(obj, list): obj = C.veccat(obj) C.makeDense(obj) shape = obj.shape assert shape[0] == 1 or shape[ 1] == 1, 'objective cannot be matrix, got shape: ' + str(shape) assert not hasattr(self, '_minLsq'), 'you can only call minimizeLsq once' self._minLsq = obj
def constrainInvariantErrs(): dcm = mhe.lookup('dcm', timestep=0) err = C.mul(dcm.T, dcm) mhe.constrain( C.veccat([ err[0, 0] - 1, err[1, 1] - 1, err[2, 2] - 1, err[0, 1], err[0, 2], err[1, 2] ]), '==', 0) mhe.constrain(mhe.lookup('c', timestep=0), '==', 0) mhe.constrain(mhe.lookup('cdot', timestep=0), '==', 0)
def _makeImplicitFunction(self): ffcn = self._makeResidualFunction() x0 = C.ssym('x0', self.dae.xVec().size()) X = C.ssym('X', self.dae.xVec().size(), self.deg * self.nicp) Z = C.ssym('Z', self.dae.zVec().size(), self.deg * self.nicp) u = C.ssym('u', self.dae.uVec().size()) p = C.ssym('p', self.dae.pVec().size()) constraints = [] ############################################################ ndiff = self.dae.xVec().size() x0_ = x0 for nicpIdx in range(self.nicp): X_ = X[:, nicpIdx * self.deg:(nicpIdx + 1) * self.deg] Z_ = Z[:, nicpIdx * self.deg:(nicpIdx + 1) * self.deg] for j in range(1, self.deg + 1): # Get an expression for the state derivative at the collocation point xp_jk = self.lagrangePoly.lDotAtTauRoot[j, 0] * x0_ for j2 in range(1, self.deg + 1): # get the time derivative of the differential states (eq 10.19b) xp_jk += self.lagrangePoly.lDotAtTauRoot[j, j2] * X_[:, j2 - 1] # Add collocation equations to the NLP [fk] = ffcn.eval( [xp_jk / self.h, X_[:, j - 1], Z_[:, j - 1], u, p]) # impose system dynamics (for the differential states (eq 10.19b)) constraints.append(fk[:ndiff]) # impose system dynamics (for the algebraic states (eq 10.19b)) constraints.append(fk[ndiff:]) # Get an expression for the state at the end of the finite element xf = self.lagrangePoly.lAtOne[0] * x0_ for j in range(1, self.deg + 1): xf += self.lagrangePoly.lAtOne[j] * X_[:, j - 1] x0_ = xf ifcn = C.SXFunction([C.veccat([X, Z]), x0, u, p], [C.veccat(constraints), xf]) ifcn.init() return ifcn
def _initialize_ekf_for_arrival_cost_update(self): self.hfcn = ca.Function("h", [self.x, self.c], [self.h]) self.H = self.hfcn.jac() ode = {"x": self.x, "p": ca.veccat(self.c, self.u, self.b, self.w), \ "ode": self.f} self.phi = ca.integrator("integrator", "cvodes", ode, \ {"t0": 0.0, "tf": self._timing.dt_day}) self.Phi = self.phi.jac()
def makeNmpc(dae, N, dt): mpc = rawe.Ocp(dae, N=N, ts=dt) ocpOpts = rawe.OcpExportOptions() ocpOpts['HESSIAN_APPROXIMATION'] = 'GAUSS_NEWTON' ocpOpts['DISCRETIZATION_TYPE'] = 'MULTIPLE_SHOOTING' ocpOpts['QP_SOLVER'] = 'QP_QPOASES' ocpOpts['HOTSTART_QP'] = False ocpOpts['SPARSE_QP_SOLUTION'] = 'CONDENSING' # ocpOpts['SPARSE_QP_SOLUTION'] = 'FULL_CONDENSING_U2' # ocpOpts['AX_NUM_QP_ITERATIONS'] = '30' ocpOpts['FIX_INITIAL_STATE'] = True mpc.minimizeLsq(C.veccat([mpc['x'], mpc['v'], mpc['u']])) mpc.minimizeLsqEndTerm(C.veccat([mpc['x'], mpc['v']])) cgOpts = {'CXX': 'g++', 'CC': 'gcc'} return rawe.OcpRT(mpc, ocpOptions=ocpOpts, integratorOptions=intOpts, codegenOptions=cgOpts)
def writeDimensions(topname, dae, measX, measU, mheHorizN, mpcHorizN): nMeasX = C.veccat([dae[n] for n in measX]).numel() nMeasU = C.veccat([dae[n] for n in measU]).numel() nOutputs = C.veccat([dae[n] for n in dae.outputNames()]).numel() ret = [] ret.append('#ifndef __' + topname + '_DIMENSIONS_H__') ret.append('#define __' + topname + '_DIMENSIONS_H__') ret.append('\n') ret.append('#define NUM_DIFFSTATES ' + str(len(dae.xNames()))) ret.append('#define NUM_ALGVARS ' + str(len(dae.zNames()))) ret.append('#define NUM_CONTROLS ' + str(len(dae.uNames()))) ret.append('#define NUM_PARAMETERS ' + str(len(dae.pNames()))) ret.append('#define NUM_OUTPUTS ' + repr(nOutputs)) ret.append('#define NUM_MEASUREMENTS_X ' + repr(nMeasX)) ret.append('#define NUM_MEASUREMENTS_U ' + repr(nMeasU)) ret.append('#define NUM_MHE_HORIZON ' + str(mheHorizN)) ret.append('#define NUM_MPC_HORIZON ' + str(mpcHorizN)) ret.append('\n') ret.append('#endif // __' + topname + '_DIMENSIONS_H__') return '\n'.join(ret)