def relex_from_pdb(seq, feature_path, input_pdb, output_pdb):
pose = pose_from_pdb(input_pdb)
raw_constraints = Constraints(seq, feature_path)
constraints = raw_constraints.get_constraint_v1_fix_gly()
constraints.apply(pose)
relax(pose)
pose.dump_pdb(output_pdb)
def main(fasta_path, feature_path, output_dir, n_workers, n_structs, n_iter):
pyrosetta.init(
"-hb_cen_soft -relax:default_repeats 5 -default_max_cycles 200 -out:level 100"
)
os.makedirs(output_dir, exist_ok=True)
name = os.path.splitext(os.path.basename(fasta_path))[0]
seq = open(fasta_path).readlines()[1].strip()
seq_no_g = "".join(["A" if _ == "G" else _ for _ in list(seq)])
raw_constraints = Constraints(seq, feature_path)
constraints = raw_constraints.get_constraint_v1()
score_function = geo_sf(dist_weight=5, dihedral_weight=1, angle_weight=1)
poses = repeat_minimize(
seq_no_g,
constraints,
score_function,
output_dir,
n_workers,
n_structs,
n_iter,
)
for i, a in enumerate(seq):
if a == "G":
mutator = rosetta.protocols.simple_moves.MutateResidue(
i + 1, "GLY")
for pose in poses:
mutator.apply(pose)
os.makedirs(os.path.join(output_dir, "final"), exist_ok=True)
for i, pose in enumerate(poses):
path = os.path.join(output_dir, "final", "%s_%02i.pdb" % (name, i))
pose.dump_pdb(path)
def init_mpc(self, N):
#self.init_vehicle(0.1)
self.init_track()
self.costFunction = CostFunction(self.raceTrack)
self.bnds = self.vehicleModel.get_bounds(N)
self.init_state_vector(N, [0, 0, 0.1, math.pi / 2, 0, 0, 0, 0])
print self.vehicleModel.get_dt()
self.constraints = Constraints(self.X0[:6], self.vehicleModel,
self.raceTrack)
self.constraints.ineq_constraint_vehicle_bounds_set_tangent_points(
self.X0)
self.cons = (
{
'type': 'eq',
'fun': self.constraints.constraint_fix_init_state
},
{
'type': 'eq',
'fun': self.constraints.constraint_vehicle_model
},
#{'type': 'ineq', 'fun': self.constraints.ineq_constraint_vehicle_model},
{
'type': 'ineq',
'fun': self.constraints.ineq_constraint_vehicle_bounds
})
#self.jac = self.costFunction.cost_dist_track_speed_jac()
self.computation_time = pygame.time.get_ticks()
self.simulation_time = pygame.time.get_ticks()
self.simulation_steps = 1
def expand_direction(self, constraint_mode, mass, contactsNumber, contacts, com, normals, direction):
g = 9.81
friction_coeff = 0.6
ng = 4
grav = np.array([[0.], [0.], [-g*mass]])
cost_function = np.vstack([direction, np.zeros((3*nc,1))])
p = matrix(cost_function)
#p = matrix(np.zeros((3*nc,1)))
torque = -np.cross(com, np.transpose(grav))
gravity_term = math.skew(grav)
#print gravity_term
A2 = np.vstack([np.zeros((3,2)), gravity_term[:,0:2]])
A1 = A2#np.zeros((6,0))
#print np.size(A2,0)
compDyn = ComputationalDynamics()
for j in range(0,contactsNumber):
r = contacts[j,:]
GraspMat = compDyn.getGraspMatrix(r)
A1 = np.hstack((A1, GraspMat[:,0:3]))
#A1 = matrix(A1)
b = matrix(np.vstack([-grav, np.transpose(torque)]).reshape((6)))
#A = np.hstack((A2, A1))
A = matrix(A1)
#contactsFourLegs = np.vstack([contacts, np.zeros((4-nc,3))])\
kin = HyQKinematics()
foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]])
contactsFourLegs = np.vstack([contacts, np.zeros((4-contactsNumber,3))])
q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = kin.inverse_kin(np.transpose(contactsFourLegs[:,0]),
np.transpose(foot_vel[:,0]),
np.transpose(contactsFourLegs[:,1]),
np.transpose(foot_vel[:,1]),
np.transpose(contactsFourLegs[:,2]),
np.transpose(foot_vel[:,2]))
if (not isOutOfWorkspace):
#kin.update_jacobians(q)
J_LF, J_RF, J_LH, J_RH = kin.update_jacobians(q)
#print J_LF
constraint = Constraints()
G_force, h, isConstraintOk = constraint.inequalities(constraint_mode, nc, ng, normals, friction_coeff, J_LF, J_RF, J_LH, J_RH)
G = np.hstack([np.zeros((np.size(G_force,0),2)), G_force])
G = matrix(G)
#print G, h
if not isConstraintOk:
print 'something is wrong in the inequalities'
else:
#print p
#print G,h
#print A,b
sol=solvers.lp(p, G, h, A, b)
x = sol['x']
status = sol['status']
inner_vertex = x[0:2]
force = x[2:np.size(A,1)]
return inner_vertex, force
def buildGrasp (self, g, h):
n = g + " grasps " + h
pn = g + " pregrasps " + h
self.graph.createGrasp (n, g, h)
self.graph.createPreGrasp (pn, g, h)
return dict ( zip (self.gfields, (
Constraints (numConstraints = _removeEmptyConstraints(self.graph.clientBasic.problem, [ n, ])),
Constraints (numConstraints = _removeEmptyConstraints(self.graph.clientBasic.problem, [ n + "/complement", ])),
Constraints (numConstraints = _removeEmptyConstraints(self.graph.clientBasic.problem, [ pn, ])),
)))
def optimize(self, N, loops=10):
"""filler"""
#Tsim = 2
#set initial position
X0 = self.create_init_state_vec(N, 1, 0, 3.0, math.pi / 2)
#constraints and bounds
#define bounds fitting to N and Statevector
bnds = self.vehicleModel.get_bounds(N)
print bnds
#constraints
# {'type': 'eq', 'fun': const_eq}
self.constraints = Constraints(X0[:4], self.vehicleModel)
cons = ({
'type': 'eq',
'fun': self.constraints.constraint_fix_init_state
}, {
'type': 'eq',
'fun': self.constraints.constraint_vehicle_model
})
#init state vector
#[x, y, v, orient, acc, steer_angle]
print "X0"
print X0
#for k in np.arange(0, Tsim, self.dt):
self.log_position(X0[0:4])
start = time.time()
for k in range(0, loops, 1):
res = minimize(self.costFunction.cost_dist_track,
X0,
method='SLSQP',
bounds=bnds,
constraints=cons) #, callback = self.callb)
x_new = self.vehicleModel.compute_next_state_(res.x[0:6])
self.log_position(x_new)
X0 = res.x
X0[0:4] = x_new
self.constraints.set_initial_state(X0[0:4])
print("schritt " + repr(k) + "von " + repr(loops) + ' ausgefuehrt')
print X0
print X0
end = time.time()
print "time to compute in s: " + repr(end - start)
self.plot_race_process()
def __init__(self):
"""Create Obsplanner instance.
Returns
-----
None
"""
self.instrument = None
self.telescope = None
self.sources = None
self.calibrators = None
self.constraints = Constraints()
self.scheduler = None
self.calscheduler = None
def standard_get_models(self, desired_number_of_models):
result = []
count = 0
self.clock.tick("first model")
while True:
self.clock.tick("unsat")
if (Options.MODE != Common.DEBUG and not(Options.PRODUCE_UNSAT_CORE)and count != desired_number_of_models and self.solver.check() == Common.SAT ) or \
(Options.MODE != Common.DEBUG and Options.PRODUCE_UNSAT_CORE and count != desired_number_of_models and self.solver.check(self.unsat_core_trackers) == Common.SAT ) or \
(Options.MODE == Common.DEBUG and count != desired_number_of_models and self.solver.check(self.unsat_core_trackers) == Common.SAT ):
if count == 0:
self.clock.tock("first model")
m = self.solver.model()
result.append(m)
# Create a new constraint that blocks the current model
if not Options.SUPPRESS_MODELS:
self.printVars(m)
preventSameModel(self, self.solver, m)
count += 1
else:
if count == 0 and Options.PRODUCE_UNSAT_CORE:
self.clock.tock("unsat")
debug_print("UNSAT")
core = self.solver.unsat_core()
debug_print(str(len(core)) + " constraints in unsat core: \n")
for i in core:
print(Constraints.interpretUnsatCore(self, str(i)))
print()
return result
elif count == 0:
standard_print("UNSAT")
return result
def main(bnfile,
bdtfile,
scoretype='BDeu',
ess=1.0,
constraint_file="",
cachefile=None,
topN=0,
dump_worklist=False):
cstrs = Constraints(constraint_file)
bn0 = bnmodule.load(bnfile)
sc = scorefactory.getscorer(data.Data(bdtfile),
scoretype,
ess,
cachefile=cachefile)
c = 0
for (_bnas, s) in hpcrawl([bn0],
sc,
cstrs=cstrs,
topN=topN,
dump_worklist=dump_worklist):
print c, s
# if c == 20000:
# bnmodule.BN(bn0.varc,_bnas,do_pic=False).save('X%d.bn' % c)
c += 1
def set_bulk_shiftpattern():
''' examples are 4 on 4 off, 5 days 2 off 5 nights 3 off, week of days followed by week of nights i.e. specific pattern from a start date. No days are specified'''
aux_obj = Aux_Methods()
s_obj = Shifts()
c_obj = Constraints()
tot = None
output_file = input('Name of output file: ')
globals.rotafile_path += output_file + '.txt'
while True:
day_type = input(
'Select Shift Type:\n(1) Same Day\n(2) Overnight\n(3) Off\n ')
tally = input('How Many? : ')
if day_type == '3':
label = 'Off'
tuple = label, tally
tot = tot + (tuple, )
else:
label = input('What label do you want displayed? : ')
s = input('Start at (HH:MM): ')
e = input('End at (HH:MM): ')
tuple = label, s, e, tally
if tot is None:
tot = (tuple, )
else:
tot = tot + (tuple, )
response = input('More ?: ')
if (response != 'y'):
return tot
def test_backtracking_can_find_a_solution(self):
problem = Variables(4)
forwardchecking = ForwardChecking(Constraints(4))
forwardchecking.findSolution(problem)
self.assertTrue(
self.fourByFourSolution(problem.queens)
or self.altFourByFourSolution(problem.queens))
def __init__ (self, factory, grasps, id, name):
self.grasps = grasps
self.id = id
self.name = name
self.manifold = Constraints()
self.foliation = Constraints()
# Add the grasps
for ig, ih in zip_idx(grasps):
if ih is not None:
self.manifold += factory.constraints.g (ig, ih, 'grasp')
self.foliation += factory.constraints.g (ig, ih, 'graspComplement')
# Add the placement constraints
for io, object in zip_idx(factory.objects):
if not factory._isObjectGrasped(grasps, io):
self.manifold += factory.constraints.p (object, 'placement')
self.foliation += factory.constraints.p (object, 'placementComplement')
def stepper_f(actions, better, accept, cstrs=None):
if not cstrs:
cstrs = Constraints()
def stepf(search_status):
scr = search_status["scr"]
bn = search_status["curr_bn"]
curr_score = search_status["curr_score"]
#print 'stepin', bn.arcs()
#for v in bn.vars(): print v, bn.path_in_counts[v]
#if not cycheck.is_dag(search_status["curr_bn"]): raise Exception('cyc')
#if not cycheck.is_dag(search_status["best_bn"]): raise Exception('cyc')
search_status["iters"] += 1
try:
aname, action = choice(acts["tryacts"].items())
changes, avars = action(bn, cstrs=cstrs)
search_status["t_no_acts"] = 0
map(scr.storevar, avars)
for avar in avars:
scr.score_new_v(bn, avar)
new_score = scr.score()
if better(new_score, search_status["best_score"]):
search_status["best_score"] = new_score
search_status["best_bn"] = bn.copy()
search_status["t_no_impr"] = 0
else:
search_status["t_no_impr"] += 1
if accept(new_score, search_status):
search_status["curr_score"] = new_score
scr.clearstore()
acts["commits"][aname](bn, changes)
search_status["t_no_acps"] = 0
else:
search_status["t_no_acps"] += 1
acts["cancels"][aname](bn, changes)
scr.restore()
except NoAction:
search_status["t_no_acts"] += 1
#if not cycheck.is_dag(search_status["curr_bn"]):
# print aname, changes, avars, bn.arcs()
# for v in bn.vars(): print v, bn.path_in_counts[v]
# raise Exception('cyc')
#if not cycheck.is_dag(search_status["best_bn"]):
# raise Exception('cyc')
return stepf
def __init__(self, domains, constraint_list):
'''
Initializes information, and sets the constraint formula.
'''
#A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
#GAC INFO
self.constraints = Constraints("column[y_index] == row[x_index]",
["column", "row", "x_index", "y_index"],
constraint_list)
self.domains = domains
#init gac
self.gac = Gac_nonogram()
def buildRelaxedPlacement (self, o):
n = "place_" + o
pn = "preplace_" + o
width = 0.05
io = self.graphfactory.objects.index(o)
ljs = []
for jn in self.graph.clientBasic.robot.getJointNames():
if jn.startswith(o + "/"):
ljs.append(jn)
q = self.graph.clientBasic.robot.getJointConfig(jn)
self.graph.clientBasic.problem.createLockedJoint(jn, jn, q)
placeAlreadyCreated = n in self.graph.clientBasic.problem.getAvailable ("numericalconstraint")
if (len(self.graphfactory.contactsPerObjects[io]) == 0 or len(self.graphfactory.envContacts) == 0) and not placeAlreadyCreated:
return dict ( zip (self.pfields, (Constraints (), Constraints (lockedJoints = ljs), Constraints (),)))
if not placeAlreadyCreated:
self.graph.client.problem.createPlacementConstraint (n, self.graphfactory.contactsPerObjects[io], self.graphfactory.envContacts)
if not pn in self.graph.clientBasic.problem.getAvailable ("numericalconstraint"):
self.graph.client.problem.createPrePlacementConstraint (pn, self.graphfactory.contactsPerObjects[io], self.graphfactory.envContacts, width)
return dict ( zip (self.pfields, (
Constraints (numConstraints = _removeEmptyConstraints(self.graph.clientBasic.problem, [ n, ])),
Constraints (lockedJoints = ljs),
Constraints (numConstraints = _removeEmptyConstraints(self.graph.clientBasic.problem, [ pn, ])),)))
def main(bdtfile,
scoretype='BDeu',
ess=1.0,
time=None,
iters=None,
outfile=None,
constraint_file="",
startbn=None,
cachefile=None):
global searchtime
cstrs = Constraints(constraint_file)
if startbn != None:
bn = bnmodule.load(startbn, do_pic=False)
sc = scorefactory.getscorer(bdtfile,
scoretype,
ess,
cachefile=cachefile)
else:
bn, sc = bnsearch.empty_net(bdtfile,
scoretype,
ess,
cachefile=cachefile)
if constraint_file: # should check if compatible with start
for a in cstrs.must:
bn.addarc(a)
sc.score_new(bn)
bn.picall()
searchtime = sigpool.str2time(time)
endtime = tim.time() + searchtime
t0 = find_initial_temperature(bn, sc)
isss = {
"searchtime": searchtime,
"endtime": endtime,
"t0": t0,
"t": t0
}
isss.update(bnsearch.initial_search_status(bn, sc))
bnsearch.localsearch(isss, sa_step, sa_stop)
if outfile:
isss["best_bn"].save(outfile)
print isss["best_score"]
def __init__(self, domains, constraint_list):
'''
Initializes the values used by astar and gac.
'''
#A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
#GAC INFO
self.constraints = Constraints("x!=y", ["x", "y"], constraint_list)
self.domains = domains
#init gac
self.gac = GAC()
def get_elite(bnfile,
bdtfile,
level,
topN,
resdir,
scoretype='BDeu',
ess=1.0,
constraint_file="",
cachefile=None):
cstrs = Constraints(constraint_file)
bn0 = bn.bn.load(bnfile)
sc = scorefactory.getscorer(data.Data(bdtfile),
scoretype,
ess,
cachefile=cachefile)
return nsearch(bn0, sc, level, topN, cstrs)
def hpcrawl(bn0s, sc, cstrs=None, topN=0, dump_worklist=False):
if cstrs == None: cstrs = Constraints('')
varc = bn0s[0].varc
worklist = []
workset = set()
for bn0 in bn0s:
sc.score_new(bn0)
bn0as = bn0.arcs()
worklist.append((-sc.score(), bn0as))
workset.add(bn0as)
heapify(worklist)
doneset = set()
c = 0
while True:
if len(workset) == 0: break
if topN > 0 and c == topN: break
(hpsc, hpbnas) = heappop(worklist)
workset.remove(hpbnas)
if hpbnas in doneset: continue
doneset.add(hpbnas)
yield (hpbnas, -hpsc)
c += 1
for (s, (nbr, seq)) in nsearch(bnmodule.BN(varc, hpbnas), sc, 1, 0,
cstrs):
nbras = nbr.arcs()
if nbras not in doneset and nbras not in workset:
heappush(worklist, (-s, nbras))
workset.add(nbras)
if dump_worklist and (c + len(workset)) >= topN: break
if dump_worklist:
while c < topN and len(worklist) > 0:
(hpsc, hpbnas) = heappop(worklist)
workset.remove(hpbnas)
if hpbnas in doneset: continue
doneset.add(hpbnas)
yield (hpbnas, -hpsc)
c += 1
def __init__(self, element, cfr, stack):
#TODO clean up unnecessary fields
self.element = element
self.cfr = cfr
self.parentStack = stack[:]
(_, upper) = self.element.card
if upper.value == -1:
self.unbounded = True
else:
self.unbounded = False
if (not self.parentStack):
self.isTopLevel = True
else:
self.isTopLevel = False
self.fields = []
self.constraints = Constraints.ClaferConstraints(self)
self.summs = []
self.refs = []
self.subs = []
self.full = None
self.marked = False
self.instanceRanges = []
self.knownPolarities = []
self.indexInSuper = 0
self.indexInHighestSuper = 0
self.highestSuperSort = self
self.currentSubIndex = 0
self.scope_summ = -1
self.numInstances = int(self.element.glCard[1].value)
if (self.numInstances == -1):
newScope = Options.GLOBAL_SCOPE
self.numInstances = newScope
#lower and upper cardinality bounds
self.lowerCardConstraint = self.element.card[0].value
self.upperCardConstraint = self.element.card[1].value
if not self.parentStack:
self.parent = None
self.parentInstances = 1
else:
self.parent = self.parentStack[-1]
self.parentInstances = self.parent.numInstances
self.beneathAnAbstract = self.element.isAbstract or (True in [
(p.element.isAbstract) for p in self.parentStack
])
def __init__(self, domains, constraint_list):
"""
Initializes information, and sets the constraint formula.
"""
# A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
# GAC INFO
self.constraints = Constraints(
"column[y_index] == row[x_index]", ["column", "row", "x_index", "y_index"], constraint_list
)
self.domains = domains
# init gac
self.gac = Gac_nonogram()
def stepper_f(actions, better, accept, cstrs=None):
if not cstrs:
cstrs = Constraints()
def stepf(search_status):
scr = search_status["scr"]
bn = search_status["curr_bn"]
curr_score = search_status["curr_score"]
search_status["iters"] += 1
try:
aname, action = choice(acts["tryacts"].items())
changes, avars = action(bn, cstrs=cstrs)
search_status["t_no_acts"] = 0
map(scr.storevar, avars)
for avar in avars: scr.score_new_v(bn, avar)
new_score = scr.score()
if better(new_score, search_status["best_score"]):
search_status["best_score"] = new_score
search_status["best_bn"] = bn.copy()
search_status["t_no_impr"] = 0
else:
search_status["t_no_impr"] += 1
if accept(new_score, search_status):
search_status["curr_score"] = new_score
scr.clearstore()
acts["commits"][aname](bn, changes)
search_status["t_no_acps"] = 0
else :
search_status["t_no_acps"] += 1
acts["cancels"][aname](bn,changes)
scr.restore()
except NoAction :
search_status["t_no_acts"] += 1
return stepf
def init(self, conf, props, globdat):
myProps = props.getProps(self.name)
myConf = conf.makeProps(self.name)
modules = myProps.get("modules")
myConf.set("modules", modules)
for module in modules:
type = myProps.get("{}.type".format(module))
if type == "Gmsh" or type == "XML":
mesh = Mesh(myConf, myProps)
globdat.set("mesh", mesh)
elif type == "Loads":
load = LoadTable(module, myConf, myProps)
globdat.set(module, load)
elif type == "Constraints":
cons = Constraints(module, myConf, myProps)
globdat.set(module, cons)
else:
raise ValueError("Unknown input type: {}".format(type))
return Status.DONE
def main(bdtfile,
scoretype='BDeu',
ess=1.0,
cachefile=None,
bnfile=None,
direction='up',
constraint_file=""):
cstrs = Constraints(constraint_file)
if bnfile == None:
bns, scr = empty_net(bdtfile, scoretype, ess, cachefile=cachefile)
else:
bns = bn.bn.load(bnfile, do_pic=True)
bdt = data.Data(bdtfile)
scr = scorefactory.getscorer(bdt, scoretype, ess, cachefile=cachefile)
scr.score_new(bns)
updown = {
'up': {
'changef': bns.addarc,
'cancelf': bns.delarc,
'tabuset': cstrs.no,
'candarcs': bns.new_dagarcs,
'actname': 'add'
},
'down': {
'changef': bns.delarc,
'cancelf': bns.addarc,
'tabuset': cstrs.must,
'candarcs': bns.arcs,
'actname': 'del'
}
}
print 'init', -1, -1, scr.score()
params = updown[direction]
for sdiff, arc in gsteps(bns, scr, params):
scr.score_new(bns) # needed?
print params['actname'], arc[0], arc[1], scr.score()
def __init__(self, module):
Common.reset() #resets variables if in test mode
self.EMPTYSTRING = SMTLib.SMT_Int("EMPTYSTRING")
self.module = module
self.smt_bracketed_constraints = []
self.cfr_sorts = {}
self.solver = BaseSolver.getSolver()
self.setOptions()
self.clock = Clock.Clock()
self.objectives = []
self.delimeter_count = 0
""" Create simple objects used to store Z3 constraints. """
self.join_constraints = Constraints.GenericConstraints("ClaferModel")
"""
Used to map constraints in the UNSAT core to Boolean variables.
"""
self.unsat_core_trackers = []
self.low_level_unsat_core_trackers = {}
self.unsat_map = {}
self.join_cache = {} # cache to store join expression (asil optimization)
self.translate()
################################################# HEURISTICS
sample_space = [(x0[0], goal[0]), (x0[1], goal[1]), (0, 0),
(0.9 * velmax_pos_plan[0], velmax_pos_plan[0]),
(-abs(velmax_neg_plan[1]), velmax_pos_plan[1]),
(-abs(velmax_neg_plan[2]), velmax_pos_plan[2])]
goal_bias = [0.2, 0.2, 0, 0, 0, 0]
FPR = 0.9
################################################# PLAN
constraints = Constraints(nstates=nstates,
ncontrols=ncontrols,
goal_buffer=goal_buffer,
is_feasible=is_feasible)
planner = Planner(dynamics,
lqr,
constraints,
horizon=2,
dt=0.1,
FPR=FPR,
error_tol=error_tol,
erf=erf,
min_time=2,
max_time=3,
max_nodes=1E5,
goal0=goal)
def setUp(self):
self.forwardchecking = ForwardChecking(Constraints(8))
def test_backtracking_can_find_a_larger_solution(self):
problem = Variables(8)
forwardchecking = ForwardChecking(Constraints(8))
forwardchecking.findSolution(problem)
for queen in problem.queens:
self.assertNotEqual(-1, queen.value)
def __init__(self, infilename, n_results):
self.n_results = n_results
self.constraints = Constraints(infilename)
self.n_dim = self.constraints.get_ndim()
self.example = self.constraints.example
def __init__(self, table, constraints, fragments):
self._table, self.tuples = table, len(table)
self._fragments = map(table.to_indices, fragments)
self._constraints = Constraints(map(table.to_indices, constraints), self._fragments)
class Loose:
def __init__(self, table, constraints, fragments):
self._table, self.tuples = table, len(table)
self._fragments = map(table.to_indices, fragments)
self._constraints = Constraints(map(table.to_indices, constraints), self._fragments)
def _get_group_data(self, fragment_id, group_id):
return ((row_id, self._table[row_id]) for row_id in self._associations.get_group(fragment_id, group_id))
def _get_nonfull_groups(self, fragment_id, allow_skips=True, **kwargs):
groups_to_check = xrange(self._first_nonfull[fragment_id], self._last_usable[fragment_id] + 1)
return (group_id for group_id in groups_to_check
if not self._associations.is_group_full(fragment_id, group_id))
# uncomment this to allow skips
# if not allow_skips or not self._skip_probability or random() > self._skip_probability)
def _are_groups_alike(self, current_group, other_group, fragment_id, constraint_id, current_row):
current_rows = self._get_group_data(fragment_id, current_group)
if current_row:
current_rows = chain([(0, current_row)], current_rows)
for _, row1 in current_rows:
for _, row2 in self._get_group_data(fragment_id, other_group):
if self._constraints.are_rows_alike_for(row1, row2, fragment_id, constraint_id):
return True
return False
def _check_group_heterogenity(self, row, fragment_id, group_id):
for _, other_row in self._get_group_data(fragment_id, group_id):
if self._constraints.are_rows_alike(row, other_row, fragment_id):
# logging.trace('GROUP VIOLATED fragment {} group {}'.format(fragment_id, group_id))
return False
return True
def _check_association_heterogenity(self, association, fragment_id, group_id):
for other_fragment, other_group in enumerate(association):
if other_fragment != fragment_id:
if self._associations.exists(fragment_id, group_id, other_fragment, other_group):
# logging.trace('ASSOCIATION VIOLATED fragment {} group {}'.format(fragment_id, group_id))
return False
return True
def _check_deep_heterogenity(self, row, association, fragment_id, group_id):
return (self._check_deep_heterogenity_of_association(row, association, fragment_id, group_id) and
self._check_deep_heterogenity_of_associated_groups(row, fragment_id, group_id))
def _check_deep_heterogenity_of_association(self, row, association, fragment_id, group_id):
for constraint_id in self._constraints.constraints_for(fragment_id):
for fragment1 in self._constraints.involved_fragments_for(constraint_id):
group1 = group_id if (fragment1 == fragment_id) else -1 if fragment1 >= len(association) else association[fragment1]
for other_association in self._associations.get_associated(fragment1, group1):
if other_association != association:
for fragment2 in self._constraints.involved_fragments_for(constraint_id):
if fragment2 != fragment1:
group2 = group_id if (fragment2 == fragment_id) else -1 if fragment2 >= len(association) else association[fragment2]
if not self._are_groups_alike(group2, other_association[fragment2], fragment2, constraint_id, row):
break
else:
# logging.trace("DEEP ASSOCIATION VIOLATED fragment {} group {}".format(fragment_id, group_id))
return False
return True
def _check_deep_heterogenity_of_associated_groups(self, row, fragment_id, group_id):
constraints_for = set(self._constraints.constraints_for(fragment_id))
# take all the pre-existing associations in (fragment_id, group_id)
for association1 in self._associations.get_associated(fragment_id, group_id):
# for every fragment1
for fragment1 in xrange(len(self._fragments)):
# which is not fragment_id
if fragment1 != fragment_id:
# I already know which constraints can break
# only the ones that insists on both fragment_id and fragment1
constraints_to_check = constraints_for & set(self._constraints.constraints_for(fragment1))
# take another association, associated with (fragment1 , association1[fragment1])
for association2 in self._associations.get_associated(fragment1, association1[fragment1]):
# which is not association1
if association1 != association2:
# for every constraint that can break
for constraint_id in constraints_to_check:
# there must exists a fragment2 (insisting on the constraint)
for fragment2 in self._constraints.involved_fragments_for(constraint_id):
# which is not fragment2
if fragment2 != fragment1:
# fow which the constraint holds
if not self._are_groups_alike(association1[fragment2], association2[fragment2],
# if fragment2 is the same as fragment_id, we inform the method are_groups_alike
# about the fact that we want to insert the tuple in that fragment
fragment2, constraint_id, row if (fragment2 == fragment_id) else None):
break
else:
# logging.trace("INNER {} {}".format(association1, association2))
return False
return True
# fragment_id is needed because it's not always bound to the association length. It is in extend_association
# but not in redistribute_group. There we want to check just a single fragment from an already complete association.
def _check_heterogenity(self, row, association, fragment_id, group_id):
return (self._check_group_heterogenity(row, fragment_id, group_id) and
self._check_association_heterogenity(association, fragment_id, group_id) and
self._check_deep_heterogenity(row, association, fragment_id, group_id))
def _extend_association(self, row, row_id, association=[]):
fragment_id = len(association)
if (fragment_id == len(self._fragments)):
return association
for group_id in self._get_nonfull_groups(fragment_id, row_id=row_id):
if self._check_heterogenity(row, association, fragment_id, group_id):
result = self._extend_association(row, row_id, association + [group_id])
if result is not None:
return result
def _full_neighbours_groups(self, fragment_id, group_id, step=1):
return (new_group_id for new_group_id in neighbours(group_id, 0, self._last_usable[fragment_id] + 1)
if self._associations.is_group_full(fragment_id, new_group_id))
def _redistribute_group(self, fragment_id, group_id, step=1):
logging.debug('redistributing group {} in fragment {}'.format(group_id, fragment_id))
for row_id, row in self._get_group_data(fragment_id, group_id):
association = self._associations[row_id]
if association:
for new_group_id in self._full_neighbours_groups(fragment_id, group_id, step):
if self._check_heterogenity(row, association, fragment_id, new_group_id):
logging.debug('fragment {} row {}: group {} -> group {}'.format(fragment_id, row_id, group_id, new_group_id))
new_association = list(association)
new_association[fragment_id] = new_group_id
self._associations[row_id] = new_association
break
else:
logging.debug('fragment {} row {} : group {} -> not reallocable'.format(fragment_id, row_id, group_id))
self._opstack.append((1, (row_id, step))) # delete
def _delete_row(self, row_id, step=1):
self._dropped.add(row_id)
association = self._associations[row_id]
logging.debug('deleting row {} = {}'.format(row_id, association))
del self._associations[row_id]
for fragment_id, group_id in enumerate(association):
if not self._associations.is_group_full(fragment_id, group_id):
self._opstack.append((0, (fragment_id, group_id, step + 1))) # redistribute
logging.info('row {} deleted (step {})'.format(row_id, step))
def _update_first_nonfull(self, association):
for fragment_id, group_id in enumerate(association):
if group_id == self._first_nonfull[fragment_id]:
while (self._associations.is_group_full(fragment_id, self._first_nonfull[fragment_id])):
self._first_nonfull[fragment_id] += 1
def _update_last_usable(self, association):
for fragment_id, group_id in enumerate(association):
if group_id >= self._last_usable[fragment_id]:
self._last_usable[fragment_id] = group_id + 1
def _update_pointers(self, association):
self._update_first_nonfull(association)
self._update_last_usable(association)
def associate(self, k_list, skip_probability=0, **kwargs):
assert(len(k_list) == len(self._fragments))
print_stats_every = kwargs.get('print_stats_every', 1000)
self._k_list = k_list
self._first_nonfull = [0 for _ in xrange(len(self._fragments))]
self._last_usable = [0 for _ in xrange(len(self._fragments))]
self._associations = Associations(self._k_list)
self._dropped = set()
self._skip_probability = skip_probability
withtime = make_withtime()
for row_id, row in enumerate(self._table):
if row_id and not row_id % print_stats_every:
logging.info(withtime('associating row: %i firsts: %s lasts: %s'
% (row_id, self._first_nonfull, self._last_usable)))
association = self._extend_association(row, row_id)
if association is None:
self._dropped.add(row_id)
logging.info('row_id {} dropped at first scan'.format(row_id))
else:
self._associations[row_id] = association
self._update_pointers(association)
self._opstack = deque()
for fragment_id in xrange(len(self._fragments)):
for group_id in self._get_nonfull_groups(fragment_id, False):
if self._associations.get_group_size(fragment_id, group_id):
self._opstack.append((0, (fragment_id, group_id))) # redistribute
counter = count()
while self._opstack:
if not next(counter) % 10:
logging.info(withtime('opstack length: %i' % len(self._opstack)))
operation = self._opstack.pop()
fn = self._delete_row if operation[0] else self._redistribute_group
fn(*operation[1])
logging.info(withtime('done after %i stack operations' % (next(counter))))
return self._associations, self._dropped
def associate_with_retries(self, k_list, retries, skip_probability=0.05, **kwargs):
for i in xrange(retries):
associations, dropped = self.associate(k_list, skip_probability, **kwargs)
if not dropped:
logging.info('Found after {} iterations\nSolution: {}'.format(i, associations))
return associations, i
logging.info('No solution found')
def print_statistics(self):
for fragment_id in xrange(len(self._fragments)):
print '\nFragment {}'.format(fragment_id)
for length, count in Counter(group_size for group_id in xrange(self._last_usable[fragment_id])
for group_size in (self._associations.get_group_size(fragment_id, group_id),)
if group_size is not 0).items():
print '{} elements: {} groups'.format(length, count)
print '\n{} ({:.3%}) lines dropped: {}'.format(len(self._dropped), float(len(self._dropped)) / self.tuples, self._dropped)
class Probleminstance:
"""
Class holding a state of the problem, with domains and constraints. Also used to represent a node in astar.
"""
def __init__(self, domains, constraint_list):
"""
Initializes information, and sets the constraint formula.
"""
# A* INFO
self.h = 0
self.g = 0
self.f = 0
self.predecessor = None
self.neighbours = []
# GAC INFO
self.constraints = Constraints(
"column[y_index] == row[x_index]", ["column", "row", "x_index", "y_index"], constraint_list
)
self.domains = domains
# init gac
self.gac = Gac_nonogram()
def initialize(self):
"""
Initializes, and runs the initial domain filtering loop.
"""
self.gac.initialize(self.domains, self.constraints)
self.domains = self.gac.domain_filtering_loop()
self.astar = Astar(self)
def solve(self):
"""
Iteratively ran to find new states.
"""
self.current = self.astar.solve("A*")
return [self.current, self.astar.prev_current]
def is_solution(self):
"""
Returns true if this state is a solution state, false if not.
"""
for domain in self.domains:
if len(self.domains[domain]) != 1:
return False
return True
def get_neighbours(self):
"""
Returns the neighbours of this state.
"""
minlen = float("inf")
current_domain = None
neighbours = list()
for domain in self.domains:
if len(self.domains[domain]) == 1:
continue
if (len(self.domains[domain])) < minlen:
minlen = len(self.domains[domain])
current_domain = domain
for variation in self.domains[current_domain]:
copy_domains = copy.deepcopy(self.domains)
copy_domains[current_domain] = [variation]
copy_domains = self.gac.rerun(copy_domains, current_domain, self.constraints)
pi = Probleminstance(copy_domains, self.constraints.involved)
neighbours.append(pi)
self.neighbours = neighbours
return neighbours
def get_arc_cost(self):
"""
Returns the cost of moving from this state. Always 1 in this problem.
"""
return 1
def get_h(self):
"""
Returns the h value, based on the amount of possible values for each row and column.
"""
h = 0
for domain in self.domains:
h += len(self.domains[domain])
self.h = h
return h
def is_illegal(self):
"""
Returns true if this is a legal state, false if not.
"""
for node in self.domains:
if self.domains[node] == []:
return True
for constraint_node in self.constraints.involved[node]:
legal = False
for x_domain in self.domains[node]:
for y_domain in self.domains[constraint_node]:
x_index = node[1]
y_index = constraint_node[1]
if self.constraints.expression(x_domain, y_domain, x_index, y_index):
legal = True
if legal == False:
return True
return False
def __lt__(self, other):
"""
Less than comparison, compares on f primarily, h secondarily. Used by astar.
"""
if self.f == other.f:
return self.h < other.h
return self.f < other.f
def __eq__(self, other):
"""
Equals operator, checks if the domains are equal.
"""
return self.domains == other.domains
def __str__(self):
"""
String representation of this state's domains.
"""
return str(self.domains)
class SimulationEnvironment():
def __init__(self):
# Initializes all pygame functionality.
pygame.init()
#needed to display text in a font
pygame.font.init()
self.myfont = pygame.font.SysFont('Comic Sans MS', 30)
# Set the size of the window, variables can be used for positioning
# within program.
window_width = 1000
window_height = 1000
# Creates the window and puts a Surface into "screen".
self.screen = pygame.display.set_mode((window_width, window_height))
# Sets title of window, not needed for the game to run but
# unless you want to try telling everyone your game is called
# "Game Title", make sure to set the caption :)
pygame.display.set_caption("Vehicle_Simulation")
#Create object of clock used for timing within the program.
self.clock = pygame.time.Clock()
self.timer_key_select = pygame.time.get_ticks()
self.computation_time = pygame.time.get_ticks()
self.simulation_time = pygame.time.get_ticks()
self.simulation_steps = 1
self.complexTrack = False
self.plotTrack = False
#is the mpcActive?
self.mpcActive = False
#shoud the path of the car be plottet?
self.plotCarPath = False
#is loop done? should always be False till termination
self.loopSimDone = False
self.X0 = np.zeros(8)
#init car_path
self.carPath = []
self.carPath.append([self.X0[0], self.X0[1], self.X0[2]])
#init rest of the required classes for mpc
self.init_vehicle(0.1)
self.init_track()
self.init_state_vector()
self.mpc_key_counter = 0
self.init_mpc(5)
def init_vehicle(self, dt):
#load race_car image
raceCarImage = pygame.image.load("race_car.png").convert()
self.surf = pygame.transform.scale(raceCarImage, (20, 18))
self.surf_rect = self.surf.get_rect(center=(10, 9))
self.vehicleModel = VehicleModel(dt)
def init_track(self):
self.raceTrack = RaceTrack()
def init_state_vector(self, N=1, X=[0, 0, 0.1, math.pi / 2, 0, 0, 0, 0]):
self.X0 = np.zeros(8 * (N + 1))
self.X0[0] = X[0]
self.X0[1] = X[1]
self.X0[2] = X[2]
self.X0[3] = X[3]
self.X0[4] = X[4]
self.X0[5] = X[5]
self.X0[6] = X[6]
self.X0[7] = X[7]
print("X02", self.X0)
def init_mpc(self, N):
#self.init_vehicle(0.1)
self.init_track()
self.costFunction = CostFunction(self.raceTrack)
self.bnds = self.vehicleModel.get_bounds(N)
self.init_state_vector(N, [0, 0, 0.1, math.pi / 2, 0, 0, 0, 0])
print self.vehicleModel.get_dt()
self.constraints = Constraints(self.X0[:6], self.vehicleModel,
self.raceTrack)
self.constraints.ineq_constraint_vehicle_bounds_set_tangent_points(
self.X0)
self.cons = (
{
'type': 'eq',
'fun': self.constraints.constraint_fix_init_state
},
{
'type': 'eq',
'fun': self.constraints.constraint_vehicle_model
},
#{'type': 'ineq', 'fun': self.constraints.ineq_constraint_vehicle_model},
{
'type': 'ineq',
'fun': self.constraints.ineq_constraint_vehicle_bounds
})
#self.jac = self.costFunction.cost_dist_track_speed_jac()
self.computation_time = pygame.time.get_ticks()
self.simulation_time = pygame.time.get_ticks()
self.simulation_steps = 1
def reset_car_position(self):
self.X0[:8] = [0, 0, 0.1, math.pi / 2, 0, 0, 0, 0]
self.X0[8:] = 0
self.carPath = []
self.carPath.append([self.X0[0], self.X0[1], self.X0[2]])
def handle_keystrokes(self):
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.loopSimDone = True
pressed = pygame.key.get_pressed()
if (pygame.time.get_ticks() - self.timer_key_select >= 500):
if pressed[pygame.K_r]:
self.reset_car_position()
self.timer_key_select = pygame.time.get_ticks()
if pressed[pygame.K_p]:
self.plotTrack = not self.plotTrack
self.timer_key_select = pygame.time.get_ticks()
if pressed[pygame.K_c]:
self.timer_key_select = pygame.time.get_ticks()
self.complexTrack = not self.complexTrack
if (self.complexTrack): self.raceTrack.complex_track()
else: self.raceTrack.simple_track()
if pressed[pygame.K_m]:
if (self.mpc_key_counter == 0):
self.vehicleModel.set_dt(0.1)
self.mpc_key_counter = self.mpc_key_counter + 1
self.timer_key_select = pygame.time.get_ticks()
self.mpcActive = not self.mpcActive
else:
self.mpc_key_counter = 0
if pressed[pygame.K_l]:
self.timer_key_select = pygame.time.get_ticks()
self.plotCarPath = not self.plotCarPath
if (not self.mpcActive):
self.X0[6] = 0
self.X0[7] = 0
if pressed[pygame.K_UP]:
self.X0[6] = self.vehicleModel.get_max_acc()
if pressed[pygame.K_DOWN]:
self.X0[6] = -self.vehicleModel.get_max_dec()
if pressed[pygame.K_LEFT]:
self.X0[7] = self.vehicleModel.get_max_steer_angle()
if pressed[pygame.K_RIGHT]:
self.X0[7] = -self.vehicleModel.get_max_steer_angle()
def plot_race_track(self):
if (self.plotTrack):
tck_bounds_left = self.raceTrack.get_spline_tck_bnds_left()
tck_bounds_right = self.raceTrack.get_spline_tck_bnds_right()
if (self.raceTrack.get_track_points().__len__() > 50):
steps = np.arange(0, 1, 0.0001)
else:
steps = np.arange(0, 1, 0.001)
bounds_left = interpolate.splev(steps, tck_bounds_left[0], der=0)
bounds_right = interpolate.splev(steps, tck_bounds_right[0], der=0)
for i in range(0, steps.__len__(), 1):
self.screen.set_at((int(bounds_left[0][i] * 10 + 150),
-int(bounds_left[1][i] * 10) + 350),
(0, 125, 255))
self.screen.set_at((int(bounds_right[0][i] * 10 + 150),
-int(bounds_right[1][i] * 10) + 350),
(0, 125, 255))
def plot_car_path(self):
if (self.plotCarPath):
for i in range(0, self.carPath.__len__(), 1):
speed = int(math.fabs(self.carPath[i][2] * 2 * 5))
if (speed > 255): speed = 255
self.screen.set_at((int(self.carPath[i][0] * 10 + 150),
-int(self.carPath[i][1] * 10) + 350),
(speed, 255 - speed, 0))
def plot_predicted_path(self, x):
n = x.size / 8
for i in range(0, n, 1):
self.screen.set_at(
(int(x[i * 8] * 10 + 150), -int(x[i * 8 + 1] * 10) + 350),
(255, 0, 255))
def set_car_path(self):
if (not self.mpcActive):
if (self.carPath.__len__() >= 1000):
self.carPath.pop(0)
if (math.sqrt((self.X0[0] - self.carPath[-1][0])**2 +
(self.X0[1] - self.carPath[-1][1])**2) > 0.2):
self.carPath.append([self.X0[0], self.X0[1], self.X0[2]])
else:
if (self.carPath.__len__() >= 1000):
self.carPath.pop(0)
self.carPath.append([self.X0[0], self.X0[1], self.X0[2]])
def display_menu(self):
textsurface = self.myfont.render('Reset Car: r', False,
(255, 255, 255))
self.screen.blit(textsurface, (20, 970))
textsurface = self.myfont.render('Plot Racetrack: p', False,
(255, 255, 255))
self.screen.blit(textsurface, (170, 970))
textsurface = self.myfont.render('Change Track: c', False,
(255, 255, 255))
self.screen.blit(textsurface, (380, 970))
textsurface = self.myfont.render('Activate MPC: m', False,
(255, 255, 255))
self.screen.blit(textsurface, (570, 970))
textsurface = self.myfont.render('Plot Car Path: l', False,
(255, 255, 255))
self.screen.blit(textsurface, (810, 970))
def display_simulation_info(self, res):
self.computation_time = pygame.time.get_ticks() - self.computation_time
self.full_simulation_time = pygame.time.get_ticks(
) - self.simulation_time
textsurface = self.myfont.render(
'Solver: feasible: ' + repr(res.success).format(self.X0[2] * 3.6),
False, (255, 255, 255))
self.screen.blit(textsurface, (020, 900))
textsurface = self.myfont.render(
'Solver: iterations: ' + repr(res.nit).format(self.X0[2] * 3.6),
False, (255, 255, 255))
self.screen.blit(textsurface, (020, 870))
textsurface = self.myfont.render(
'Solver: nfev: ' + repr(res.nfev).format(self.X0[2] * 3.6), False,
(255, 255, 255))
self.screen.blit(textsurface, (020, 840))
textsurface = self.myfont.render(
'Solver: time to compute: ' + repr(self.computation_time) +
"ms".format(self.X0[2] * 3.6), False, (255, 255, 255))
self.screen.blit(textsurface, (020, 810))
textsurface = self.myfont.render(
'Solver: median time to compute: ' +
repr(self.full_simulation_time / self.simulation_steps) +
"ms".format(self.X0[2] * 3.6), False, (255, 255, 255))
self.screen.blit(textsurface, (020, 780))
self.computation_time = pygame.time.get_ticks()
def display_car_info(self, acc_, steer):
textsurface = self.myfont.render(
'Speed: {0:.3f} km/h'.format(self.X0[2] * 3.6), False,
(255, 255, 255))
self.screen.blit(textsurface, (020, 930))
acc = acc_
rect = pygame.Rect(890, 499, 60, 2)
self.screen.fill((255, 255, 255), rect)
rect = pygame.Rect(890, 649, 60, 2)
self.screen.fill((255, 255, 255), rect)
rect = pygame.Rect(890, 349, 60, 2)
self.screen.fill((255, 255, 255), rect)
if (acc > 0):
#norm acceleration to 1
tmp = acc / self.vehicleModel.get_max_acc()
size_max = 150
rect = pygame.Rect(900, 500 - int(size_max * tmp), 40,
int(size_max * tmp))
self.screen.fill((int(tmp * 255), 255 - int(255 * tmp), 0), rect)
else:
#norm acceleration to 1
tmp = -acc / self.vehicleModel.get_max_acc()
size_max = 150
rect = pygame.Rect(900, 500, 40, int(size_max * tmp))
self.screen.fill((int(tmp * 255), 255 - int(255 * tmp), 0), rect)
rect = pygame.Rect(700, 700, 2, 50)
self.screen.fill((255, 255, 255), rect)
rect = pygame.Rect(800, 700, 2, 50)
self.screen.fill((255, 255, 255), rect)
rect = pygame.Rect(900, 700, 2, 50)
self.screen.fill((255, 255, 255), rect)
if (steer < 0):
#norm acceleration to 1
tmp = -steer / self.vehicleModel.get_max_steer_angle()
size_max = 100
rect = pygame.Rect(800, 710, int(size_max * tmp), 30)
self.screen.fill((int(tmp * 255), 255 - int(255 * tmp), 0), rect)
else:
#norm acceleration to 1
tmp = steer / self.vehicleModel.get_max_steer_angle()
size_max = 100
rect = pygame.Rect(800 - int(size_max * tmp), 710,
int(size_max * tmp), 30)
self.screen.fill((int(tmp * 255), 255 - int(255 * tmp), 0), rect)
def plot_track_bound_constraint(self, X):
N = X.size / 8
for i in range(0, N, 3):
arc = self.raceTrack.get_bnd_left_spline_arc_pos(X[i * 8:i * 8 +
2])
#left bound line
tck, u = self.raceTrack.get_spline_tck_bnds_left()
x, y = interpolate.splev(arc - 0.004, tck, der=0)
p1 = np.array([x, y])
x, y = interpolate.splev(arc + 0.004, tck, der=0)
p2 = np.array([x, y])
angle = np.arctan((p2[1] - p1[1]) / (p2[0] - p1[0]))
p3 = np.array(
[p1[0] + 7 * np.cos(angle), p1[1] + 7 * np.sin(angle)])
p4 = np.array(
[p1[0] - 7 * np.cos(angle), p1[1] - 7 * np.sin(angle)])
p4 = np.array([p4[0] * 10 + 150, -p4[1] * 10 + 350])
p3 = np.array([p3[0] * 10 + 150, -p3[1] * 10 + 350])
pygame.draw.line(self.screen, (255, 0, 0), p3, p4, 1)
#right bound line
arc = self.raceTrack.get_bnd_right_spline_arc_pos(X[i * 8:i * 8 +
2])
tck, u = self.raceTrack.get_spline_tck_bnds_right()
x, y = interpolate.splev(arc - 0.004, tck, der=0)
p1 = np.array([x, y])
x, y = interpolate.splev(arc + 0.004, tck, der=0)
p2 = np.array([x, y])
angle = np.arctan((p2[1] - p1[1]) / (p2[0] - p1[0]))
p3 = np.array(
[p1[0] + 7 * np.cos(angle), p1[1] + 7 * np.sin(angle)])
p4 = np.array(
[p1[0] - 7 * np.cos(angle), p1[1] - 7 * np.sin(angle)])
p4 = np.array([p4[0] * 10 + 150, -p4[1] * 10 + 350])
p3 = np.array([p3[0] * 10 + 150, -p3[1] * 10 + 350])
pygame.draw.line(self.screen, (255, 0, 0), p3, p4, 1)
def rotate(self, image, rect, angle):
"""Rotate the image while keeping its center."""
# Rotate the original image without modifying it.
new_image = pygame.transform.rotate(image, angle)
# Get a new rect with the center of the old rect.
rect = new_image.get_rect(center=rect.center)
return new_image, rect
def simulate(self):
while not self.loopSimDone:
self.handle_keystrokes()
#reset canvas
self.screen.fill((0, 0, 0))
rect = self.surf_rect
if (not self.mpcActive):
self.plot_race_track()
self.plot_car_path()
#plot racecar
self.vehicleModel.set_dt(self.clock.get_time() / 1000.0)
#self.X0[0:6] = self.vehicleModel.compute_next_state_long_(self.X0[0:8])
#self.X0[0:6] = self.vehicleModel.dynamic_model(self.X0[0:8])
self.X0[0:6] = self.vehicleModel.kamsches_model(self.X0[0:8])
#self.display_car_info(self.X0[6], self.X0[7])
self.set_car_path()
#rotate car picture
#car_pic = pygame.transform.rotate(self.surf, self.X0[3] * 180/math.pi)
pic, rect = self.rotate(self.surf, rect,
self.X0[3] * 180 / math.pi)
#print("self.X0", self.X0[0])
rect.center = [self.X0[0] * 10 + 150, -self.X0[1] * 10 + 350]
self.screen.blit(pic, rect)
else:
self.plot_race_track()
self.plot_car_path()
res = minimize(self.costFunction.cost_dist_track_speed,
self.X0,
method='SLSQP',
bounds=self.bnds,
constraints=self.cons,
options={
'ftol': 1e-4,
'disp': False
})
#compute movement of car later on real hardware this is piped to the actuators
self.X0[0:6] = self.vehicleModel.compute_next_state_(
res.x[0:8])
#self.X0[0:6] = self.vehicleModel.compute_next_state_long(self.X0[0:8])
#shift the state vector one step to the left and predict last step for constraint handling
self.X0[6:-8] = res.x[14:]
#self.X0[-8:-2] = self.vehicleModel.compute_next_state_(self.X0[-16:-8])
self.X0[-8:-2] = self.vehicleModel.dynamic_model(
self.X0[-16:-8])
self.constraints.ineq_constraint_vehicle_bounds_set_tangent_points(
self.X0)
#print self.costraints.ineq_constraint_vehicle_bounds(self.X0)
print("X0", self.X0[0:8])
#self.raceTrack.set_new_vehicle_positon(self.X0[0:2])
#set new init_state for constraint
self.constraints.set_initial_state(self.X0[0:6])
self.set_car_path()
self.display_car_info(res.x[6], res.x[7])
self.plot_predicted_path(res.x)
self.plot_track_bound_constraint(res.x)
self.display_simulation_info(res)
#used for calculation of median value for solver
self.simulation_steps = self.simulation_steps + 1
#plot racecar
pic, rect = self.rotate(self.surf, rect,
self.X0[3] * 180 / math.pi)
rect.center = [self.X0[0] * 10 + 150, -self.X0[1] * 10 + 350]
self.screen.blit(pic, rect)
self.display_menu()
#render new picture
pygame.display.flip()
# Defines the frame rate. The number is number of frames per second.
self.clock.tick(60)
class ConstraintOptimization:
"""
Main class that samples the high dimensional space
Optimizes the distance between the points sample,
While satisfying the constraints
"""
def __init__(self, infilename, n_results):
self.n_results = n_results
self.constraints = Constraints(infilename)
self.n_dim = self.constraints.get_ndim()
self.example = self.constraints.example
def sample_points(self):
"""
Main function, calls find_new_vector method for the required number of times
"""
objective = Objective(self.n_dim, self.n_results)
objective.add_point(np.array(self.example))
addI = 0
while addI < self.n_results - 1:
if addI % 10 == 0:
print addI
new_vector, count_fail, best_val = self.find_new_vector(objective)
if new_vector is not None:
addI += 1
objective.add_point(new_vector)
# adds to total values for final reporting
# sets min and max objective values for subsequent normalization
objective.total_obj_distance += best_val
objective.add_count_fail(count_fail)
objective.set_max_objective(best_val)
objective.set_min_objective(parameters.OBJECTIVE_METHOD)
else:
print "New vector not found...Consider parameter change if problem exists"
print
print "Total Count Fail ", objective.total_count_fail
print "Total Objective Distance ", objective.total_obj_distance
return objective.sample_points
def find_new_vector(self, objective):
"""
Uses branch and bound algorithm to find next vector that is
far from previous vectors but also obeys all constraints.
Uses priority queue/stack to search greedily for areas that minimizes
the constraint error and maximizes the distance
:param objective: object that contains the previous points
:return: new vector
"""
best_branch = Branch(self.n_dim)
best_branch.set_score_explore(objective, self.constraints)
# starting of with a stack, then a heap
branch_list = [best_branch]
heap = []
prev_vector = None
# keeping track of no. of branches visited using stack (False) and heap (True)
best_obj_distance = count_fail = 0
total_count_fail = {False: 0, True: 0}
# keeping track of branches that most likely do not lead to the right solution
# err_ancestor_info helps deciding whether branches should be made an error ancestor (Details in set_as_err_ancestor_info)
inefficient_ancestors = set()
err_ancestor_info = {'len': 0, 'prev': 0, 'num': 0}
# search ends till the new vector is not that far off from the previously found vector
change_dist_th = math.sqrt(parameters.CHANGE_TH**2 * self.n_dim)
change_dist = sys.float_info.max
while change_dist > change_dist_th and len(branch_list) > 0:
if best_obj_distance > 0:
best_branch = heapq.heappop(branch_list)[1]
else:
best_branch = branch_list.pop()
vector = best_branch.get_center()
count_fail += 1
# do not check if likely not going to find a solution
do_check = best_branch.ancestor not in inefficient_ancestors
if do_check:
if best_branch.is_pass:
if best_branch.obj_distance > best_obj_distance:
if prev_vector is not None:
change_dist = toolbox.calc_distance(
vector, prev_vector)
prev_vector = vector
total_count_fail[best_obj_distance > 0] += count_fail
count_fail = 0
# first solution found, switch to heap
if best_obj_distance == 0:
objective.set_max_objective(
best_branch.obj_distance)
branch_list = heap
best_obj_distance = best_branch.obj_distance
# if solution still not found, consider setting branch as a bad error ancestor
if best_obj_distance == 0:
set_as_err_ancestor(best_branch, err_ancestor_info)
child_branches, is_smallest = best_branch.split_longest()
# two different scoring methods for new branches depending on whether solution is found
for branch in child_branches:
if best_obj_distance > 0:
branch.set_score_diff(objective, self.constraints)
branch.set_ancestor_pass()
else:
branch.set_score_explore(objective, self.constraints)
add_branches_to_list(branch_list, child_branches,
best_obj_distance, is_smallest)
# if branch is small and is not a viable solution, add to set
if is_smallest and not count_fail == 0:
if best_branch.ancestor is not None:
add_inefficient_branches(inefficient_ancestors,
best_branch.ancestor,
err_ancestor_info)
print "obj distance ", best_obj_distance
print prev_vector
total_count_fail[best_obj_distance > 0] += count_fail
return prev_vector, total_count_fail, best_obj_distance
def defaultMakeTransition(self, grasps, nGrasps, ig, priority):
sf = self.makeState(grasps , priority)
st = self.makeState(nGrasps, priority)
names = self._transitionNames(sf, st, ig)
if names in self.transitions:
return
iobj = self.objectFromHandle [nGrasps[ig]]
obj = self.objects[iobj]
noPlace = self._isObjectGrasped (grasps, iobj)
gc = self.graspConstraints (ig, nGrasps[ig])
gcc = self.graspComplementConstraints (ig, nGrasps[ig])
pgc = self.pregraspConstraints (ig, nGrasps[ig])
if noPlace:
pc = Constraints()
pcc = Constraints()
ppc = Constraints()
else:
pc = self.placementConstraints (self.objectFromHandle[nGrasps[ig]])
pcc = self.placementComplementConstraints (self.objectFromHandle[nGrasps[ig]])
ppc = self.prePlacementConstraints (self.objectFromHandle[nGrasps[ig]])
manifold = sf.manifold - pc
# The different cases:
pregrasp = not pgc.empty()
intersec = (not gc.empty()) and (not pc.empty())
preplace = not ppc.empty()
nWaypoints = pregrasp + intersec + preplace
nTransitions = 1 + nWaypoints
nStates = 2 + nWaypoints
def _createWaypointState (name, constraints):
self.graph.createNode (name, True)
self.graph.addConstraints (node = name, constraints = constraints)
return name
# Create waypoint states
intersection = 0
wStates = [ sf.name, ]
if pregrasp:
wStates.append (_createWaypointState (names[0] + "_pregrasp",
pc + pgc + manifold))
if intersec:
wStates.append (_createWaypointState (names[0] + "_intersec",
pc + gc + manifold))
if preplace:
wStates.append (_createWaypointState (names[0] + "_preplace",
ppc + gc + manifold))
wStates.append(st.name)
# Link waypoints
transitions = names[:]
if nWaypoints > 0:
self.graph.createWaypointEdge (sf.name, st.name, names[0], nWaypoints, automaticBuilder = False)
self.graph.createWaypointEdge (st.name, sf.name, names[1], nWaypoints, automaticBuilder = False)
wTransitions = []
for i in range(nTransitions):
nf = "{0}_{1}{2}".format(names[0], i, i+1)
nb = "{0}_{2}{1}".format(names[1], i, i+1)
self.graph.createEdge (wStates[i], wStates[i+1], nf, -1)
self.graph.createEdge (wStates[i+1], wStates[i], nb, -1)
self.graph.graph.setWaypoint (self.graph.edges[transitions[0]],
i, self.graph.edges[nf], self.graph.nodes[wStates[i+1]])
self.graph.graph.setWaypoint (self.graph.edges[transitions[1]],
nTransitions - 1 - i, self.graph.edges[nb], self.graph.nodes[wStates[i]])
wTransitions.append ( (nf, nb) )
# Set states
M = 0 if gc.empty() else 1 + pregrasp
for i in range(M):
self.graph.setContainingNode (wTransitions[i][0], sf.name)
self.graph.addConstraints (edge = wTransitions[i][0], constraints = sf.foliation)
self.graph.setContainingNode (wTransitions[i][1], sf.name)
self.graph.addConstraints (edge = wTransitions[i][1], constraints = sf.foliation)
for i in range(M, nTransitions):
self.graph.setContainingNode (wTransitions[i][0], st.name)
self.graph.addConstraints (edge = wTransitions[i][0], constraints = st.foliation)
self.graph.setContainingNode (wTransitions[i][1], st.name)
self.graph.addConstraints (edge = wTransitions[i][1], constraints = st.foliation)
# Set all to short except first one.
for i in range(nTransitions - 1):
self.graph.setShort (wTransitions[i + 1][0], True)
self.graph.setShort (wTransitions[i ][1], True)
else:
#TODO This case will likely never happen
raise NotImplementedError("This case has not been implemented")
self.graph.createEdge (sf.name, st.name, names[0])
self.graph.createEdge (st.name, sf.name, names[1])
self.transitions.add(names)