def __init__(self, thread_id, name, counter):
threading.Thread.__init__(self)
self.threadID = thread_id
self.name = name
self.counter = counter
self.sender = ActionModule(sim=SIM, serial_port="COM8")
self.vx = 0.7
self.vw = 0.8
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):
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])
class MovingObstacles(threading.Thread):
def __init__(self, thread_id, name, counter):
threading.Thread.__init__(self)
self.threadID = thread_id
self.name = name
self.counter = counter
self.sender = ActionModule(sim=SIM, serial_port="COM8")
self.vx = 0.7
self.vw = 0.8
# # Reset the 4 robots to its start pos
# self.sender.reset(0, 1, 0, 90)
# self.sender.reset(1, 0, 1, 180)
# self.sender.reset(2, -1, 0, 270)
# self.sender.reset(3, 0, -1, 0)
def run(self):
while True:
self.sender.send_action_real(robot_id=1, vx=200*self.vx, vy=0, vw=120*self.vw)
time.sleep(0.05)
# print("send success")
self.sender.send_action_real(robot_id=2, vx=200*self.vx, vy=0, vw=120*self.vw)
time.sleep(0.05)
self.sender.send_action_real(robot_id=4, vx=200*self.vx, vy=0, vw=120*self.vw)
time.sleep(0.05)
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
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]
(vision.ball_info[0] - vision.robot_info[0]))
delt_theta = robot2ball_theta - vision.robot_info[2]
delt_theta = angle_normalize(delt_theta)
state.append(np.sin(delt_theta)) # 1
state.append(np.cos(delt_theta)) # 2
v_t = vision.robot_info[3] * np.cos(
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)
if LOG:
file = open('data.txt', 'w')
run_loop(vision, sender)
if LOG:
file.close()
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
Through test, this function costs approximately 2ms to be executed,
and has only small influence on the main function.
:return: None
"""
# start = time.clock() # This is for debug. We will see how much time will
# pass for our python module to read a set of data from GrSim.
vision.get_info()
global ob
ob = vision.other_robots[:, :-1] # Dessert the orientation of other robots
# Restart the timer
# print("Elapsed Time: "+str(time.clock()-start))
if __name__ == "__main__":
vision = VisionModule(VISION_PORT)
sender = ActionModule('127.0.0.1', 20011)
debug = DebugModule()
sender.reset(robot_num=0)
# Initialize the initial situation of our robot and other robots:
vision.get_info()
pos = vision.robot_info
goal = np.array([4.8, 0])
ob = vision.other_robots[:, :-1] # Dessert the orientation of other robots
min_u = np.zeros(3)
config = Config()
traj = np.array(pos)
path = None # For now, there is no global path
count = 0 # This will be a flag in the running loop
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 !')
except: