def setup_targets(self):
self.targets = [AgentDoubleInt2D(agent_id = 'target-' + str(i),
dim=self.target_dim, sampling_period=self.sampling_period,
limit=self.limit['target'],
collision_func=lambda x: map_utils.is_collision(self.MAP, x),
A=self.targetA, W=self.target_true_noise_sd)
for i in range(self.num_targets)]
def get_init_pose_random(
self,
lin_dist_range_target=(METADATA['init_distance_min'],
METADATA['init_distance_max']),
ang_dist_range_target=(-np.pi, np.pi),
lin_dist_range_belief=(METADATA['init_belief_distance_min'],
METADATA['init_belief_distance_max']),
ang_dist_range_belief=(-np.pi, np.pi),
blocked=False,
**kwargs):
is_agent_valid = False
init_pose = {}
init_pose['agents'] = []
for ii in range(self.num_agents):
is_agent_valid = False
if self.MAP.map is None and ii == 0:
if blocked:
raise ValueError(
'Unable to find a blocked initial condition. There is no obstacle in this map.'
)
a_init = self.agent_init_pos[:2]
else:
while (not is_agent_valid):
a_init = np.random.random(
(2, )) * (self.MAP.mapmax -
self.MAP.mapmin) + self.MAP.mapmin
is_agent_valid = not (map_utils.is_collision(
self.MAP, a_init))
init_pose_agent = [
a_init[0], a_init[1],
np.random.random() * 2 * np.pi - np.pi
]
init_pose['agents'].append(init_pose_agent)
init_pose['targets'], init_pose['belief_targets'] = [], []
for jj in range(self.num_targets):
is_target_valid = False
while (not is_target_valid):
rand_agent = np.random.randint(self.num_agents)
is_target_valid, init_pose_target = self.gen_rand_pose(
init_pose['agents'][rand_agent][:2],
init_pose['agents'][rand_agent][2],
lin_dist_range_target[0], lin_dist_range_target[1],
ang_dist_range_target[0], ang_dist_range_target[1])
is_blocked = map_utils.is_blocked(
self.MAP, init_pose['agents'][rand_agent][:2],
init_pose_target[:2])
if is_target_valid:
is_target_valid = (blocked == is_blocked)
init_pose['targets'].append(init_pose_target)
is_belief_valid, init_pose_belief = False, np.zeros((2, ))
while ((not is_belief_valid) and is_target_valid):
is_belief_valid, init_pose_belief = self.gen_rand_pose(
init_pose['targets'][jj][:2], init_pose['targets'][jj][2],
lin_dist_range_belief[0], lin_dist_range_belief[1],
ang_dist_range_belief[0], ang_dist_range_belief[1])
init_pose['belief_targets'].append(init_pose_belief)
return init_pose
def setup_agents(self):
self.agents = [
AgentSE2(
agent_id='agent-' + str(i),
dim=self.agent_dim,
sampling_period=self.sampling_period,
limit=self.limit['agent'],
collision_func=lambda x: map_utils.is_collision(self.MAP, x))
for i in range(self.num_agents)
]
def setup_belief_targets(self):
self.belief_targets = [
KFbelief(
agent_id='target-' + str(i),
dim=self.target_dim,
limit=self.limit['target'],
A=self.targetA,
W=self.target_noise_cov,
obs_noise_func=self.observation_noise,
collision_func=lambda x: map_utils.is_collision(self.MAP, x))
for i in range(self.num_targets)
]
def setup_targets(self):
self.targets = [
AgentDoubleInt2D_Avoidance(
agent_id='target-' + str(i),
dim=self.target_dim,
sampling_period=self.sampling_period,
limit=self.limit['target'],
collision_func=lambda x: map_utils.is_collision(self.MAP, x),
A=self.targetA,
W=self.target_true_noise_sd,
)
# obs_check_func=lambda x: self.MAP.get_closest_obstacle(
# x, fov=2*np.pi, r_max=10e2))
for i in range(self.num_targets)
]
def gen_rand_pose(self, o_xy, c_theta, min_lin_dist, max_lin_dist, min_ang_dist, max_ang_dist):
"""Generates random position and yaw.
Parameters
--------
o_xy : xy position of a point in the global frame which we compute a distance from.
c_theta : angular position of a point in the global frame which we compute an angular distance from.
min_lin_dist : the minimum linear distance from o_xy to a sample point.
max_lin_dist : the maximum linear distance from o_xy to a sample point.
min_ang_dist : the minimum angular distance (counter clockwise direction) from c_theta to a sample point.
max_ang_dist : the maximum angular distance (counter clockwise direction) from c_theta to a sample point.
"""
if max_ang_dist < min_ang_dist:
max_ang_dist += 2*np.pi
rand_ang = util.wrap_around(np.random.rand() * \
(max_ang_dist - min_ang_dist) + min_ang_dist + c_theta)
rand_r = np.random.rand() * (max_lin_dist - min_lin_dist) + min_lin_dist
rand_xy = np.array([rand_r*np.cos(rand_ang), rand_r*np.sin(rand_ang)]) + o_xy
is_valid = not(map_utils.is_collision(self.MAP, rand_xy))
return is_valid, [rand_xy[0], rand_xy[1], rand_ang]
