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 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 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): """ :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): 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 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])