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)
