class Env(gym.Env):
def __init__(self):
self.low_action = np.array([-1, -1, -1])
self.high_action = np.array([1, 1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(6, ),
dtype=np.float32)
self.am = ActionModule()
self.vm = VisionModule()
self.gm = GameModule()
self.am.reset()
time.sleep(0.1)
def step(self, action=[0, 0, 0]):
self.gm.sendCommand(action, self.vm.me_angle)
feedback = self.vm.get_feedback(action)
return feedback
def reset(self):
self.vm.reset()
self.am.reset()
time.sleep(0.1)
self.am.reset()
return self.vm.get_feedback([0, 0, 0])
def __init__(self):
self.low_action = np.array([-1, -1, -1])
self.high_action = np.array([1, 1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(6, ),
dtype=np.float32)
self.am = ActionModule()
self.vm = VisionModule()
self.gm = GameModule()
self.am.reset()
time.sleep(0.1)
class Env(gym.Env):
_feedback = None
target_pos_x = 0
target_pos_y = 0
def __init__(self):
self.low_action = np.array([-1, -1, -1])
self.high_action = np.array([1, 1, 1])
self.action_space = spaces.Box(low=self.low_action,
high=self.high_action,
dtype=np.float32)
self.observation_space = spaces.Box(low=-np.inf,
high=np.inf,
shape=(14, ),
dtype=np.float32)
self.am = ActionModule()
self.vm = VisionModule()
self.gm = GameModule()
self.dm = DebugModule()
self.am.reset()
time.sleep(0.1)
def step(self, action=[0, 0, 0]):
self.gm.sendCommand([action[0], 0, 0], self.vm.me_angle,
self._feedback)
self._feedback = self.vm.get_feedback(
[self.target_pos_x, self.target_pos_y], action)
return self._feedback
def reset(self):
self.target_pos_x = random2axis(random.random())
self.target_pos_y = random2axis(random.random())
target = [self.target_pos_x, self.target_pos_y]
self.vm.reset()
self.am.reset()
time.sleep(0.1)
self.am.reset()
self.dm.gui_debug_x(target)
self.dm.send()
time.sleep(0.1)
self._feedback = self.vm.get_feedback(target, [0, 0, 0])
return self._feedback[0]
def __init__(self,
destination,
start,
map=map_grid,
vision_module=VisionModule(VISION_PORT=23333)):
self.vision = vision_module
self.destination = destination
self.start = start
self.open_list = np.array([[], [], [], [], [], []])
self.close_list = np.array([[], [], [], [], [], []])
self.best_path_array = np.array([[], []])
self.map = map
def __init__(self):
ROBOT_ID = 6
train = True
targetLine = [[4.5, 0.5], [4.5, -0.5]]
self.MULTI_GROUP = '224.5.23.2'
self.VISION_PORT = 10094
self.ACTION_IP = '127.0.0.1' # local host grSim
self.ACTION_PORT = 20011 # grSim command listen port
self.ROBOT_ID = ROBOT_ID
self.STATUS_PORT = 30011
self.vision = VisionModule(self.MULTI_GROUP, self.VISION_PORT,
self.STATUS_PORT)
self.actionSender = ActionModule(self.ACTION_IP, self.ACTION_PORT)
self.train = train
self.lowest = np.array([-3])
self.uppest = np.array([3])
self.height = 3.0
self.width = 4.5
self.action_space = spaces.Box(low=self.lowest,
high=self.uppest,
dtype=np.float32)
# self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32)
self.targetLine = targetLine
best = self.open_list[:, 0]
# print('检验第%s次当前点坐标*******************' % ite)
# print(best)
self.close_list = np.c_[self.close_list, best]
if best[0] == self.destination[0] and best[1] == self.destination[
1]: # 如果best是目标点，退出
print('搜索成功！')
return
self.child_point(best) # 生成子节点并判断数目
# print(self.open_list)
self.open_list = np.delete(self.open_list, 0, axis=1) # 删除open中最优点
ite = ite + 1
if __name__ == "__main__":
VISION_PORT = 23333
start = np.array([0, 0])
destinaton = np.array([49, 49])
vision = VisionModule(VISION_PORT)
finder = find_global_path(start=start,
destination=destinaton,
vision_module=vision)
finder.run()
finder.path_backtrace()
plot = MAP(map=finder.map, finder=finder)
plot.draw_three_axes(finder)
class GrsimEnv(gym.Env):
metadata = {
'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': 60
}
def __init__(self):
ROBOT_ID = 6
train = True
targetLine = [[4.5, 0.5], [4.5, -0.5]]
self.MULTI_GROUP = '224.5.23.2'
self.VISION_PORT = 10094
self.ACTION_IP = '127.0.0.1' # local host grSim
self.ACTION_PORT = 20011 # grSim command listen port
self.ROBOT_ID = ROBOT_ID
self.STATUS_PORT = 30011
self.vision = VisionModule(self.MULTI_GROUP, self.VISION_PORT,
self.STATUS_PORT)
self.actionSender = ActionModule(self.ACTION_IP, self.ACTION_PORT)
self.train = train
self.lowest = np.array([-3])
self.uppest = np.array([3])
self.height = 3.0
self.width = 4.5
self.action_space = spaces.Box(low=self.lowest,
high=self.uppest,
dtype=np.float32)
# self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(4,), dtype=np.float32)
self.targetLine = targetLine
""" action is [w. kick] """
def step(self, action=[0, 0]):
reward = -0.1
if self.vision.robot_info[3] != 1:
done = True
reward -= 0.5
if action[1] > 0:
kp = 5
else:
kp = 0
self.vision.robot_info[-1] = action[0]
self.actionSender.send_action(robot_num=self.ROBOT_ID,
vx=0,
vy=0,
w=action[0],
kp=kp)
self.vision.get_info(self.ROBOT_ID)
state = [self.vision.robot_info, self.vision.ball_info]
done = False
'''
kick ball task
'''
if action[1] > 0:
done = True
reward = self.get_reward()
return state, reward, done, info
def get_reward(self):
ball_pos = self.vision.ball_info
while True:
self.vision.get_info(self.ROBOT_ID)
if ball_pos[0] == self.vision.ball_info[0] and ball_pos[
1] == self.vision.ball_info[1]:
print("ball static!")
return -100
ball_pos = self.vision.ball_info
if ball_pos[1] >= self.height or ball_pos[
1] <= -self.height or ball_pos[0] < -self.width:
print("ball outside!")
return -100
if ball_pos[0] >= self.width and (
ball_pos[1] >= self.targetLine[1][1]
and ball_pos[1] <= self.targetLine[0][1]):
print("goal!!!")
return 100
def reset(self):
if self.train:
self.actionSender.send_reset(self.ROBOT_ID, 1)
else:
self.actionSender.send_reset(self.ROBOT_ID, 0)
time.sleep(0.5)
self.vision.get_info(self.ROBOT_ID)
state = [self.vision.robot_info, self.vision.ball_info[:2]]
if state[0][3] != 1:
self.reset()
return state
if __name__ == "__main__":
# 主控制器初始化
kp_main = 0.2
ki_main = 0.1
kd_main = 0
h_ref = 20
I_main_Max = 3
Output_main_Max = 90
controller = PidController(kp_main, ki_main, kd_main, h_ref, I_main_Max,
Output_main_Max)
# Initialize
fish = Fish(com='COM4')
time_config = TimeConfig()
vision = VisionModule()
d_h = vision.get_d_h()
alpha = controller.main_control(d_h)
d_alpha = vision.get_d_alpha()
cmd = controller.sub_control(d_alpha)
last_cmd = cmd
run_flag = True
# Start the threading:
thread_get_vision = GetVision()
thread_get_vision.start()
# thread_fish_demo = Demo()
# thread_fish_demo.start()
control_alpha_signal = []
robot2ball_theta) + vision.robot_info[4] * np.sin(robot2ball_theta)
v_n = -vision.robot_info[3] * np.sin(
robot2ball_theta) + vision.robot_info[4] * np.cos(robot2ball_theta)
state.append(v_t) # 3
state.append(v_n) # 4
state.append(np.sin(robot2ball_theta)) # 5
state.append(np.cos(robot2ball_theta)) # 6
action = target_policy(state)
if LOG:
file.write(str(state)[1:-1] + ', ' + str(action)[1:-1] + '\n')
sender.send_action(ROBOT_ID, vx=action[0], vy=action[1], w=action[2])
if __name__ == '__main__':
vision = VisionModule(VISION_PORT)
sender = ActionModule(ACTION_IP, ACTION_PORT)
obstacles = vision.get_obstacles(num_list=robot_list)
start = vision.get_info(ROBOT_ID=ROBOT_ME)
x = 0
y = 0
destination = np.array([x, y])
path_finder = find_global_path(vision_module=vision,
destination=destination,
start=start)
# if LOG:
# file = open('data.txt', 'w')
#
# run_loop(vision, sender)
#
vel = 0
return [vel, -theta]
def run_loop(vision, sender):
action = [0, 0]
while True:
for _ in range(8):
vision.get_info(ROBOT_ID)
#robot2ball_dist = np.sqrt((vision.robot_info[0]-vision.ball_info[0])**2 + (vision.robot_info[1]-vision.ball_info[1])**2)
robot2ball_theta = np.arctan2(
(vision.ball_info[1] - vision.robot_info[1]),
(vision.ball_info[0] - vision.robot_info[0]))
delt_theta = robot2ball_theta - vision.robot_info[2]
delt_theta = angle_normalize(delt_theta)
action = target_policy(delt_theta)
action[0] = np.clip(action[0], -3, 3)
action[1] = np.clip(action[1], -2, 2)
#print(robot2ball_theta/np.pi*180, vision.robot_info[2]/np.pi*180, delt_theta/np.pi*180)
print(action)
sender.send_action(ROBOT_ID, vx=action[0], vy=0, w=action[1])
if __name__ == '__main__':
vision = VisionModule(MULTI_GROUP, VISION_PORT)
sender = ActionModule(ACTION_IP, ACTION_PORT)
run_loop(vision, sender)
return [
robot2ball_dist,
np.sin(delt_theta),
np.cos(delt_theta), v_t, v_n,
np.sin(robot2ball_theta),
np.cos(robot2ball_theta)
]
if __name__ == "__main__":
MULTI_GROUP = '224.5.23.2'
VISION_PORT = 10094
ACTION_IP = '127.0.0.1' # local host grSim
ACTION_PORT = 20011 # grSim command listen port
ROBOT_ID = 6
vision = VisionModule(MULTI_GROUP, VISION_PORT)
actionSender = ActionModule(ACTION_IP, ACTION_PORT)
# actionSender.reset(robot_num=ROBOT_ID)
state_dim = 7
action_dim = 3
max_action = np.array([1, 1, 1])
min_action = np.array([-1, -1, -1])
ACTION_BOUND = [min_action, max_action]
max_v = 3.0
# Initialize policy
policy = TD3(state_dim, action_dim, max_action)
file_name = "TD3_%s_%s" % ("RoboCup-v1", str(0))
try:
policy.load(filename=file_name, directory="./pytorch_models")
print('Load model successfully !')