def sample_push_contact(world, feature, parameter, under=False):
robot = world.robot
arm = feature['arm_name']
body = world.get_body(feature['block_name'])
push_yaw = feature['push_yaw']
center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
max_backoff = width + 0.1 # TODO: add gripper bounding box
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
collision_links = get_link_subtree(robot, gripper_link)
urdf_from_center = Pose(point=center)
reverse_z = Pose(euler=Euler(pitch=math.pi))
rotate_theta = Pose(euler=Euler(yaw=push_yaw))
#translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))
grasps = []
for i in range(1 + under):
flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
for x in np.arange(0, max_backoff, step=0.01):
translate_x = Pose(point=Point(x=-x))
grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
reverse_z, invert(urdf_from_center))
set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
grasps.append(grasp_pose)
break
return grasps
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE):
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
#half_extent = 1.0*FINGER_EXTENT[2] # Collides
half_extent = 1.05 * FINGER_EXTENT[2]
grasps = []
for link in get_link_subtree(world.kitchen, joint):
if 'handle' in get_link_name(world.kitchen, link):
# TODO: can adjust the position and orientation on the handle
for yaw in [0, np.pi]: # yaw=0 DOESN'T WORK WITH LULA
handle_grasp = (Point(z=-half_extent),
quat_from_euler(
Euler(roll=np.pi, pitch=np.pi / 2,
yaw=yaw)))
#if not pull:
# handle_pose = get_link_pose(world.kitchen, link)
# for distance in np.arange(0., 0.05, step=0.001):
# pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp)
# tool_pose = multiply(handle_pose, invert(pregrasp))
# set_tool_pose(world, tool_pose)
# # TODO: check collisions
# wait_for_user()
handle_pregrasp = multiply((pre_direction, unit_quat()),
handle_grasp)
grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp))
return grasps
def get_link_obstacles(world, link_name):
if link_name in world.movable:
return flatten_links(world.get_body(link_name))
elif has_link(world.kitchen, link_name):
link = link_from_name(world.kitchen, link_name)
return flatten_links(world.kitchen, get_link_subtree(world.kitchen, link)) # subtree?
assert link_name in SURFACE_FROM_NAME
return set()
def create_gripper(robot, arm):
# gripper = load_pybullet(os.path.join(get_data_path(), 'pr2_gripper.urdf'))
# gripper = load_pybullet(os.path.join(get_models_path(), 'pr2_description/pr2_l_gripper.urdf'), fixed_base=False)
links = get_link_subtree(robot, link_from_name(robot, GRIPPER_LINKS[arm]))
#print([get_link_name(robot, link) for link in links])
gripper = clone_body(robot, links=links, visual=True,
collision=True) # TODO: joint limits
return gripper
def __init__(self, robot, ee_link, tool_link, color=TRANSPARENT, **kwargs):
self.robot = robot
self.ee_link = ee_link
self.tool_link = tool_link
self.tool_from_ee = get_relative_pose(self.robot, self.ee_link,
self.tool_link)
tool_links = get_link_subtree(robot, self.ee_link)
self.body = clone_body(robot,
links=tool_links,
visual=False,
collision=True,
**kwargs)
set_static(self.body)
if color is not None:
for link in get_all_links(self.body):
set_color(self.body, color, link) # None = white
def get_base_aabb(self):
franka_links = get_link_subtree(self.robot, self.franka_link)
base_links = get_link_subtree(self.robot, self.base_link)
return aabb_union(
get_aabb(self.robot, link)
for link in set(base_links) - set(franka_links))
def door_links(self):
door_links = set()
for joint in self.kitchen_joints:
door_links.update(get_link_subtree(self.kitchen, joint))
return door_links
def get_descendant_obstacles(body, link=BASE_LINK):
# TODO: deprecate?
return {(body, frozenset([link]))
for link in get_link_subtree(body, link)}