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 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 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 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 fft(X_real, X_imag, IndexTable, F_real, F_imag):
L = X_real.shape[0]
if np.log2(L) % 1 > 0:
raise ValueError("Length of input vector (1d tensor) must be power of 2")
num_stages = int(np.log2(L))
# bit reverse permutation
hcl.update(F_real, lambda i: X_real[IndexTable[i]], name='F_real_update')
hcl.update(F_imag, lambda i: X_imag[IndexTable[i]], name='F_imag_update')
with hcl.Stage("Out"):
one = hcl.scalar(1, dtype="int32")
with hcl.for_(0, num_stages) as stage:
DFTpts = one[0] << (stage + 1)
numBF = DFTpts / 2
e = -2 * np.pi / DFTpts
a = hcl.scalar(0)
with hcl.for_(0, numBF) as j:
c = hcl.scalar(hcl.cos(a[0]))
s = hcl.scalar(hcl.sin(a[0]))
a[0] = a[0] + e
with hcl.for_(j, L + DFTpts - 1, DFTpts) as i:
i_lower = i + numBF
temp_r = hcl.scalar(F_real[i_lower] * c - F_imag[i_lower] * s)
temp_i = hcl.scalar(F_imag[i_lower] * c + F_real[i_lower] * s)
F_real[i_lower] = F_real[i] - temp_r[0]
F_imag[i_lower] = F_imag[i] - temp_i[0]
F_real[i] = F_real[i] + temp_r[0]
F_imag[i] = F_imag[i] + temp_i[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 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")
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 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 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")
x1_dot[0] = state[3] * hcl.cos(state[2] + uOpt[0])
x2_dot[0] = state[3] * hcl.sin(state[2] + uOpt[0])
x3_dot[0] = (state[3] / self.l_r) * hcl.sin(uOpt[0])
x4_dot[0] = uOpt[1]
return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0])
def dynamics(self, t, state, uOpt,
dOpt): # Assume order of state is (x_a, z_a, u_r, w_r, x, z)
# Some constants for convenience
G1 = hcl.scalar(0, "G1")
F1 = hcl.scalar(0, "F1")
sigma = hcl.scalar(0, "sigma")
Phi_11 = hcl.scalar(0, "Phi_11")
Phi_12 = hcl.scalar(0, "Phi_12")
Phi_21 = hcl.scalar(0, "Phi_21")
Phi_22 = hcl.scalar(0, "Phi_22")
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")
x6_dot = hcl.scalar(0, "x6_dot")
# Get the values
G1[0] = self.A * self.omega * hcl.exp(-self.k * state[5])
F1[0] = self.k * G1[0]
sigma[0] = self.k * state[4] - self.omega * (-t[0])
Phi_11[0] = F1[0] * (-hcl.sin(sigma[0]))
Phi_12[0] = F1[0] * hcl.cos(sigma[0])
Phi_21[0] = Phi_12[0]
Phi_22[0] = -Phi_11[0]
# Question: What is B matrix
x1_dot[0] = state[2] + G1[0] * hcl.cos(sigma[0]) + dOpt[0] - uOpt[2]
x2_dot[0] = state[3] + G1[0] * (-hcl.sin(sigma[0])) + dOpt[1] - uOpt[3]
x3_dot[0] = (1/(self.m - self.X_udot))*(-Phi_11[0]*(self.b - self.X_udot)* state[2] + (self.b - self.m)*(G1[0] * self.omega * hcl.sin(sigma[0])) +\
Phi_11[0] * (state[2] + G1*hcl.cos(sigma[0])) + Phi_21[0]*(state[3] + G1[0]*(-hcl.sin(sigma[0]))) - (self.X_u + self.X_uu * my_abs(state[2]))*state[2] + \
self.B[0,0]*uOpt[0] + self.B[0,1]*uOpt[1]) + dOpt[2]
x4_dot[0] = (1/(self.m - self.Z_wdot))*(-Phi_22*(self.b - self.Z_wdot)*state[3] + \
(self.b - self.m) * (G1 * self.omega * hcl.cos(sigma[0])) + \
Phi_12[0] * (state[2] + G1 * hcl.cos(sigma[0])) + \
Phi_22[0] * (state[3] + G1 * (-hcl.sin(sigma[0]))) - \
(-(self.W - self.Buoy)) - (self.Z_w + self.Z_ww * my_abs(state[3])) * state[3] + \
self.B[1,0] * uOpt[0] + self.B[1,1] * uOpt[1]) + \
dOpt[3]
x5_dot[0] = state[2] + G1[0] * hcl.cos(sigma[0]) + dOpt[0]
x6_dot[0] = state[3] + G1[0] * (-hcl.sin(sigma[0])) + dOpt[1]
return (x1_dot[0], x2_dot[0], x3_dot[0], x4_dot[0], x5_dot[0],
x6_dot[0])
def HJ_PDE_solver(V_new, V_init, thetas):
# Calculate spatial derivative based on index and dimension number
def spatial_derivative(i, j, k, dim):
left = i * j - k
right = i * j + k
return left, right
# Calculate Hamiltonian for every grid point in V_init
with hcl.Stage("Hamiltonian"):
with hcl.for_(1, V_init.shape[0], name="i") as i:
with hcl.for_(1, V_init.shape[1], name="j") as j:
with hcl.for_(1, V_init.shape[2], name="k") as k:
# Calculate dV_dx
dV_dx_L, dV_dx_R = spatial_derivative(i, j, k, 0)
dV_dy_L, dV_dy_R = spatial_derivative(i, j, k, 1)
dV_dtheta_L, dV_dtheta_R = spatial_derivative(i, j, k, 2)
# Calculate average gradient
dV_dx_C = (dV_dx_L + dV_dx_R) / 2
dV_dy_C = (dV_dy_L + dV_dy_R) / 2
dV_dtheta_C = (dV_dtheta_L + dV_dtheta_R) / 2
# Get optimal control
uOpt = 1
# Velocity
v = 1
# Assume that mode is min
with hcl.if_(dV_dtheta_C > 0):
uOpt = -uOpt
# Calculate dynamics function
#V_new[i,j,k] = 1 * cos(thetas[k]) * dV_dx_C +1 * sin(thetas[k]) * dV_dy_C +uOpt * dV_theta_C
#angle = hcl.scalar(thetas[k], "angle")
V_new[i, j,
k] = v * hcl.cos(thetas[k]) * dV_dx_C + v * hcl.sin(
thetas[k]) * dV_dy_C + dV_dtheta_C * uOpt
def single_fft_hls(X_real, X_imag, F_real=None, F_imag=None, name=None):
if name is None: name = "hls::fft<config>"
L = X_real.shape[0]
assert X_real.shape == X_imag.shape
assert np.log2(L) % 1 == 0, "length must be power of 2: " + str(L)
return_tensors = False
if (F_real is None) and (F_imag is None):
return_tensors = True
F_real = hcl.compute((L, ), lambda i: 0, name='F_real')
F_imag = hcl.compute((L, ), lambda i: 0, name='F_imag')
# functional behavior
with hcl.Stage("ExternModule") as Module:
num_stages = int(np.log2(L))
bit_width = int(np.log2(L))
IndexTable = np.zeros((L), dtype='int')
for i in range(L):
b = '{:0{width}b}'.format(i, width=bit_width)
IndexTable[i] = int(b[::-1], 2)
Table = hcl.copy(IndexTable, "table", dtype=hcl.Int())
hcl.update(F_real, lambda i: X_real[Table[i]], name='F_real_update')
hcl.update(F_imag, lambda i: X_imag[Table[i]], name='F_imag_update')
with hcl.Stage("Out"):
one = hcl.scalar(1, dtype="int32", name="one")
with hcl.for_(0, num_stages) as stage:
DFTpts = one[0] << (stage + 1)
numBF = DFTpts / 2
e = -2 * np.pi / DFTpts
a = hcl.scalar(0, "a")
with hcl.for_(0, numBF) as j:
c = hcl.scalar(hcl.cos(a[0]), name="cos")
s = hcl.scalar(hcl.sin(a[0]), name="sin")
a[0] = a[0] + e
with hcl.for_(j, L + DFTpts - 1, DFTpts) as i:
i_lower = i + numBF
temp_r = hcl.scalar(
F_real[i_lower] * c - F_imag[i_lower] * s,
"temp_r")
temp_i = hcl.scalar(
F_imag[i_lower] * c + F_real[i_lower] * s,
"temp_i")
F_real[i_lower] = F_real[i] - temp_r[0]
F_imag[i_lower] = F_imag[i] - temp_i[0]
F_real[i] = F_real[i] + temp_r[0]
F_imag[i] = F_imag[i] + temp_i[0]
dicts = {}
dicts["name"] = name
tensors = [X_real, X_imag, F_real, F_imag]
dicts["args"] = [(_.name, _.dtype) for _ in tensors]
# declare headers and typedef
dicts["header"] = """
#include \"hls_fft.h\"
#include <complex>
struct config : hls::ip_fft::params_t {
static const unsigned ordering_opt = hls::ip_fft::natural_order;
static const unsigned config_width = 16; // FFT_CONFIG_WIDTH
};
typedef ap_fixed<16,1> data_t;
typedef std::complex<data_t> fxpComplex;
"""
# extern ip function
dicts["func"] = """
hls::ip_fft::config_t<config> fft_config;
hls::ip_fft::status_t<config> fft_status;
#pragma HLS INTERFACE ap_fifo port=fft_config
fft_config.setDir(0);
fft_config.setSch(0x2AB);
std::complex<data_t> xn[{}];
std::complex<data_t> xk[{}];
#pragma HLS INTERFACE ap_fifo port=xn depth=16
#pragma HLS INTERFACE ap_fifo port=xk depth=16
for (int i = 0; i < {}; i++) {{
#pragma HLS pipeline rewind
xn[i] = fxpComplex({}[i], {}[i]);
}}
hls::fft<config>(xn, xk, &fft_status, &fft_config);
for (int i = 0; i < {}; i++) {{
#pragma HLS pipeline rewind
{}[i] = xk[i].real();
{}[i] = xk[i].imag();
}}
""".format(L, L, L, X_real.name, X_imag.name, L, F_real.name, F_imag.name)
create_extern_module(Module, dicts, ip_type="hls")
if return_tensors: return F_real, F_imag
def opt_ctrl(self, t, state, spat_deriv):
"""
For all the notation here, please refer to doc "reachability for relative dynamics"
:param state:
:param spat_deriv:
:return:
"""
# uOpt1: beta_r, uOpt2: a_r
uOpt1 = hcl.scalar(0, "uOpt1")
uOpt2 = hcl.scalar(0, "uOpt2")
# # Define some constant
c1 = hcl.scalar(0, "c1")
c2 = hcl.scalar(0, "c2")
# According to doc, c1, c2 are defined as follow
c1[0] = -spat_deriv[0] * state[3] * hcl.sin(
state[2]) + spat_deriv[1] * state[3] * hcl.cos(
state[2]) + spat_deriv[2] * (state[3] / self.l_r)
c2[0] = spat_deriv[0] * state[3] * hcl.cos(
state[2]) + spat_deriv[1] * state[3] * hcl.sin(state[2])
# Define some intermediate variables to store
tmp1 = hcl.scalar(0, "tmp1")
tmp2 = hcl.scalar(0, "tmp2")
# Value these decision variable
tmp1[0] = -my_atan(c2[0] / c1[0]) + math.pi / 2
tmp2[0] = -my_atan(c2[0] / c1[0]) - math.pi / 2
# Store umin and umax
# uOpt = (uOpt[0], uOpt[1]) = (beta_r, a_r)
umin1 = hcl.scalar(0, "umin1")
umin2 = hcl.scalar(0, "umin2")
umax1 = hcl.scalar(0, "umax1")
umax2 = hcl.scalar(0, "umax2")
umin1[0] = self.uMin[0]
umin2[0] = self.uMin[1]
umax1[0] = self.uMax[0]
umax2[0] = self.uMax[1]
# Just create and pass back, even though they're not used
in3 = hcl.scalar(0, "in3")
in4 = hcl.scalar(0, "in4")
with hcl.if_(self.uMode == "max"):
# For uOpt1: beta_r
# TODO: Some logic error fixed here
with hcl.if_(c1[0] > 0):
with hcl.if_(tmp1[0] >= umin1[0]):
with hcl.if_(tmp1[0] <= umax1[0]):
uOpt1[0] = tmp1[0]
with hcl.if_(tmp1[0] > umax1[0]):
uOpt1[0] = umax1[0]
with hcl.if_(tmp1[0] < umin1[0]):
uOpt1[0] = umin1[0]
with hcl.if_(c1[0] < 0):
with hcl.if_(tmp2[0] >= umin1[0]):
with hcl.if_(tmp2[0] <= umax1[0]):
uOpt1[0] = tmp2[0]
with hcl.if_(tmp2[0] > umax1[0]):
uOpt1[0] = umax1[0]
with hcl.if_(tmp2[0] < umin1[0]):
uOpt1[0] = umin1[0]
with hcl.if_(c1[0] == 0):
with hcl.if_(c2[0] >= 0):
with hcl.if_(0 >= umin1[0]):
with hcl.if_(0 <= umax1[0]):
uOpt1[0] = 0
with hcl.if_(0 < umin1[0]):
uOpt1[0] = my_min(my_abs(umin1[0]), my_abs(umax1[0]))
with hcl.if_(0 > umax1[0]):
uOpt1[0] = my_min(my_abs(umin1[0]), my_abs(umax1[0]))
with hcl.if_(c2[0] < 0):
with hcl.if_(my_abs(umin1[0]) >= my_abs(umax1[0])):
uOpt1[0] = my_abs(umin1[0])
with hcl.if_(my_abs(umin1[0]) < my_abs(umax1[0])):
uOpt1[0] = my_abs(umax1[0])
# For uOpt2: a_r
with hcl.if_(spat_deriv[3] > 0):
uOpt2[0] = umax2[0]
with hcl.if_(spat_deriv[3] <= 0):
uOpt2[0] = umin2[0]
return (uOpt1[0], uOpt2[0], in3[0], in4[0])
