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]
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()
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)
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)
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()
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)
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()
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()
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()
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)
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)