def construct(self): config = self.fleet.configuration rel_pos_c = {} for veh in self.vehicles: ind_veh = sorted(config[veh].keys()) rel_pos_c[veh] = veh.define_parameter('rel_pos_c', len(ind_veh)) FixedTPoint2point.construct(self) # get formation center as seen by vehicles centra = {} splines = {} for veh in self.vehicles: splines[veh] = self.father.get_variables(veh, 'splines_seg0', symbolic=True) ind_veh = sorted(config[veh].keys()) centra[veh] = veh.get_fleet_center(splines[veh], rel_pos_c[veh], substitute=False) # formation constraints couples = {veh: [] for veh in self.vehicles} for veh in self.vehicles: for nghb in self.fleet.get_neighbors(veh): if veh not in couples[nghb] and nghb not in couples[veh]: couples[veh].append(nghb) if self.fleet.interconnection == 'circular': couples.pop(self.vehicles[-1], None) couples.pop(self.vehicles[-2], None) t = self.define_symbol('t') T = self.define_symbol('T') for veh, nghbs in couples.items(): ind_veh = sorted(config[veh].keys()) pos_c_veh = centra[veh] for nghb in nghbs: ind_nghb = sorted(config[nghb].keys()) pos_c_nghb = centra[nghb] for ind_v, ind_n in zip(ind_veh, ind_nghb): if self.options['soft_formation']: weight = self.options['soft_formation_weight'] eps = self.define_spline_variable( 'eps_form_' + str(ind_v) + str(ind_n), basis=veh.basis)[0] obj = weight * definite_integral(eps, t / T, 1.) self.define_objective(obj) self.define_constraint( pos_c_veh[ind_v] - pos_c_nghb[ind_n] - eps, -inf, 0.) self.define_constraint( -pos_c_veh[ind_v] + pos_c_nghb[ind_n] - eps, -inf, 0.) if self.options['max_formation_deviation'] != inf: max_dev = abs( self.options['max_formation_deviation']) self.define_constraint(eps, -max_dev, max_dev) else: self.define_constraint( pos_c_veh[ind_v] - pos_c_nghb[ind_n], 0., 0.)
def set_parameters(self, current_time): parameters = FixedTPoint2point.set_parameters(self, current_time) for veh in self.vehicles: if veh not in parameters: parameters[veh] = {} parameters[veh].update({'rel_pos_c': veh.rel_pos_c}) return parameters
def set_parameters(self, current_time): parameters = FixedTPoint2point.set_parameters(self, current_time) for veh in self.vehicles: if veh not in parameters: parameters[veh] = {} parameters[veh].update({'rel_pos_c': veh.rel_pos_c}) return parameters
def construct(self): config = self.fleet.configuration rel_pos_c = {} for veh in self.vehicles: ind_veh = sorted(config[veh].keys()) rel_pos_c[veh] = veh.define_parameter('rel_pos_c', len(ind_veh)) FixedTPoint2point.construct(self) # get formation center as seen by vehicles centra = {} splines = {} for veh in self.vehicles: splines[veh] = self.father.get_variables(veh, 'splines_seg0', symbolic=True) ind_veh = sorted(config[veh].keys()) centra[veh] = veh.get_fleet_center(splines[veh], rel_pos_c[veh], substitute=False) # formation constraints couples = {veh: [] for veh in self.vehicles} for veh in self.vehicles: for nghb in self.fleet.get_neighbors(veh): if veh not in couples[nghb] and nghb not in couples[veh]: couples[veh].append(nghb) if self.fleet.interconnection == 'circular': couples.pop(self.vehicles[-1], None) couples.pop(self.vehicles[-2], None) t = self.define_symbol('t') T = self.define_symbol('T') for veh, nghbs in couples.items(): ind_veh = sorted(config[veh].keys()) pos_c_veh = centra[veh] for nghb in nghbs: ind_nghb = sorted(config[nghb].keys()) pos_c_nghb = centra[nghb] for ind_v, ind_n in zip(ind_veh, ind_nghb): if self.options['soft_formation']: weight = self.options['soft_formation_weight'] eps = self.define_spline_variable('eps_form_'+str(ind_v)+str(ind_n), basis=veh.basis)[0] obj = weight*definite_integral(eps, t/T, 1.) self.define_objective(obj) self.define_constraint(pos_c_veh[ind_v] - pos_c_nghb[ind_n] - eps, -inf, 0.) self.define_constraint(-pos_c_veh[ind_v] + pos_c_nghb[ind_n] - eps, -inf, 0.) if self.options['max_formation_deviation'] != inf: max_dev = abs(self.options['max_formation_deviation']) self.define_constraint(eps, -max_dev, max_dev) else: self.define_constraint(pos_c_veh[ind_v] - pos_c_nghb[ind_n], 0., 0.)
def update(self, current_time, update_time, sample_time): FixedTPoint2point.update(self, current_time, update_time, sample_time) self.fleet.update_plots()
def set_default_options(self): FixedTPoint2point.set_default_options(self) self.options['soft_formation'] = False self.options['soft_formation_weight'] = 10 self.options['max_formation_deviation'] = inf
def __init__(self, fleet, environment, options=None): FixedTPoint2point.__init__(self, fleet, environment, options)
def update(self, current_time, update_time, sample_time): FixedTPoint2point.update(self, current_time, update_time, sample_time) self.fleet.update_plots()
def set_default_options(self): FixedTPoint2point.set_default_options(self) self.options['soft_formation'] = False self.options['soft_formation_weight'] = 10 self.options['max_formation_deviation'] = inf
def __init__(self, fleet, environment, options=None): FixedTPoint2point.__init__(self, fleet, environment, options)