コード例 #1
0
def test_rendering_fps(benchmark):
    sensor = tacto.Sensor(width=120, height=160, visualize_gui=True)

    px.init_pybullet(mode=p.DIRECT)

    digit = px.Body(
        "../meshes/digit.urdf", base_orientation=[0.0, -0.707106, 0.0, 0.707106],
    )

    obj = px.Body(
        "../examples/objects/sphere_small.urdf",
        base_position=[-0.015, 0, 0.035],
        global_scaling=0.15,
    )

    sensor.add_camera(digit.id, [-1])
    sensor.add_body(obj)

    # step the simulation till the ball falls on the digit camera
    for i in range(10):
        p.stepSimulation()

    # Use pytest-benchmark to benchmark the rendering performance
    # run 1000 times, the reported number will be millisecond instead of seconds
    benchmark(render, sensor)

    # visualize the resulting image to make sure it's not empty
    colors, depths = sensor.render()
    return colors[0]
コード例 #2
0
def main(cfg):
    # Initialize digits
    bg = cv2.imread("conf/bg_digit_240_320.jpg")
    digits = tacto.Sensor(**cfg.tacto, background=bg)

    # Initialize World
    log.info("Initializing world")
    px.init()

    p.resetDebugVisualizerCamera(**cfg.pybullet_camera)

    # Create and initialize DIGIT
    digit_body = px.Body(**cfg.digit)
    digits.add_camera(digit_body.id, [-1])

    # Add object to pybullet and tacto simulator
    obj = px.Body(**cfg.object)
    digits.add_body(obj)

    # Create control panel to control the 6DoF pose of the object
    panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel)
    panel.start()
    log.info(
        "Use the slides to move the object until in contact with the DIGIT")

    # run p.stepSimulation in another thread
    t = px.utils.SimulationThread(real_time_factor=1.0)
    t.start()

    while True:
        color, depth = digits.render()
        digits.updateGUI(color, depth)

    t.stop()
コード例 #3
0
ファイル: demo_pybullet_grasp.py プロジェクト: poweic/tacto
def main(cfg):
    # Initialize digits
    digits = tacto.Sensor(**cfg.tacto)

    # Initialize World
    log.info("Initializing world")
    px.init()

    p.resetDebugVisualizerCamera(**cfg.pybullet_camera)

    robot = SawyerGripper(**cfg.sawyer_gripper)

    # [21, 24]
    digits.add_camera(robot.id, robot.digit_links)

    # Add object to pybullet and digit simulator
    obj = px.Body(**cfg.object)
    digits.add_body(obj)

    np.set_printoptions(suppress=True)

    # run p.stepSimulation in another thread
    t = px.utils.SimulationThread(real_time_factor=1.0)
    t.start()

    robot.reset()

    panel = px.gui.RobotControlPanel(robot)
    panel.start()

    while True:
        color, depth = digits.render()
        digits.updateGUI(color, depth)
        time.sleep(0.1)
コード例 #4
0
def test_contact_points(helpers):
    with px.Client(mode=p.DIRECT) as c:
        body = px.Body("teddy_vhacd.urdf")
        body.set_base_pose([0.0, 0.0, -0.02])
        c.stepSimulation()

        contact_points = c.getContactPoints(body.id)

        """
        check_getitem_method will check the following:
        assert contact_point[0] == contact_point.contact_flag
        assert contact_point[1] == contact_point.body_unique_id_a
        assert contact_point[2] == contact_point.body_unique_id_b
        assert contact_point[3] == contact_point.link_index_a
        assert contact_point[4] == contact_point.link_index_b
        assert contact_point[5] == contact_point.position_on_a
        assert contact_point[6] == contact_point.position_on_b
        assert contact_point[7] == contact_point.contact_normal_on_b
        assert contact_point[8] == contact_point.contact_distance
        assert contact_point[9] == contact_point.normal_force
        assert contact_point[10] == contact_point.lateral_friction_1
        assert contact_point[11] == contact_point.lateral_friction_dir_1
        assert contact_point[12] == contact_point.lateral_friction_2
        assert contact_point[13] == contact_point.lateral_friction_dir_2
        """
        for contact_point in contact_points:
            helpers.check_getitem_method(contact_point)
コード例 #5
0
    def load_objects(self):
        # Initialize digit and robot
        self.digits = tacto.Sensor(**self.cfg.tacto)
        if "env" not in self.cfg.sawyer_gripper.robot_params.urdf_path:
            robot_urdf_path = "env/" + self.cfg.sawyer_gripper.robot_params.urdf_path
            self.cfg.sawyer_gripper.robot_params.urdf_path = pkg_path(robot_urdf_path)
        self.robot = SawyerGripper(**self.cfg.sawyer_gripper, physics_client=self.physics_client)
        self.digits.add_camera(self.robot.id, self.robot.digit_links)

        # Load objects
        self.table = px.Body(**self.cfg.objects.table, physics_client=self.physics_client)
        self.load_board_and_peg()
コード例 #6
0
    def load_board_and_peg(self):
        board_orientation = p.getQuaternionFromEuler((np.pi/2, 0, 0))
        if self.difficulty == "easy":
            board_path = pkg_path("env/data/peg_in_hole_board/board_easy.urdf")   
        elif self.difficulty == "hard":
            board_path = pkg_path("env/data/peg_in_hole_board/board_hard.urdf")   
        else:
            raise Exception("Please enter a valid difficulty")

        board_cfg = {"urdf_path": board_path, 
                    "base_position": [np.random.uniform(0.50, 0.55), np.random.uniform(-0.1, 0.1), 0],
                    "base_orientation": board_orientation,
                    "use_fixed_base": True}
        self.board = px.Body(**board_cfg, physics_client=self.physics_client)

        peg_position = self.get_end_effector_position()
        peg_position[2] -= 0.025
        peg_urdf_path = pkg_path("env/" + self.cfg.objects.cylinder.urdf_path)
        peg_cfg = {"urdf_path": peg_urdf_path, "base_position": peg_position,
                    "base_orientation": p.getQuaternionFromEuler((0, 0, 0))}
        self.peg = px.Body(**peg_cfg, physics_client=self.physics_client)
        self.digits.add_body(self.peg)
