def create_goal(self, goal): ## Goal self.goal_transforms.append( 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_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_3dmodel(self, model): # 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 = 0.5 * np.array(color) + 0.2 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)) links.append(link_transf) ## ARROWS arrow = r3d.Color((0.2, 0.3, 0.9), r3d.arrow(0.12 * prop_r, 2.5 * prop_r, 16)) links.append(arrow) self.have_state = False return r3d.Transform(np.eye(4), links)
def _quadrotor_simple_3dmodel(self, diam): 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] self.have_state = False return r3d.Transform(np.eye(4), bodies)
def _make_scene(self, create_goals=True): # 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 = self._quadrotor_3dmodel(self.model) else: self.quad_transform = self._quadrotor_simple_3dmodel(self.diameter) 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 if create_goals: self.goal_arrows = [] for _ in range(self.goal_count): self.create_goal(goal=(0, 0, 0)) bodies = [ r3d.BackToFront([floor, self.shadow_transform]) ] + self.goal_transforms + [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 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