def loop_kernel(labels):
# assign cluster
with hcl.for_(0, N, name="n") as n:
min_dist = hcl.scalar(100000)
new_label = hcl.scalar(labels[n])
with hcl.for_(0, K) as k:
dist = hcl.scalar(0)
with hcl.for_(0, dim) as d:
dist_ = hcl.scalar(points[n, d] - means[k, d], "temp")
dist.v += dist_.v * dist_.v
with hcl.if_(dist.v < min_dist.v):
min_dist.v = dist.v
new_label[0] = k
labels[n] = new_label
# update mean
num_k = hcl.compute((K, ), lambda x: 0, "num_k")
sum_k = hcl.compute((K, dim), lambda x, y: 0, "sum_k")
def calc_sum(n):
num_k[labels[n]] += 1
with hcl.for_(0, dim) as d:
sum_k[labels[n], d] += points[n, d]
hcl.mutate((N, ), lambda n: calc_sum(n), "calc_sum")
hcl.update(means, lambda k, d: sum_k[k, d] // num_k[k],
"update_mean")
def transition(sVals, action, bounds, trans, goal):
dx = hcl.scalar(0, "dx")
dy = hcl.scalar(0, "dy")
mag = hcl.scalar(0, "mag")
# Check if moving from a goal state
dx[0] = sVals[0] - goal[0, 0]
dy[0] = sVals[1] - goal[0, 1]
mag[0] = hcl.sqrt((dx[0] * dx[0]) + (dy[0] * dy[0]))
with hcl.if_(
hcl.and_(mag[0] <= 1.0, sVals[2] <= goal[1, 1],
sVals[2] >= goal[1, 0])):
trans[0, 0] = 0
# Check if moving from an obstacle
with hcl.elif_(
hcl.or_(sVals[0] < bounds[0, 0] + 0.2,
sVals[0] > bounds[0, 1] - 0.2)):
trans[0, 0] = 0
with hcl.elif_(
hcl.or_(sVals[1] < bounds[1, 0] + 0.2,
sVals[1] > bounds[1, 1] - 0.2)):
trans[0, 0] = 0
# Standard move
with hcl.else_():
trans[0, 0] = 1.0
trans[0, 1] = sVals[0] + (0.6 * action[0] * hcl.cos(sVals[2]))
trans[0, 2] = sVals[1] + (0.6 * action[0] * hcl.sin(sVals[2]))
trans[0, 3] = sVals[2] + (0.6 * action[1])
# Adjust for periodic dimension
with hcl.while_(trans[0, 3] > 3.141592653589793):
trans[0, 3] -= 6.283185307179586
with hcl.while_(trans[0, 3] < -3.141592653589793):
trans[0, 3] += 6.283185307179586
def spa_derivX3_6d(i, j, k, l, m, n, V,
g): # Left -> right == Outer Most -> Inner Most
left_deriv = hcl.scalar(0, "left_deriv")
right_deriv = hcl.scalar(0, "right_deriv")
with hcl.if_(k == 0):
left_boundary = hcl.scalar(0, "left_boundary")
left_boundary[0] = V[i, j, k, l, m,
n] + my_abs(V[i, j, k + 1, l, m, n] -
V[i, j, k, l, m, n]) * my_sign(
V[i, j, k, l, m, n])
left_deriv[0] = (V[i, j, k, l, m, n] - left_boundary[0]) / g.dx[2]
right_deriv[0] = (V[i, j, k + 1, l, m, n] -
V[i, j, k, l, m, n]) / g.dx[2]
with hcl.elif_(k == V.shape[2] - 1):
right_boundary = hcl.scalar(0, "right_boundary")
right_boundary[0] = V[i, j, k, l, m,
n] + my_abs(V[i, j, k, l, m, n] -
V[i, j, k - 1, l, m, n]) * my_sign(
V[i, j, k, l, m, n])
left_deriv[0] = (V[i, j, k, l, m, n] -
V[i, j, k - 1, l, m, n]) / g.dx[2]
right_deriv[0] = (right_boundary[0] - V[i, j, k, l, m, n]) / g.dx[2]
with hcl.elif_(k != 0 and k != V.shape[2] - 1):
left_deriv[0] = (V[i, j, k, l, m, n] -
V[i, j, k - 1, l, m, n]) / g.dx[2]
right_deriv[0] = (V[i, j, k + 1, l, m, n] -
V[i, j, k, l, m, n]) / g.dx[2]
return left_deriv[0], right_deriv[0]
def optDstb(self, state, spat_deriv):
"""
:param state:
:param spat_deriv:
:return:
"""
dOpt1 = hcl.scalar(0, "dOpt1")
dOpt2 = hcl.scalar(0, "dOpt2")
# Just create and pass back, even though they're not used
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
in5 = hcl.scalar(0, "in5")
# dOpt = (dOpt[0], dOpt[1]) = (w_h, a_h)
with hcl.if_(self.uMode == "max"):
# For dOpt1: w_h
with hcl.if_(spat_deriv[2] > 0):
dOpt1[0] = self.dMin[0]
with hcl.if_(spat_deriv[2] <= 0):
dOpt1[0] = self.dMax[0]
# For dOpt2: a_h
with hcl.if_(spat_deriv[3] > 0):
dOpt2[0] = self.dMin[1]
with hcl.if_(spat_deriv[3] <= 0):
dOpt2[0] = self.dMax[1]
return (dOpt1[0], dOpt2[0], in3[0], in4[0], in5[0])
def opt_ctrl(self, t, state, spat_deriv):
"""
:param t: time t
:param state: tuple of coordinates
:param spat_deriv: tuple of spatial derivative in all dimensions
:return:
"""
# System dynamics
# x_dot = v * cos(theta) + d_1
# y_dot = v * sin(theta) + d_2
# v_dot = a
# theta_dot = v * tan(delta) / L
# Graph takes in 4 possible inputs, by default, for now
opt_a = hcl.scalar(self.uMax[0], "opt_a")
opt_w = hcl.scalar(self.uMax[1], "opt_w")
# Just create and pass back, even though they're not used
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
if self.uMode == "min":
with hcl.if_(spat_deriv[2] > 0):
opt_a[0] = self.uMin[0]
with hcl.if_(spat_deriv[3] > 0):
opt_w[0] = self.uMin[1]
else:
with hcl.if_(spat_deriv[2] < 0):
opt_a[0] = self.uMin[0]
with hcl.if_(spat_deriv[3] < 0):
opt_w[0] = self.uMin[1]
# return 3, 4 even if you don't use them
return (opt_a[0], opt_w[0], in3[0], in4[0])
def opt_ctrl(self, t, state, spat_deriv):
"""
:param spat_deriv: tuple of spatial derivative in all dimensions
state: x1, x2, x3
t: time
:return: a tuple of optimal disturbances
"""
opt_w = hcl.scalar(self.wMax, "opt_w")
# Just create and pass back, even though they're not used
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
#a_term = spat_deriv[0] * self.x[1] - spat_deriv[1]*self.x[0] - spat_deriv[2]
# Declare a variable
a_term = hcl.scalar(0, "a_term")
# use the scalar by indexing 0 everytime
a_term[0] = spat_deriv[0] * state[1] - spat_deriv[1] * state[
0] - spat_deriv[2]
with hcl.if_(a_term >= 0):
with hcl.if_(self.uMode == "min"):
opt_w[0] = -opt_w[0]
with hcl.elif_(a_term < 0):
with hcl.if_(self.uMode == "max"):
opt_w[0] = -opt_w[0]
return (opt_w[0], in3[0], in4[0])
def dynamics(self, t, state, uOpt, dOpt):
"""
:param t:
:param state:
:param uOpt:
:param dOpt:
:return:
"""
x1_dot = hcl.scalar(0, "x1_dot")
x2_dot = hcl.scalar(0, "x2_dot")
x3_dot = hcl.scalar(0, "x3_dot")
x4_dot = hcl.scalar(0, "x4_dot")
x5_dot = hcl.scalar(0, "x5_dot")
# state = (state[0]: , state[1], state[2], state[3], state[4]) = (x_rel, y_rel, psi_rel, v_h, v_r)
# uOpt = (uOpt[0], uOpt[1]) = (beta_r, a_r)
# dOpt = (dOpt[0], dOpt[1]) = (w_h, a_h)
x1_dot[0] = (state[4] / self.l_r) * hcl.sin(
uOpt[0]) * state[1] + state[3] * hcl.cos(
state[2]) - state[4] * hcl.cos(uOpt[0])
x2_dot[0] = (-state[4] / self.l_r) * hcl.sin(
uOpt[0]) * state[0] + state[3] * hcl.sin(
state[2]) - state[4] * hcl.sin(uOpt[0])
x3_dot[0] = dOpt[0] - (state[4] / self.l_r) * hcl.sin(uOpt[0])
x4_dot[0] = dOpt[1]
x5_dot[0] = uOpt[1]
return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0], x5_dot[0])
def optDstb(self, t, state, spat_deriv):
"""
:param spat_deriv: tuple of spatial derivative in all dimensions
state: x1, x2, x3
t: time
:return: a tuple of optimal disturbances
"""
# Graph takes in 4 possible inputs, by default, for now
d1 = hcl.scalar(self.dMax, "d1")
d2 = hcl.scalar(0, "d2")
# Just create and pass back, even though they're not used
d3 = hcl.scalar(0, "d3")
# Declare a variable
b_term = hcl.scalar(0, "b_term")
# use the scalar by indexing 0 everytime
b_term[0] = spat_deriv[2]
with hcl.if_(b_term[0] >= 0):
with hcl.if_(self.dMode == "min"):
d1[0] = -d1[0]
with hcl.elif_(b_term[0] < 0):
with hcl.if_(self.dMode == "max"):
d1[0] = -d1[0]
return (d1[0], d2[0], d3[0])
def reward(sVals, action, bounds, goal, trans):
dx = hcl.scalar(0, "dx")
dy = hcl.scalar(0, "dy")
mag = hcl.scalar(0, "mag")
rwd = hcl.scalar(0, "rwd")
# Check if moving from a collision state, if so, assign a penalty
with hcl.if_(
hcl.or_(sVals[0] < bounds[0, 0] + 0.2,
sVals[0] > bounds[0, 1] - 0.2)):
rwd[0] = -400
with hcl.elif_(
hcl.or_(sVals[1] < bounds[1, 0] + 0.2,
sVals[1] > bounds[1, 1] - 0.2)):
rwd[0] = -400
with hcl.else_():
# Check if moving from a goal state
dx[0] = sVals[0] - goal[0, 0]
dy[0] = sVals[1] - goal[0, 1]
mag[0] = hcl.sqrt((dx[0] * dx[0]) + (dy[0] * dy[0]))
with hcl.if_(
hcl.and_(mag[0] <= 1.0, sVals[2] <= goal[1, 1],
sVals[2] >= goal[1, 0])):
rwd[0] = 1000
# Standard move
with hcl.else_():
rwd[0] = 0
return rwd[0]
def optDstb(self, spat_deriv):
"""
:param spat_deriv: tuple of spatial derivative in all dimensions
:return: a tuple of optimal disturbances
"""
# Graph takes in 4 possible inputs, by default, for now
d1 = hcl.scalar(0, "d1")
d2 = hcl.scalar(0, "d2")
# Just create and pass back, even though they're not used
d3 = hcl.scalar(0, "d3")
d4 = hcl.scalar(0, "d4")
with hcl.if_(self.dMode == "max"):
with hcl.if_(spat_deriv[0] > 0):
d1[0] = self.dMax[0]
with hcl.elif_(spat_deriv[0] < 0):
d1[0] = self.dMin[0]
with hcl.if_(spat_deriv[1] > 0):
d2[0] = self.dMax[1]
with hcl.elif_(spat_deriv[1] < 0):
d2[0] = self.dMin[1]
with hcl.else_():
with hcl.if_(spat_deriv[0] > 0):
d1[0] = self.dMin[0]
with hcl.elif_(spat_deriv[0] < 0):
d1[0] = self.dMax[0]
with hcl.if_(spat_deriv[1] > 0):
d2[0] = self.dMin[1]
with hcl.elif_(spat_deriv[1] < 0):
d2[0] = self.dMax[1]
return (d1[0], d2[0], d3[0], d4[0])
def popcount(value):
#Calculate the number of one in a binary number
count = hcl.scalar(0, "count")
numb = hcl.scalar(value, "numb", dtype=hcl.UInt(in_bw))
with hcl.while_(numb.v > 0):
count.v += 1
numb.v &= numb.v - 1
return count.v
def step_bound(): # Function to calculate time step
stepBoundInv = hcl.scalar(0, "stepBoundInv")
stepBound = hcl.scalar(0, "stepBound")
stepBoundInv[0] = max_alpha1[0] / g.dx[0] + max_alpha2[0] / g.dx[1] + max_alpha3[0] / g.dx[2]
stepBound[0] = 0.8 / stepBoundInv[0]
with hcl.if_(stepBound > t[1] - t[0]):
stepBound[0] = t[1] - t[0]
t[0] = t[0] + stepBound[0]
return stepBound[0]
def dynamics(self, theta, opt_ctrl):
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = self.speed*hcl.cos(theta)
y_dot[0] = self.speed*hcl.sin(theta)
theta_dot[0] = opt_ctrl
return (x_dot[0], y_dot[0], theta_dot[0])
def dynamics(self, t, state, uOpt, dOpt):
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = self.speed * hcl.cos(state[2])
y_dot[0] = self.speed * hcl.sin(state[2])
theta_dot[0] = uOpt[0]
return (x_dot[0], y_dot[0], theta_dot[0])
def dynamics(self, t, state, uOpt, dOpt):
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = -self.speed + self.speed*hcl.cos(state[2]) + uOpt[0]*state[1]
y_dot[0] = self.speed*hcl.sin(state[2]) - uOpt[0]*state[0]
theta_dot[0] = dOpt[0] - uOpt[0]
return (x_dot[0], y_dot[0], theta_dot[0])
def step_bound(): # Function to calculate time step
stepBoundInv = hcl.scalar(0, "stepBoundInv")
stepBound = hcl.scalar(0, "stepBound")
stepBoundInv[0] = max_alpha1[0] / g.dx[0] + max_alpha2[0] / g.dx[
1] + max_alpha3[0] / g.dx[2] + max_alpha4[0] / g.dx[3]
stepBound[0] = 0.8 / stepBoundInv[0]
with hcl.if_(stepBound > t_step):
stepBound[0] = t_step
time = stepBound[0]
return time
def optDstb(self, t, state, spat_deriv):
"""
:param spat_deriv: tuple of spatial derivative in all dimensions
:return: a tuple of optimal disturbances
"""
# Graph takes in 4 possible inputs, by default, for now
d1 = hcl.scalar(0, "d1")
d2 = hcl.scalar(0, "d2")
# Just create and pass back, even though they're not used
d3 = hcl.scalar(0, "d3")
return (d1[0], d2[0], d3[0])
def opt_ctrl(self, t, state, spat_deriv):
opt_w = hcl.scalar(self.wMax, "opt_w")
# Just create and pass back, even though they're not used
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
with hcl.if_(spat_deriv[2] > 0):
with hcl.if_(self.uMode == "min"):
opt_w[0] = -opt_w
with hcl.elif_(spat_deriv[2] < 0):
with hcl.if_(self.uMode == "max"):
opt_w[0] = -opt_w
return (opt_w[0], in3[0], in4[0])
def dynamics(self, t, state, uOpt, dOpt):
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
v_dot = hcl.scalar(0, "v_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = state[2] * hcl.cos(state[3]) + dOpt[0]
y_dot[0] = state[2] * hcl.sin(state[3]) + dOpt[1]
v_dot[0] = uOpt[0]
theta_dot[0] = uOpt[1]
return (x_dot[0], y_dot[0], v_dot[0], theta_dot[0])
def test_hdc_accu(proto, pack_data, labels, type):
#pack the prototype
pack_proto = hcl.pack(proto,
axis=1,
dtype=hcl.UInt(bw),
name="pack_proto")
###data preparation
distance1 = hcl.compute((pack_data.shape[1], ),
lambda x: 0,
'distance1',
dtype=hcl.UInt(bw))
pre_hamming = hcl.compute((pack_data.shape[1], ), lambda x: 0,
"pre_hamming")
hamming_dist1 = hcl.compute((numClasses, ), lambda x: 0,
"hamming_dist1")
m1 = hcl.reduce_axis(0, pack_data.shape[1], "m1")
correct1 = hcl.scalar(0, 'correct1')
###
with hcl.for_(0, pack_data.shape[0]) as i:
hcl.print((i), "%d suc\n")
with hcl.for_(0, numClasses) as n:
#Do hdc multiplication(XOR) on sample[i]'s hdv and prototype[n]'s hdv (elementwise on the high-bit data)
hcl.update(distance1,
lambda x: pack_data[i][x] ^ pack_proto[n][x])
#Calculate the hamming distance of the two vectors by adding 1s
hcl.update(pre_hamming, lambda x: popcount(distance1[x]))
hcl.print((), "sum of 1s suc")
###########################seg fault
hamming_dist1[n] = hcl.sum(pre_hamming[m1], axis=m1)
#Find the one having the least hamming distance and choose it's label as the predicted label
pred1 = hcl.scalar(0, 'pred1')
with hcl.for_(0, hamming_dist1.shape[0]) as j:
with hcl.if_(hamming_dist1[j] < hamming_dist1[pred1]):
pred1.v = j
with hcl.if_(pred1.v == labels[i]):
correct1.v += 1
#Print the accuracy
all1 = hcl.scalar(pack_data.shape[0], "all1", dtype=hcl.Float(32))
accuracy1 = hcl.compute((1, ),
lambda x: correct1.v / all1.v * 100,
"accuracy1",
dtype=hcl.Float(32))
with hcl.if_(type == 1):
hcl.print((correct1, pack_data.shape[0], accuracy1[0]),
"Training accu: %d/%d (%.2f%%)\n")
with hcl.else_():
hcl.print((correct1, pack_data.shape[0], accuracy1[0]),
"Testing accu: %d/%d (%.2f%%)\n")
def optDstb(self, spat_deriv):
"""
:param spat_deriv: spatial derivative in all dimensions
:return: tuple of optimal disturbance
"""
dOpt1 = hcl.scalar(0, "dOpt1")
dOpt2 = hcl.scalar(0, "dOpt2")
dOpt3 = hcl.scalar(0, "dOpt3")
dOpt4 = hcl.scalar(0, "dOpt4")
#dOpt5 = hcl.scalar(0, "dOpt5")
#dOpt6 = hcl.scalar(0, "dOpt6")
return (dOpt1[0], dOpt2[0], dOpt3[0], dOpt4[0]) #, dOpt5[0], dOpt6[0])
def step_bound(): # Function to calculate time step
stepBoundInv = hcl.scalar(0, "stepBoundInv")
stepBound = hcl.scalar(0, "stepBound")
stepBoundInv[0] = max_alpha1[0]/g.dx[0] + max_alpha2[0]/g.dx[1] + max_alpha3[0]/g.dx[2] + max_alpha4[0]/g.dx[3] \
+ max_alpha5[0]/g.dx[4] + max_alpha6[0]/g.dx[5]
stepBound[0] = 0.8 / stepBoundInv[0]
with hcl.if_(stepBound > t[1] - t[0]):
stepBound[0] = t[1] - t[0]
# Update the lower time ranges
t[0] = t[0] + stepBound[0]
return stepBound[0]
def updateQopt(i, j, k, a, iVals, sVals, Qopt, actions, intermeds, trans,
interpV, gamma, bounds, goal, ptsEachDim, useNN, fillVal):
r = hcl.scalar(0, "r")
p = hcl.scalar(0, "p")
# set iVals equal to (i,j,k) and sVals equal to the corresponding state values at (i,j,k)
updateStateVals(i, j, k, iVals, sVals, bounds, ptsEachDim)
# call the transition function to obtain the outcome(s) of action a from state (si,sj,sk)
transition(sVals, actions[a], bounds, trans, goal)
# initialize Qopt[i,j,k,a] with the immediate reward
r[0] = reward(sVals, actions[a], bounds, goal, trans)
Qopt[i, j, k, a] = r[0]
# maximize over successor Q-values
with hcl.for_(0, trans.shape[0], name="si") as si:
p[0] = trans[si, 0]
sVals[0] = trans[si, 1]
sVals[1] = trans[si, 2]
sVals[2] = trans[si, 3]
# Nearest neighbour
with hcl.if_(useNN[0] == 1):
# obtain the nearest neighbour successor state
stateToIndex(sVals, iVals, bounds, ptsEachDim)
# maximize over successor state Q-values
with hcl.if_(
hcl.and_(iVals[0] < Qopt.shape[0],
iVals[1] < Qopt.shape[1],
iVals[2] < Qopt.shape[2])):
with hcl.if_(
hcl.and_(iVals[0] >= 0, iVals[1] >= 0, iVals[2] >= 0)):
with hcl.for_(0, actions.shape[0], name="a_") as a_:
with hcl.if_(
(r[0] +
(gamma[0] *
(p[0] * Qopt[iVals[0], iVals[1], iVals[2], a_]))
) > Qopt[i, j, k, a]):
Qopt[i, j, k, a] = r[0] + (gamma[0] * (
p[0] * Qopt[iVals[0], iVals[1], iVals[2], a_]))
# Linear interpolation
with hcl.if_(useNN[0] == 0):
with hcl.if_(
hcl.and_(sVals[0] <= bounds[0, 1],
sVals[1] <= bounds[1, 1],
sVals[2] <= bounds[2, 1])):
with hcl.if_(
hcl.and_(sVals[0] >= bounds[0, 0],
sVals[1] >= bounds[1, 0],
sVals[2] >= bounds[2, 0])):
stateToIndexInterpolants(Qopt, sVals, actions, bounds,
ptsEachDim, interpV, fillVal)
Qopt[i, j, k, a] += (gamma[0] * (p[0] * interpV[0]))
r[0] += Qopt[i, j, k, a]
def opt_ctrl(self, t, state, spat_deriv):
"""
:param t: time t
:param state: tuple of coordinates in 6 dimensions
:param spat_deriv: spatial derivative in all dimensions
:return: tuple of optimal control
"""
# Optimal control 1, 2, 3
uOpt1 = hcl.scalar(0, "uOpt1")
uOpt2 = hcl.scalar(0, "uOpt2")
uOpt3 = hcl.scalar(0, "uOpt3")
SumUU = hcl.scalar(0, "SumUU")
SumUL = hcl.scalar(0, "SumUL")
SumLU = hcl.scalar(0, "SumLU")
SumLL = hcl.scalar(0, "SumLL")
parSum = hcl.scalar(0, "parSum")
with hcl.if_(self.uMode == "min"):
# everything containing (u1, u2) in Hamiltonian, for all combinations of u1 and u2
SumUU[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMax[0])/state[2] + \
spat_deriv[3] * self.uMax[1]
SumUL[0] = spat_deriv[1]*(self.g + self.uMax[1]) * (state[0] + self.uMin[0])/state[2] + \
spat_deriv[3] * self.uMax[1]
SumLU[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMax[0])/state[2] + \
spat_deriv[3] * self.uMin[1]
SumLL[0] = spat_deriv[1]*(self.g + self.uMin[1]) * (state[0] + self.uMin[0])/state[2] + \
spat_deriv[3] * self.uMin[1]
# try every combination of u1 and u2 and take minimum
with hcl.if_(SumUU[0] > SumUL[0]):
uOpt1[0] = self.uMin[0]
uOpt2[0] = self.uMax[1]
SumUU[0] = SumUL[0]
with hcl.elif_(SumUU[0] < SumUL[0]):
uOpt1[0] = self.uMax[0]
uOpt2[0] = self.uMax[1]
with hcl.if_(SumUU[0] > SumLU[0]):
uOpt1[0] = self.uMax[0]
uOpt2[0] = self.uMin[1]
SumUU[0] = SumLU[0]
with hcl.if_(SumUU[0] > SumLL[0]):
uOpt1[0] = self.uMin[0]
uOpt2[0] = self.uMin[1]
# Find u3
# everything multiplied by u3 in Hamiltonian
parSum[0] = spat_deriv[1] + spat_deriv[5] * state[2] / self.J
with hcl.if_(parSum[0] > 0):
uOpt3[0] = self.uMin[2]
with hcl.elif_(parSum[0] < 0):
uOpt3[0] = self.uMax[2]
return (uOpt1[0], uOpt2[0], uOpt3[0])
def insertion_sort(A):
# Introduce a stage.
with hcl.Stage("S"):
# for i in range(1, A.shape[0])
# We can name the axis
with hcl.for_(1, A.shape[0], name="i") as i:
key = hcl.scalar(A[i], "key")
j = hcl.scalar(i - 1, "j")
# while(j >= 0 && key < A[j])
with hcl.while_(hcl.and_(j >= 0, key < A[j])):
A[j + 1] = A[j]
j.v -= 1
A[j + 1] = key.v
def dynamics(self, t, state, uOpt, dOpt):
L = hcl.scalar(3.0, "L") # Car length
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
v_dot = hcl.scalar(0, "v_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = state[2] * hcl.cos(state[3]) + dOpt[0]
y_dot[0] = state[2] * hcl.sin(state[3]) + dOpt[1]
v_dot[0] = uOpt[0]
theta_dot[0] = state[2] * (hcl.sin(uOpt[1]) / hcl.cos(uOpt[1])) / L[0]
return (x_dot[0], y_dot[0], v_dot[0], theta_dot[0])
def optDstb(self, spat_deriv):
dOpt1 = hcl.scalar(0, "dOpt1")
dOpt2 = hcl.scalar(0, "dOpt2")
dOpt3 = hcl.scalar(0, "dOpt3")
dOpt4 = hcl.scalar(0, "dOpt4")
with hcl.if_(self.dMode == "max"):
with hcl.if_(spat_deriv[0] > 0):
dOpt1[0] = self.dMax[0]
with hcl.elif_(spat_deriv[0] < 0):
dOpt1[0] = self.dMin[0]
with hcl.if_(spat_deriv[1] > 0):
dOpt2[0] = self.dMax[1]
with hcl.elif_(spat_deriv[1] < 0):
dOpt2[0] = self.dMin[1]
with hcl.if_(spat_deriv[2] > 0):
dOpt3[0] = self.dMax[2]
with hcl.elif_(spat_deriv[2] < 0):
dOpt3[0] = self.dMin[2]
with hcl.if_(spat_deriv[3] > 0):
dOpt4[0] = self.dMax[3]
with hcl.elif_(spat_deriv[3] < 0):
dOpt4[0] = self.dMin[3]
with hcl.elif_(self.dMode == "min"):
with hcl.if_(spat_deriv[0] > 0):
dOpt1[0] = self.dMin[0]
with hcl.elif_(spat_deriv[0] < 0):
dOpt1[0] = self.dMax[0]
with hcl.if_(spat_deriv[1] > 0):
dOpt2[0] = self.dMin[1]
with hcl.elif_(spat_deriv[1] < 0):
dOpt2[0] = self.dMax[1]
with hcl.if_(spat_deriv[2] > 0):
dOpt3[0] = self.dMin[2]
with hcl.elif_(spat_deriv[2] < 0):
dOpt3[0] = self.dMax[2]
with hcl.if_(spat_deriv[3] > 0):
dOpt4[0] = self.dMin[3]
with hcl.elif_(spat_deriv[3] < 0):
dOpt4[0] = self.dMax[3]
return (dOpt1[0], dOpt2[0], dOpt3[0], dOpt4[0])
def dynamics(self, t, state, uOpt, dOpt):
# wheelbase of Tamiya TT02
L = hcl.scalar(0.3, "L")
x_dot = hcl.scalar(0, "x_dot")
y_dot = hcl.scalar(0, "y_dot")
v_dot = hcl.scalar(0, "v_dot")
theta_dot = hcl.scalar(0, "theta_dot")
x_dot[0] = state[2] * hcl.cos(state[3]) + dOpt[0]
y_dot[0] = state[2] * hcl.sin(state[3]) + dOpt[1]
v_dot[0] = uOpt[0]
theta_dot[0] = state[2] * (hcl.sin(uOpt[1]) / hcl.cos(uOpt[1])) / L[0]
return (x_dot[0], y_dot[0], v_dot[0], theta_dot[0])
def dynamics(self, t, state, uOpt, dOpt):
x1_dot = hcl.scalar(0, "x_1_dot")
x2_dot = hcl.scalar(0, "x_2_dot")
x3_dot = hcl.scalar(0, "x_3_dot")
x4_dot = hcl.scalar(0, "x_4_dot")
x5_dot = hcl.scalar(0, "x_5_dot")
x1_dot[0] = -state[3] + state[4] * hcl.cos(
state[2]) + uOpt[0] * state[1]
x2_dot[0] = state[4] * hcl.sin(state[2]) - uOpt[0] * state[0]
x3_dot[0] = dOpt[0] - uOpt[0]
x4_dot[0] = uOpt[1]
x5_dot[0] = dOpt[1]
return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0], x5_dot[0])
def optDstb(self, spat_deriv):
"""
:param state:
:param spat_deriv:
:return:
"""
# Just create and pass back, even though they're not used
in1 = hcl.scalar(0, "in1")
in2 = hcl.scalar(0, "in2")
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
return (in1[0], in2[0], in3[0], in4[0])