コード例 #7
0
def main(cfg):
    # Initialize OmniTact
    omnitact = tacto.Sensor(
        **cfg.tacto, **{"config_path": tacto.get_omnitact_config_path()})

    # Initialize World
    log.info("Initializing world")
    px.init()

    p.resetDebugVisualizerCamera(**cfg.pybullet_camera)
    p.configureDebugVisualizer(
        p.COV_ENABLE_SHADOWS,
        1,
        lightPosition=[50, 0, 80],
    )

    # Create and initialize OmniTact
    body = px.Body(**cfg.omnitact)
    omnitact.add_camera(body.id, [-1])

    obj = px.Body(**cfg.object)
    omnitact.add_body(obj)

    # Create control panel to control the 6DoF pose of the object
    panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel)
    panel.start()
    log.info(
        "Use the slides to move the object until in contact with the OmniTact")

    # run p.stepSimulation in another thread
    t = px.utils.SimulationThread(real_time_factor=1.0)
    t.start()

    while True:
        color, depth = omnitact.render()
        omnitact.updateGUI(color, depth)

    t.stop()
コード例 #8
0
    def __init__(self, config_path=_get_default_config_path()):
        px.init(mode=p.GUI)
        self.cfg = OmegaConf.load(config_path)
        self.robot = SawyerGripper(**self.cfg.sawyer_gripper)
        self.obj = px.Body(**self.cfg.object)
        self.digits = tacto.Sensor(**self.cfg.tacto)
        self.camera = Camera()
        self.viewer = None

        self.digits.add_camera(self.robot.id, self.robot.digit_links)

        self.digits.add_body(self.obj)

        self.reset()
コード例 #9
0
def main(cfg):
    # Initialize digits
    # FIXME(poweic): this has to be done before p.connect if PYOPENGL_PLATFORM set to "egl"
    digits = tacto.Sensor(**cfg.tacto)

    # Initialize World
    log.info("Initializing world")
    px.init()

    p.resetDebugVisualizerCamera(**cfg.pybullet_camera)

    # Add allegro hand
    allegro = px.Body(**cfg.allegro)

    # Add cameras to tacto simulator
    digits.add_camera(allegro.id, cfg.digit_link_id_allegro)

    # Add object to pybullet and tacto simulator
    obj = px.Body(**cfg.object)
    digits.add_body(obj)

    # Create control panel to control the 6DoF pose of the object
    panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel)
    panel.start()

    log.info(
        "Use the slides to move the object until in contact with the DIGIT")

    # run p.stepSimulation in another thread
    t = px.utils.SimulationThread(real_time_factor=1.0)
    t.start()

    while True:
        color, depth = digits.render()
        digits.updateGUI(color, depth)

    t.stop()
コード例 #10
0
    def load_board_and_peg(self):
        board_orientation = p.getQuaternionFromEuler((0, 0, -np.pi / 2))
        board_cfg = {
            "urdf_path":
            add_cwd("env/" + self.cfg.objects.board.urdf_path),
            "base_position": [
                np.random.uniform(0.60, 0.70),
                np.random.uniform(-0.2, 0.2), 0.075
            ],
            "base_orientation":
            board_orientation,
            "use_fixed_base":
            True
        }
        self.board = px.Body(**board_cfg, physics_client=self.physics_client)

        peg_position = self.get_end_effector_position()
        peg_position[2] -= 0.025
        self.target = np.random.randint(low=0, high=3)
        peg_urdf_path = ""
        if self.target == 0:
            peg_urdf_path = add_cwd("env/" +
                                    self.cfg.objects.cylinder.urdf_path)
        elif self.target == 1:
            peg_urdf_path = add_cwd("env/" +
                                    self.cfg.objects.hexagonal_prism.urdf_path)
        elif self.target == 2:
            peg_urdf_path = add_cwd("env/" +
                                    self.cfg.objects.square_prism.urdf_path)
        peg_cfg = {
            "urdf_path": peg_urdf_path,
            "base_position": peg_position,
            "base_orientation": board_orientation
        }
        self.peg = px.Body(**peg_cfg, physics_client=self.physics_client)
        self.digits.add_body(self.peg)
コード例 #11
0
def test_pybullet_body(helpers):
    with px.Client(mode=p.DIRECT):
        init_position = (0, 0, 1)
        init_orientation = (0, 0, 0, 1)
        body = px.Body("kuka_iiwa/model.urdf", init_position, init_orientation)

        assert body.num_joints == 7

        # Move the robot to a target position/orientation
        position, orientation = (0, 1, 4), (0.03427, 0.10602, 0.14357, 0.98334)

        body.set_base_pose(position, orientation)
        position_, orientation_ = body.get_base_pose()
        assert np.allclose(position_, position)
        assert np.allclose(orientation_, orientation)

        # Reset the robot back to init position/orientation
        body.reset()
        position_, orientation_ = body.get_base_pose()
        assert np.allclose(position_, init_position)
        assert np.allclose(orientation_, init_orientation)

        print(body)