def workA(self):
count = 1
fisrt_time_hold = False
while not rospy.is_shutdown():
res = self.call_Handle(1) #start holding device
if not self.call_Handle(1).BallIsHolding: # BallIsHolding = 0
self.call_Handle(1)
self.chase(MaxSpd_A)
fisrt_time_hold = True
else: # BallIsHolding = 1
global RadHead
# [] give input to L
# [] recieve output from L
if fisrt_time_hold == True:
rel_turn_ang = 90
rel_turn_rad = math.radians(rel_turn_ang)
self.RadTurn = rel_turn_rad + self.RadHead
fisrt_time_hold = False
else:
fisrt_time_hold = False
error = math.fabs (turnHead2Kick(self.RadHead, self.RadTurn))
if error > angle_thres : # 還沒轉到
self.turn(self.RadTurn)
else : # 轉到
self.kick()
def workA(self):
count = 1
while not rospy.is_shutdown():
# while not self.ball_out():
# ball_out()
res = self.call_Handle(1) #start holding device
# print(turn(100))
# print(res)
if not self.call_Handle(1).BallIsHolding: # BallIsHolding = 0
self.call_Handle(1)
self.chase(MaxSpd_A)
rel_turn_ang = 45
rel_turn_rad = math.radians(rel_turn_ang)
self.RadTurn = rel_turn_rad + self.RadHead
else: # BallIsHolding = 1
global RadHead
# [] you will recieve a relative kick ang (-180~180) rel_turn_ang
# print(rel_turn_rad)
# print(self.RadTurn)
error = math.fabs(turnHead2Kick(self.RadHead, self.RadTurn))
if error > angle_thres: # 還沒轉到
self.turn(self.RadTurn)
else: # 轉到
self.kick()
def turn(self, angle):
global MaxSpd_A
self.vec.Vx = MaxSpd_A * math.cos(self.RadHead2Ball)
self.vec.Vy = MaxSpd_A * math.sin(self.RadHead2Ball)
# self.vec.Vx = 0
# self.vec.Vy = 0
self.vec.w = turnHead2Kick(self.RadHead, angle) * RotConst
self.vel_pub.publish(self.vec)
self.show("Turning")
def workA(self):
np.set_printoptions(suppress=True)
i = 0
fisrt_time_hold = False
while not rospy.is_shutdown():
# while self.step_count < 30000:
while 1:
# print(self.ball_in(), self.ball_out(), self.stealorfly())
rospy.wait_for_service('/nubot1/BallHandle')
self.call_Handle(1) # open holding device
if self.game_is_done() and self.real_resart:
# print('self.game_is_done()',self.game_is_done())
self.r = self.cnt_rwd()
# print('h',self.HowEnd)
s_ = self.sac_cal.input(self.HowEnd) #out state
if i > 1:
if len(self.s) == 7 and len(s_) == 7:
# print('000000000000000000',np.shape(self.s), np.shape(self.a))
self.agent.replay_buffer.store_transition(
self.s, self.a, self.r, s_, self.done)
# print('d',self.done)
self.ep_rwd = self.r
print('ep rwd value=', self.r)
self.end_rate(self.HowEnd)
self.step_count = self.step_count + 1
i += 1
self.s = s_
self.done = False
if i > 512: # 64
self.agent.learn(i, self.r, self.ep_rwd)
# self.ready2restart_pub.publish(True)
# self.ready2restart_pub.publish(False)
self.real_resart = False
self.HowEnd = 0
# print('i want to go in self.restart()')
self.restart()
# self.end_rate(self.HowEnd)
# print('---')
# elif not self.game_is_done():
else:
# print('self.game_is_done()',self.game_is_done())
rospy.wait_for_service('/nubot1/BallHandle')
if not self.call_Handle(
1).BallIsHolding: # BallIsHolding = 0
self.chase(MaxSpd_A)
fisrt_time_hold = True
rospy.wait_for_service('/nubot1/BallHandle')
# do real reset before holding
self.ready2restart = False
self.is_fly = False
self.is_steal = False
self.is_stealorfly = False
self.is_in = False
self.is_out = False
self.real_resart = True #
elif self.call_Handle(
1).BallIsHolding: # BallIsHolding = 1
global RadHead
self.chase(MaxSpd_A)
if fisrt_time_hold == True:
self.real_resart = True #
self.chase(MaxSpd_A)
self.show('Touch')
self.r = self.cnt_rwd()
s_ = self.sac_cal.input(0) #state_for_sac
if i >= 1:
if len(self.s) == 7 and len(s_) == 7:
self.agent.replay_buffer.store_transition(
self.s, self.a, self.r, s_, self.done)
print('step rwd value= ', self.r)
self.step_count = self.step_count + 1
self.done = False
i += 1
self.s = s_
self.a = self.agent.choose_action(
self.s) #action_from_sac
rel_turn_ang = self.sac_cal.output(
self.a) #action_from_sac
global pwr, MAX_PWR
pwr = (self.a[1] +
1) * MAX_PWR / 2 + 1.4 #normalize
# sac]
rel_turn_rad = math.radians(rel_turn_ang)
self.RadTurn = rel_turn_rad + self.RadHead
fisrt_time_hold = False
if i > 512:
self.agent.learn(i, self.r, self.ep_rwd)
elif fisrt_time_hold == False:
self.chase(MaxSpd_A)
error = math.fabs(
turnHead2Kick(self.RadHead, self.RadTurn))
if error > angle_thres: # 還沒轉到
self.turn(self.RadTurn)
else: # 轉到
self.kick()
def turn(self, angle):
self.vec.Vx = 0
self.vec.Vy = 0
self.vec.w = turnHead2Kick(self.RadHead, angle) * RotConst
self.vel_pub.publish(self.vec)
self.show("Turning")
def workA(self):
np.set_printoptions(suppress=True)
i = 0
while not rospy.is_shutdown():
rospy.wait_for_service('/nubot1/BallHandle')
self.Acall_Handle(1) # open holding device
rospy.wait_for_service('/nubot2/BallHandle')
self.Bcall_Handle(1) # open holding device
if self.game_is_done() and self.real_resart:
self.r = self.cnt_rwd()
s_ = self.sac_cal.input(self.HowEnd) #out state
if i > 1:
if len(self.s) == 10 and len(s_) == 10:
self.agent.replay_buffer.store_transition(self.s , self.a, self.r, s_, self.done)
self.ep_rwd = self.r
print('ep rwd value=',self.r)
self.end_rate(self.HowEnd)
self.step_count = self.step_count + 1
i += 1
self.s = s_
self.done = False
if i > 512: # 64
self.agent.learn(i, self.r, self.ep_rwd)
self.real_resart = False
self.HowEnd = 0
self.restart()
else:
rospy.wait_for_service('/nubot1/BallHandle')
rospy.wait_for_service('/nubot2/BallHandle')
self.Acall_Handle(1)
self.Bcall_Handle(1)
self.real_resart = True
self.r = self.cnt_rwd()
s_ = self.sac_cal.input(0) #state_for_sac
if i >= 1:
if len(self.s) == 10 and len(s_) == 10:
self.agent.replay_buffer.store_transition(self.s, self.a, self.r, s_, self.done)
print('step rwd value= ',self.r)
self.step_count = self.step_count + 1
self.done = False
i += 1
self.s = s_
self.a = self.agent.choose_action(self.s) #action_from_sac
self.a2Action
global pwr, MAX_PWR
pwr = (self.a[1]+1) * MAX_PWR/2 + 1.4 #normalize
# sac]
rel_turn_rad = math.radians(rel_turn_ang)
self.RadTurn = rel_turn_rad + self.RadHead
fisrt_time_hold = False
if i > 512:
self.agent.learn(i, self.r, self.ep_rwd)
elif fisrt_time_hold == False:
self.chase(MaxSpd_A)
error = math.fabs (turnHead2Kick(self.RadHead, self.RadTurn))
if error > angle_thres : # 還沒轉到
self.turn(self.RadTurn)
else : # 轉到
self.kick()