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 update_goal(self, goal): self.goal_transform.set_transform(r3d.translate(goal[0:3])) self.goal_arrows[0].set_transform( r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[0])) self.goal_arrows[1].set_transform( r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[1])) self.goal_arrows[2].set_transform( r3d.trans_and_rot(goal[0:3], self.goal_arrows_rot[2]))
def update_state(self, dynamics, goal): if self.scene: self.chase_cam.step(dynamics.pos, dynamics.vel) self.have_state = True self.fpv_lookat = dynamics.look_at() self.update_goal(goal=goal) matrix = r3d.trans_and_rot(dynamics.pos, dynamics.rot) self.quad_transform.set_transform_nocollide(matrix) shadow_pos = 0 + dynamics.pos shadow_pos[2] = 0.001 # avoid z-fighting matrix = r3d.translate(shadow_pos) self.shadow_transform.set_transform_nocollide(matrix)
def _random_obstacles(np_random, N, arena, our_radius): arena = float(arena) # all primitives should be tightly bound by unit circle in xy plane boxside = np.sqrt(2) box = r3d.box(boxside, boxside, boxside) sphere = r3d.sphere(radius=1.0, facets=16) cylinder = r3d.cylinder(radius=1.0, height=2.0, sections=32) # TODO cone-sphere collision #cone = r3d.cone(radius=0.5, height=1.0, sections=32) primitives = [box, sphere, cylinder] bodies = [] max_radius = 2.0 positions, radii, test_list = _place_obstacles(np_random, N, arena, (0.5, max_radius), our_radius) for i in range(N): primitive = np_random.choice(primitives) tex_type = r3d.random_textype() tex_dark = 0.5 * np_random.uniform() tex_light = 0.5 * np_random.uniform() + 0.5 color = 0.5 * np_random.uniform(size=3) heightscl = np.random.uniform(0.5, 2.0) height = heightscl * 2.0 * radii[i] z = (0 if primitive is cylinder else (height / 2.0 if primitive is sphere else (height * boxside / 4.0 if primitive is box else np.nan))) translation = np.append(positions[i, :], z) matrix = np.matmul(r3d.translate(translation), r3d.scale(radii[i])) matrix = np.matmul(matrix, np.diag([1, 1, heightscl, 1])) body = r3d.Transform( matrix, #r3d.ProceduralTexture(tex_type, (tex_dark, tex_light), primitive)) r3d.Color(color, primitive)) bodies.append(body) return ObstacleMap(arena, bodies, test_list)
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
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)) 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.12 * prop_r, 2.5 * prop_r, 16)) links.append(arrow) self.have_state = False return r3d.Transform(np.eye(4), links)