def __init__(self, robot_id):
self.robot_id = robot_id # for now...will figure out how to do this later
self.cmd_vel_topic = "/action#" + str(self.robot_id)
self.policy = DoubleQ(
doubleQPars,
name='Dummy',
task=Task(),
load_path=
'/home/jimmy/Documents/Research/AN_Bridging/results/policy_comparison_results/all_final/hierarchical_q_policy2.txt'
)
self.controller = HierarchicalController()
rospy.Subscriber("/robotState" + str(self.robot_id),
String,
self.receive_state_info,
queue_size=1)
rospy.Subscriber("/shutdown" + str(self.robot_id),
Int16,
self.shutdown,
queue_size=1)
rospy.Subscriber('/finished', Int8, self.receive_restart, queue_size=1)
self.pub = rospy.Publisher("/robotAction" + str(self.robot_id),
Vector3,
queue_size=1)
self.pub_finish = rospy.Publisher("/robot_finished",
Int16,
queue_size=1)
self.period = 50
self.counter = 0
self.shut_down = False
self.sleep_duration = 5
self.box_distance_finished_condition = .4 # These was not the parameter used in training
self.marker_distance_finished_condition = .4 # These were the parameters used in training
print(' Robot number:', robot_id, ' is ready!')
rospy.wait_for_message('/map_is_ready',
Int16) # just wait for the message indicator
self.publish_finished_to_map()
self.waiting_for_target = False
rospy.spin()
return
'mu': torch.Tensor([0, 0, 0, 0, 0, 0, 0]),
'std': torch.Tensor([1, 1, 1, 1, 1, 1, 1]),
'trainMode': False,
'load': False,
'dual': False,
}
valTrain = {
'batch': 256,
'lr': 3e-4,
'buffer': 2000, #500
'gamma': .99,
'explore': .9,
'double': True,
}
params = {"valPars": valPars, "valTrain": valTrain, "agents": agents}
tanker = DoubleQ(params, NAME, HierarchyTask())
#agent = SAC(params, NAME, HierarchyTask())
if description == 'DOUBLE_CONTROL':
NAME = 'a_bot'
agents = OrderedDict({
#ensure ordering matches ros message
"a_bot": {
"sub": "/state",
"pub": "/action1"
}, #joint action space
})
agents["b_bot"] = {"sub": "/state", "pub": "/action2"}
# agents["c_bot"] = {"sub": "/state", "pub": "/action3"}
# Placeholders
'w_phase1': 1,
'w_phase2': 1,
'w_phase3': 1,
'buffer': 10000,
'explore': False,
'gamma': .99
'explore': .4,
'baseExplore': .1,
'decay': .6,
'step': 50,
'double': True,
'prioritySample': True,
'a': 1
}
params = {"valPars": valPars, "valTrain": valTrain, "agents": agents}
tanker = DoubleQ(params, NAME, MoveTask("argmax"))
if description == "TWIN_DDPG":
agents = OrderedDict({
#ensure ordering matches ros messages
"bot": {"sub": "/state", "pub": "/action"} #joint action space
})
valPars = {
'neurons': (4, 256, 256, 1),
'act': ['F.relu','F.relu'],
'mu': torch.Tensor([0, 0, 0, 0]),
'std': torch.Tensor([2, 2, 3, 3]),
'trainMode': True,
'load': False,
'tau': .005
def __init__(self, network_parameters, name):
og_params = copy.deepcopy(network_parameters)
self.fail = rospy.Publisher("/restart", Int8, queue_size=1)
self.agents = network_parameters['agents']
self.net_params = network_parameters['valPars']
self.train_params = network_parameters['valTrain']
self.pubs = OrderedDict()
for key in self.agents.keys():
bot = self.agents[key]
self.pubs[key] = rospy.Publisher(bot['pub'], Vector3, queue_size=1)
self.name = name
rospy.Subscriber(self.agents[self.name]['sub'],
String,
self.receiveState,
queue_size=1)
self.changePoint = rospy.Publisher('/phaseChange', Int8, queue_size=1)
self.action_map = {
0: 'APPROACH',
1: 'ANGLE_TOWARDS',
2: 'PUSH_IN',
3: 'ALIGN_Y',
4: 'PUSH_LEFT',
5: 'PUSH_RIGHT',
6: 'MOVE_BACK',
7: 'ANGLE_TOWARDS_GOAL'
}
self.goalSphere = None # (point, radius)
self.prevPrim = None
self.reset_sequence = True
self.max_num_actions = 5
self.max_num_rollouts = 1
self.MPC = False
self.GP = False
self.mode = "Analysis" #'Plan' #
self.box_policy = DoubleQ(
network_parameters,
self.name,
HierarchyTask(),
load_path=
'/home/austinnguyen517/Documents/Research/BML/MultiRobot/AN_Bridging/model_training_data/Hybrid/Combination_Gaussian/hierarchical_q_policy2.txt'
)
# self.model = Network(self.vPars, self.vTrain)
self.controller = HierarchyTask()
self.analysis = Analysis(self.mode)
self.success, self.success_prime, self.fail, self.fail_prime, self.id_to_primitive, self.primitive_to_id = self.analysis.get_data(
)
success_start, fail_start = self.analysis.get_start_data()
if self.mode == 'Analysis':
self.analysis.analyze_requested_data()
sys.exit(0)
self.train_model()
while (True):
x = 1 + 1