def reset(self): time.sleep(0.1) data_elimation() # Turn on when previous data needs to be cleared self.t = 0 self._t.clear() # float self.x1.clear() self.y1.clear() self.z1.clear() self.phi1.clear() self.u1.clear() self.v1.clear() self.w1.clear() self.r1.clear() # glider self.x2.clear() self.y2.clear() self.z2.clear() self.phit.clear() self.u2.clear() self.v2.clear() self.w2.clear() self.r2.clear() # forces self.T.clear() self.Ffoil_x.clear() self.Ffoil_z.clear() self.Rudder_angle.clear() self.Frudder_x.clear() self.Frudder_y.clear() self.Frudder_n.clear() # initial state self.state_0 = np.array( [ [0], [0], [0], [0], # eta1 [0], [0], [0], [0], # V1 [0], [0], [6.2], [0], # eta2 [0], [0], [0], [0] ], float) # V2 #self.rudder_angle = [0] return np.array([ self.state_0.item(8), self.state_0.item(9), self.state_0.item(11) ])
def reset(self): time.sleep(0.1) data_elimation() # Turn on when previous data needs to be cleared self.t = 0 self._t.clear() # float self.x1.clear() self.y1.clear() self.z1.clear() self.phi1.clear() self.u1.clear() self.v1.clear() self.w1.clear() self.r1.clear() # forces self.Thrust.clear() self.Rudder_angle.clear() self.Frudder_x.clear() self.Frudder_y.clear() self.Frudder_n.clear() # initial state self.state_0 = np.array([[0], [0], [0], [0], # eta1 [0], [0], [0], [0]], float) # V1 self.distance_target = math.hypot(self.state_0.item(0) - self.target_position[0],self.state_0.item(1) - self.target_position[1]) / 100 self.distance_obstacle_1 = (math.hypot(self.state_0.item(0) - self.obstacle_1[0],self.state_0.item(1) - self.obstacle_1[1])-self.obst_R1) / 100 self.distance_obstacle_2 = (math.hypot(self.state_0.item(0) - self.obstacle_2[0],self.state_0.item(1) - self.obstacle_2[1])-self.obst_R2) / 100 self.distance_obstacle_3 = (math.hypot(self.state_0.item(0) - self.obstacle_3[0],self.state_0.item(1) - self.obstacle_3[1])-self.obst_R3) / 100 self.distance_obstacle_4 = (math.hypot(self.state_0.item(0) - self.obstacle_4[0],self.state_0.item(1) - self.obstacle_4[1])-self.obst_R4) / 100 self.distance_obstacle_5 = (math.hypot(self.state_0.item(0) - self.obstacle_5[0],self.state_0.item(1) - self.obstacle_5[1])-self.obst_R5) / 100 self.course_error_target = self.state_0.item(3) - self.desired_course(self.target_position[0],self.target_position[1],self.state_0.item(0),self.state_0.item(1)) self.course_error_obstacle_1 = self.state_0.item(3) - self.desired_course(self.obstacle_1[0],self.obstacle_1[1],self.state_0.item(0),self.state_0.item(1)) self.course_error_obstacle_2 = self.state_0.item(3) - self.desired_course(self.obstacle_2[0],self.obstacle_2[1],self.state_0.item(0),self.state_0.item(1)) self.course_error_obstacle_3 = self.state_0.item(3) - self.desired_course(self.obstacle_3[0],self.obstacle_3[1],self.state_0.item(0),self.state_0.item(1)) self.course_error_obstacle_4 = self.state_0.item(3) - self.desired_course(self.obstacle_4[0],self.obstacle_4[1],self.state_0.item(0),self.state_0.item(1)) self.course_error_obstacle_5 = self.state_0.item(3) - self.desired_course(self.obstacle_5[0],self.obstacle_5[1],self.state_0.item(0),self.state_0.item(1)) self.distance_obstacle = list([self.distance_obstacle_1, self.distance_obstacle_2, self.distance_obstacle_3, self.distance_obstacle_4,self.distance_obstacle_5]) self.course_error_obstacle = list([self.course_error_obstacle_1, self.course_error_obstacle_2, self.course_error_obstacle_3,self.course_error_obstacle_4, self.course_error_obstacle_5]) # find the nearest obstacle i_obstacle = self.distance_obstacle.index(min(self.distance_obstacle)) return np.array([self.distance_target, self.course_error_target, self.distance_obstacle[i_obstacle], self.course_error_obstacle[i_obstacle], 0])
def reset(self): time.sleep(0.1) data_elimation() # Turn on when previous data needs to be cleared self.t = 0 self._t.clear() # float self.x1.clear() self.y1.clear() self.z1.clear() self.phi1.clear() self.u1.clear() self.v1.clear() self.w1.clear() self.r1.clear() # forces self.Thrust.clear() self.Rudder_angle.clear() self.Frudder_x.clear() self.Frudder_y.clear() self.Frudder_n.clear() # initial state self.state_0 = np.array( [ [0], [0], [0], [0], # eta1 [0], [0], [0], [0] ], float) # V1 #self.rudder_angle = [0] return np.array([ self.state_0.item(0), self.state_0.item(1), self.state_0.item(3), 0 ])
def simulation(): # initial state state_0 = np.array( [ [0], [0], [0], [0], # eta1 [0], [0], [0], [0], # V1 [0], [0], [6.2], [0], # eta2 [0], [0], [0], [0] ], float) # V2 # sea state H = 1 omega = 0.15 c_dir = 0 c_speed = 0 WG = WG_dynamics(H, omega, c_dir, c_speed) data_elimation() # Turn on when previous data needs to be cleared for t in simulation_time(terminal_time=500): rudder_angle = 0.5 # data storage if t % 1 == 0: _t.append(t) x1.append(state_0.item(0)) y1.append(state_0.item(1)) z1.append(state_0.item(2)) phi1.append(state_0.item(3)) u1.append(state_0.item(4)) v1.append(state_0.item(5)) w1.append(state_0.item(6)) r1.append(state_0.item(7)) x2.append(state_0.item(8)) y2.append(state_0.item(9)) z2.append(state_0.item(10)) phit.append(state_0.item(11)) u2.append(state_0.item(12)) v2.append(state_0.item(13)) w2.append(state_0.item(14)) r2.append(state_0.item(15)) Rudder_angle.append(rudder_angle) T0, Ffoil_0, Ffoil_2, Frudder_0, Frudder_1, Frudder_3 = WG.forces( state_0, rudder_angle, t) T.append(T0) Ffoil_x.append(Ffoil_0) Ffoil_z.append(Ffoil_2) Frudder_x.append(Frudder_0) Frudder_y.append(Frudder_1) Frudder_n.append(Frudder_3) # print(t) data_storage(x1, y1, phit, t, rudder_angle=Rudder_angle, u1=u1, T=T) # store data in local files # Runge-Kutta time_step = 0.001 k1 = WG.f(state_0, rudder_angle, t) k2 = WG.f(state_0 + 0.5 * k1 * time_step, rudder_angle, t + 0.5 * time_step) k3 = WG.f(state_0 + 0.5 * k2 * time_step, rudder_angle, t + 0.5 * time_step) k4 = WG.f(state_0 + k3 * time_step, rudder_angle, t + time_step) state_0 += (1 / 6) * (k1 + 2 * k2 + 2 * k3 + k4) * time_step ''' if state_0.item(11) > pi: state_0[11] = state_0.item(11) - 2*pi elif state_0.item(11) < -pi: state_0[11] = state_0.item(11) + 2*pi ''' # print(u1[-1], np.mean(u1)) # interval of refreshing the monitor, default: 2s if t % 2 == 0: data_viewer(x1, y1, u1, phit, Rudder_angle, _t, xlim_left=0, xlim_right=200, ylim_left=-100, ylim_right=100, goal_x=50, goal_y=50) # T=T, Ffoil_x=Ffoil_x return x1, y1, z1, phi1, \ x2, y2, z2, phit, \ u1, v1, w1, r1, \ u2, v2, w2, r2, \ _t, T, Ffoil_x, Ffoil_z, \ Frudder_x, Frudder_y, Frudder_n, Rudder_angle