def get_bounds_geom_objs(pos_bounds):
"""
Generate 6 faces corresponding to the agent deplacement bounds
"""
size = pos_bounds[1] - pos_bounds[0]
center = np.mean(pos_bounds, axis=0)
thickness = 0.05
color = (1, 1, 1, 0.3)
geom_objs = []
aas = [
eigenpy.AngleAxis(0, np.array([1, 0, 0])),
eigenpy.AngleAxis(np.pi / 2, np.array([0, 1, 0])),
eigenpy.AngleAxis(np.pi / 2, np.array([0, 0, 1])),
]
placement = pin.SE3.Identity()
for i, angle_axis in enumerate(aas):
placement.rotation = angle_axis.matrix()
size_bound = size.copy()
translation = np.zeros(3)
size_bound[i] = thickness
translation[i] = -size[i] / 2 - thickness / 2.1
for j in range(2):
geom = hppfcl.Box(*size_bound)
placement.translation = translation
mesh = Mesh(
name=f"bound{i}{j}",
geometry=geom,
placement=placement,
color=color,
)
geom_objs.append(mesh.geom_obj())
translation *= -1
return geom_objs
def add_robot(self, robot_name, bounds):
model_wrapper = self.model_wrapper
color = self.robot_color
if robot_name == "sphere":
radius = 0.03
geom = hppfcl.Sphere(radius)
sphere_mesh = Mesh(name="robot", geometry=geom, color=color)
robot = FreeFlyer(model_wrapper, sphere_mesh, bounds)
elif robot_name == "sphere2d":
radius = 0.01
geom = hppfcl.Sphere(radius)
sphere_mesh = Mesh(name="robot", geometry=geom, color=color)
robot = FreeFlyer(model_wrapper, sphere_mesh, bounds)
elif robot_name == "s_shape":
mesh_path = "../assets/s_shape_description/s_shape.stl"
scale = (0.2, 0.2, 0.2)
s_mesh = Mesh(name="robot",
geometry_path=mesh_path,
color=color,
scale=scale)
robot = FreeFlyer(model_wrapper, s_mesh, bounds)
else:
raise ValueError(f"Unknown robot: {robot_name}")
self.robot_n_joints = robot.n_joints
return robot
def extract_obstacles(condition, num_obstacles):
w_dim = 3
dw = 0.1
gap1 = condition[0:w_dim]
gap2 = condition[w_dim:2 * w_dim]
gap3 = condition[2 * w_dim:3 * w_dim]
num_obstacles = 5
obst1 = [0, gap1[1] - dw, -0.5, gap1[0], gap1[1], 1.5]
obst2 = [gap2[0] - dw, 0, -0.5, gap2[0], gap2[1], 1.5]
obst3 = [gap2[0] - dw, gap2[1] + dw, -0.5, gap2[0], 1, 1.5]
obst4 = [gap1[0] + dw, gap1[1] - dw, -0.5, gap3[0], gap1[1], 1.5]
obst5 = [gap3[0] + dw, gap1[1] - dw, -0.5, 1, gap1[1], 1.5]
obstacles = [obst1, obst2, obst3, obst4, obst5]
obstacles = obstacles[:num_obstacles]
for i, obst in enumerate(obstacles):
x0, y0, x1, y1 = obst[0], obst[1], obst[3], obst[4]
box_size = [x1 - x0, y1 - y0, 0.1]
pos = [(x0 + x1) / 2, (y0 + y1) / 2, 0]
placement = pin.SE3(np.eye(3), np.array(pos))
mesh = Mesh(
name=f"obstacle{i}",
geometry=hppfcl.Box(*box_size),
placement=placement,
color=(0, 0, 1, 0.8),
)
obstacles[i] = mesh.geom_obj()
return obstacles
def extract_obstacles(maze, thickness):
scx = 1 / maze.nx
scy = 1 / maze.ny
obstacles_coord = []
for x in range(maze.nx):
obstacles_coord.append((x / maze.nx, 0, (x + 1) / maze.nx, 0))
for y in range(maze.ny):
obstacles_coord.append((0, y / maze.ny, 0, (y + 1) / maze.ny))
# Draw the "South" and "East" walls of each cell, if present (these
# are the "North" and "West" walls of a neighbouring cell in
# general, of course).
for x in range(maze.nx):
for y in range(maze.ny):
if maze.cell_at(x, y).walls["S"]:
x1, y1, x2, y2 = (
x * scx,
(y + 1) * scy,
(x + 1) * scx,
(y + 1) * scy,
)
obstacles_coord.append((x1, y1, x2, y2))
if maze.cell_at(x, y).walls["E"]:
x1, y1, x2, y2 = (
(x + 1) * scx,
y * scy,
(x + 1) * scx,
(y + 1) * scy,
)
obstacles_coord.append((x1, y1, x2, y2))
obstacles = []
for i, obst_coord in enumerate(obstacles_coord):
x1, y1, x2, y2 = obst_coord[0], obst_coord[1], obst_coord[
2], obst_coord[3]
x1 -= thickness / 2
x2 += thickness / 2
y1 -= thickness / 2
y2 += thickness / 2
box_size = [x2 - x1, y2 - y1, 0.1]
pos = [(x1 + x2) / 2, (y1 + y2) / 2, 0]
placement = pin.SE3(np.eye(3), np.array(pos))
mesh = Mesh(
name=f"obstacle{i}",
geometry=hppfcl.Box(*box_size),
placement=placement,
color=(0, 0, 1, 0.8),
)
obstacles.append(mesh.geom_obj())
return obstacles
def generate_geom_objs(
np_random,
freeflyer_bounds,
center_bounds,
size_bounds,
cube_bounds,
n_obstacles,
obstacles_type,
dynamic_obstacles,
obstacles_color,
obstacles_alpha,
):
colors = np_random.uniform(0, 1, (n_obstacles, 4))
colors[:, 3] = obstacles_alpha
if obstacles_color is not None:
colors = [obstacles_color for _ in range(n_obstacles)]
name = "box{}"
geom_objs = []
placement_tuple = []
# obstacles
for i in range(n_obstacles):
rand_se3_init, obst_size = sample_box_parameters(
np_random, center_bounds, size_bounds)
rand_se3_target, obst_size = sample_box_parameters(
np_random, center_bounds, size_bounds)
placement_tuple.append((rand_se3_init, rand_se3_target))
# rand_se3.rotation = np.eye(3)
geom, path, scale = sample_geom(np_random, obstacles_type, obst_size,
i)
mesh = Mesh(
name=name.format(i),
geometry=geom,
placement=rand_se3_init,
color=colors[i],
geometry_path=path,
scale=scale,
)
geom_obj_obstacle = mesh.geom_obj()
geom_objs.append(geom_obj_obstacle)
if cube_bounds:
geom_objs_bounds = envs_utils.get_bounds_geom_objs(
freeflyer_bounds[:, :3])
geom_objs += geom_objs_bounds
return geom_objs, placement_tuple
def make_geom_obj(self, name, radius):
geom = hppfcl.Sphere(radius)
mesh = Mesh(name=name, geometry=geom)
return mesh.geom_obj()
def make_geom_obj(self, name):
geom = hppfcl.Sphere(self.radius)
mesh = Mesh(name=name, geometry=geom, color=self.color)
return mesh.geom_obj()