Пример #1
0
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)
Пример #2
0
    def update_goal(self, goal):
        r3d = self.r3d

        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]))
Пример #3
0
    def update_state(self, dynamics, goal):
        r3d = self.r3d

        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)
Пример #4
0
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)
Пример #5
0
    def update_state(self, all_dynamics, goals, multi_obstacles, collisions):
        import gym_art.quadrotor_multi.rendering3d as r3d

        if self.scene:
            if self.viewpoint == 'global':
                goal = np.mean(goals, axis=0)
                self.chase_cam.step(center=goal)
            else:
                self.chase_cam.step(all_dynamics[self.camera_drone_index].pos, all_dynamics[self.camera_drone_index].vel)
                self.fpv_lookat = all_dynamics[self.camera_drone_index].look_at()
            # use this to get trails on the goals and visualize the paths they follow
            # bodies = []
            # bodies.extend(self.goal_transforms)
            # world = r3d.World(bodies)
            # batch = r3d.Batch()
            # world.build(batch)
            # self.scene.batches.extend([batch])
            self.store_path_count += 1
            self.update_goals(goals=goals)
            if self.obstacle_mode != 'no_obstacles':
                self.update_obstacles(multi_obstacles=multi_obstacles)

            for i, dyn in enumerate(all_dynamics):
                matrix = r3d.trans_and_rot(dyn.pos, dyn.rot)
                self.quad_transforms[i].set_transform_nocollide(matrix)

                translation = r3d.translate(dyn.pos)

                if self.viz_traces and self.store_path_count % self.viz_trace_nth_step == 0:
                    if len(self.path_store[i]) >= self.viz_traces:
                        self.path_store[i].pop(0)

                    self.path_store[i].append(translation)
                    color_rgba = quad_color[i % len(quad_color)] + (1.0,)
                    path_storage_length = len(self.path_store[i])
                    for k in range(path_storage_length):
                        scale = k/path_storage_length + 0.01
                        transformation = self.path_store[i][k] @ r3d.scale(scale)
                        self.path_transforms[i][k].set_transform_and_color(transformation, color_rgba)

                shadow_pos = 0 + dyn.pos
                shadow_pos[2] = 0.001  # avoid z-fighting
                matrix = r3d.translate(shadow_pos)
                self.shadow_transforms[i].set_transform_nocollide(matrix)

                if self.vis_acc_arrows:
                    if len(self.vector_array[i]) > 10:
                        self.vector_array[i].pop(0)

                    self.vector_array[i].append(dyn.acc)

                    # Get average of the vectors
                    avg_of_vecs = np.mean(self.vector_array[i], axis=0)

                    # Calculate direction
                    vector_dir = np.diag(np.sign(avg_of_vecs))

                    # Calculate magnitude and divide by 3 (for aesthetics)
                    vector_mag = np.linalg.norm(avg_of_vecs) / 3

                    s = np.diag([1.0, 1.0, vector_mag, 1.0])

                    cone_trans = np.eye(4)
                    cone_trans[:3, 3] = [0.0, 0.0, 0.12 * vector_mag]

                    cyl_mat = r3d.trans_and_rot(dyn.pos, vector_dir @ dyn.rot) @ s

                    cone_mat = r3d.trans_and_rot(dyn.pos, vector_dir @ dyn.rot) @ cone_trans

                    self.vec_cyl_transforms[i].set_transform_nocollide(cyl_mat)
                    self.vec_cone_transforms[i].set_transform_nocollide(cone_mat)

                matrix = r3d.translate(dyn.pos)
                if collisions['drone'][i] > 0.0 or collisions['obstacle'][i] > 0.0 or collisions['ground'][i] > 0.0:
                    # Multiplying by 1 converts bool into float
                    self.collision_transforms[i].set_transform_and_color(matrix, (
                        (collisions['drone'][i] > 0.0) * 1.0, (collisions['obstacle'][i] > 0.0) * 1.0,
                        (collisions['ground'][i] > 0.0) * 1.0, 0.4))
                else:
                    self.collision_transforms[i].set_transform_and_color(matrix, (0, 0, 0, 0.0))
Пример #6
0
    def update_goals(self, goals):
        import gym_art.quadrotor_multi.rendering3d as r3d

        for i, g in enumerate(goals):
            self.goal_transforms[i].set_transform(r3d.translate(g[0:3]))
Пример #7
0
    def update_obstacles(self, multi_obstacles):
        import gym_art.quadrotor_multi.rendering3d as r3d

        for i, g in enumerate(multi_obstacles.obstacles):
            self.obstacle_transforms[i].set_transform(r3d.translate(g.pos))
Пример #8
0
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)
Пример #9
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