def create_goal(self, goal):
r3d = self.r3d
## Goal
self.goal_transform = r3d.transform_and_color(
np.eye(4), (0.85, 0.55, 0), r3d.sphere(self.goal_diameter / 2, 18))
goal_arr_len, goal_arr_r, goal_arr_sect = 1.5 * self.goal_diameter, 0.02 * self.goal_diameter, 10
self.goal_arrows = []
self.goal_arrows_rot = []
self.goal_arrows_rot.append(
np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
self.goal_arrows_rot.append(
np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]]))
self.goal_arrows_rot.append(np.eye(3))
self.goal_arrows.append(
r3d.transform_and_color(
np.array([[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0],
[0, 0, 0, 1]]), (1., 0., 0.),
r3d.arrow(goal_arr_r, goal_arr_len, goal_arr_sect)))
self.goal_arrows.append(
r3d.transform_and_color(
np.array([[1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0],
[0, 0, 0, 1]]), (0., 1., 0.),
r3d.arrow(goal_arr_r, goal_arr_len, goal_arr_sect)))
self.goal_arrows.append(
r3d.transform_and_color(
np.eye(4), (0., 0., 1.),
r3d.arrow(goal_arr_r, goal_arr_len, goal_arr_sect)))
def quadrotor_simple_3dmodel(diam):
import gym_art.quadrotor_multi.rendering3d as r3d
r = diam / 2
prop_r = 0.3 * diam
prop_h = prop_r / 15.0
# "X" propeller configuration, start fwd left, go clockwise
rr = r * np.sqrt(2) / 2
deltas = ((rr, rr, 0), (rr, -rr, 0), (-rr, -rr, 0), (-rr, rr, 0))
colors = ((1, 0, 0), (1, 0, 0), (0, 1, 0), (0, 1, 0))
def disc(translation, color):
color = 0.5 * np.array(list(color)) + 0.2
disc = r3d.transform_and_color(r3d.translate(translation), color,
r3d.cylinder(prop_r, prop_h, 32))
return disc
props = [disc(d, c) for d, c in zip(deltas, colors)]
arm_thicc = diam / 20.0
arm_color = (0.6, 0.6, 0.6)
arms = r3d.transform_and_color(
np.matmul(r3d.translate((0, 0, -arm_thicc)), r3d.rotz(np.pi / 4)),
arm_color, [
r3d.box(diam / 10, diam, arm_thicc),
r3d.box(diam, diam / 10, arm_thicc)
])
arrow = r3d.Color((0.2, 0.3, 0.9),
r3d.arrow(0.12 * prop_r, 2.5 * prop_r, 16))
bodies = props + [arms, arrow]
return r3d.Transform(np.eye(4), bodies)
def create_goals(self):
import gym_art.quadrotor_multi.rendering3d as r3d
goal_sphere = r3d.sphere(self.goal_diameter / 2, 18)
for i in range(len(self.models)):
color = quad_color[i % len(quad_color)]
goal_transform = r3d.transform_and_color(np.eye(4), color, goal_sphere)
self.goal_transforms.append(goal_transform)
def _make_scene(self):
r3d = self.r3d
# if target is None:
# self.window_target = r3d.WindowTarget(self.window_w, self.window_h, resizable=self.resizable)
# self.obs_target = r3d.FBOTarget(self.obs_hw[0], self.obs_hw[1])
# self.video_target = r3d.FBOTarget(self.window_h, self.window_h)
self.cam1p = r3d.Camera(fov=90.0)
self.cam3p = r3d.Camera(fov=45.0)
if self.model is not None:
self.quad_transform = quadrotor_3dmodel(self.model)
else:
self.quad_transform = quadrotor_simple_3dmodel(self.diameter)
self.have_state = False
self.shadow_transform = r3d.transform_and_color(
np.eye(4), (0, 0, 0, 0.4), r3d.circle(0.75 * self.diameter, 32))
# TODO make floor size or walls to indicate world_box
floor = r3d.ProceduralTexture(
r3d.random_textype(), (0.15, 0.25),
r3d.rect((1000, 1000), (0, 100), (0, 100)))
self.update_goal_diameter()
self.chase_cam.view_dist = self.diameter * 15
self.create_goal(goal=(0, 0, 0))
bodies = [
r3d.BackToFront([floor, self.shadow_transform]),
self.goal_transform, self.quad_transform
] + self.goal_arrows
if self.obstacles:
bodies += self.obstacles.bodies
world = r3d.World(bodies)
batch = r3d.Batch()
world.build(batch)
self.scene = r3d.Scene(batches=[batch], bgcolor=(0, 0, 0))
self.scene.initialize()
def create_obstacles(self):
import gym_art.quadrotor_multi.rendering3d as r3d
for item in self.multi_obstacles.obstacles:
color = quad_color[14]
if item.shape == 'cube':
obstacle_transform = r3d.transform_and_color(np.eye(4), color, r3d.box(item.size, item.size, item.size))
elif item.shape == 'sphere':
num_facets = 18
facet_split_value = 10
facet_range_1, facet_range_2 = (0, num_facets-facet_split_value),\
(num_facets-facet_split_value, num_facets-1)
obstacle_transform = r3d.transform_and_dual_color(np.eye(4), (0, 0, 1), (1, 1, 0),
r3d.sphere(item.size / 2, 18, facet_range_1),
r3d.sphere(item.size / 2, 18, facet_range_2))
else:
raise NotImplementedError()
self.obstacle_transforms.append(obstacle_transform)
def _make_scene(self):
import gym_art.quadrotor_multi.rendering3d as r3d
self.cam1p = r3d.Camera(fov=90.0)
self.cam3p = r3d.Camera(fov=45.0)
self.quad_transforms, self.shadow_transforms, self.goal_transforms, self.collision_transforms = [], [], [], []
self.obstacle_transforms, self.vec_cyl_transforms, self.vec_cone_transforms = [], [], []
self.path_transforms = [[] for _ in range(self.num_agents)]
shadow_circle = r3d.circle(0.75 * self.diameter, 32)
collision_sphere = r3d.sphere(0.75 * self.diameter, 32)
arrow_cylinder = r3d.cylinder(0.005, 0.12, 16)
arrow_cone = r3d.cone(0.01, 0.04, 16)
path_sphere = r3d.sphere(0.15 * self.diameter, 16)
for i, model in enumerate(self.models):
if model is not None:
quad_transform = quadrotor_3dmodel(model, quad_id=i)
else:
quad_transform = quadrotor_simple_3dmodel(self.diameter)
self.quad_transforms.append(quad_transform)
self.shadow_transforms.append(
r3d.transform_and_color(np.eye(4), (0, 0, 0, 0.4), shadow_circle)
)
self.collision_transforms.append(
r3d.transform_and_color(np.eye(4), (0, 0, 0, 0.0), collision_sphere)
)
if self.vis_acc_arrows:
self.vec_cyl_transforms.append(
r3d.transform_and_color(np.eye(4), (1, 1, 1), arrow_cylinder)
)
self.vec_cone_transforms.append(
r3d.transform_and_color(np.eye(4), (1, 1, 1), arrow_cone)
)
if self.viz_traces:
color = quad_color[i % len(quad_color)] + (1.0,)
for j in range(self.viz_traces):
self.path_transforms[i].append(r3d.transform_and_color(np.eye(4), color, path_sphere))
# TODO make floor size or walls to indicate world_box
floor = r3d.ProceduralTexture(r3d.random_textype(), (0.15, 0.25),
r3d.rect((100, 100), (0, 100), (0, 100)))
self.update_goal_diameter()
self.chase_cam.view_dist = self.diameter * 15
self.create_goals()
bodies = [r3d.BackToFront([floor, st]) for st in self.shadow_transforms]
bodies.extend(self.goal_transforms)
bodies.extend(self.quad_transforms)
bodies.extend(self.vec_cyl_transforms)
bodies.extend(self.vec_cone_transforms)
for path in self.path_transforms:
bodies.extend(path)
# visualize walls of the room if True
if self.walls_visible:
room = r3d.ProceduralTexture(r3d.random_textype(), (0.15, 0.25), r3d.envBox(*self.room_dims))
bodies.append(room)
if self.obstacle_mode != 'no_obstacles':
self.create_obstacles()
bodies.extend(self.obstacle_transforms)
world = r3d.World(bodies)
batch = r3d.Batch()
world.build(batch)
self.scene = r3d.Scene(batches=[batch], bgcolor=(0, 0, 0))
self.scene.initialize()
# Collision spheres have to be added in the ending after everything has been rendered, as it transparent
bodies = []
bodies.extend(self.collision_transforms)
world = r3d.World(bodies)
batch = r3d.Batch()
world.build(batch)
self.scene.batches.extend([batch])
def quadrotor_3dmodel(model, quad_id=0):
import gym_art.quadrotor_multi.rendering3d as r3d
# params["body"] = {"l": 0.03, "w": 0.03, "h": 0.004, "m": 0.005}
# params["payload"] = {"l": 0.035, "w": 0.02, "h": 0.008, "m": 0.01}
# params["arms"] = {"l": 0.022, "w":0.005, "h":0.005, "m":0.001}
# params["motors"] = {"h":0.02, "r":0.0035, "m":0.0015}
# params["propellers"] = {"h":0.002, "r":0.022, "m":0.00075}
# params["motor_pos"] = {"xyz": [0.065/2, 0.065/2, 0.]}
# params["arms_pos"] = {"angle": 45., "z": 0.}
# params["payload_pos"] = {"xy": [0., 0.], "z_sign": 1}
## PROPELLERS
# "X" propeller configuration, start fwd left, go clockwise
# IDs: https://wiki.bitcraze.io/projects:crazyflie2:userguide:assembly
link_colors = {
"body": (0.67843137, 1., 0.18431373),
"payload": (0., 0., 1.),
"prop_0": (1, 0, 0),
"prop_1": (0, 1, 0),
"prop_2": (0, 1, 0),
"prop_3": (1, 0, 0),
"motor_0": (0, 0, 0),
"motor_1": (0, 0, 0),
"motor_2": (0, 0, 0),
"motor_3": (0, 0, 0),
"arm_0": (0, 0, 1),
"arm_1": (0, 0, 1),
"arm_2": (0, 0, 1),
"arm_3": (0, 0, 1),
}
links = []
for i, link in enumerate(model.links):
xyz, R, color = model.poses[i].xyz, model.poses[i].R, link_colors[
link.name]
rot = np.eye(4)
rot[:3, :3] = R
# print("LINK: ", link.name, "R:", rot, end=" ")
if link.name[:4] == "prop":
prop_r = link.r
color = np.array(quad_color[quad_id % len(quad_color)])
if link.type == "box":
# print("Type: Box")
link_transf = r3d.transform_and_color(
np.matmul(r3d.translate(xyz), rot), color,
r3d.box(link.l, link.w, link.h))
elif link.type == "cylinder":
# print("Type: Cylinder")
link_transf = r3d.transform_and_color(
r3d.translate(xyz), color, r3d.cylinder(link.r, link.h, 32))
elif link.type == "rod":
# print("Type: Rod")
R_y = np.eye(4)
R_y[:3, :3] = rpy2R(0, np.pi / 2, 0)
xyz[0] = -link.l / 2
link_transf = r3d.transform_and_color(
np.matmul(rot, np.matmul(r3d.translate(xyz), R_y)), color,
r3d.rod(link.r, link.l, 32))
links.append(link_transf)
## ARROWS
arrow = r3d.Color((0.2, 0.3, 0.9),
r3d.arrow(0.05 * prop_r, 1.5 * prop_r, 16))
links.append(arrow)
return r3d.Transform(np.eye(4), links)
def disc(translation, color):
color = 0.5 * np.array(list(color)) + 0.2
disc = r3d.transform_and_color(r3d.translate(translation), color,
r3d.cylinder(prop_r, prop_h, 32))
return disc