def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs):
use_width = world.robot_name == FRANKA_CARTER
body = world.get_body(name)
#fraction = 0.25
obj_type = type_from_name(name)
body_pose = REFERENCE_POSE.get(obj_type, unit_pose())
center, extent = approximate_as_prism(body, body_pose)
for grasp_type in grasp_types:
if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)):
continue
#assert is_valid_grasp_type(name, grasp_type)
if grasp_type == TOP_GRASP:
grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, max_width=np.inf, **kwargs)
elif grasp_type == SIDE_GRASP:
# Take max of height and something
grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little
x, z = pre_distance * get_unit_vector([3, -1])
pre_direction = [0, 0, x]
post_direction = [0, 0, z]
top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0]
# Under grasps are actually easier for this robot
# TODO: bug in under in that it grasps at the bottom
generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs)
# if world.robot_name == FRANKA_CARTER else unit_pose()
generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp)
for grasp in generator for yaw in [0, np.pi])
else:
raise ValueError(grasp_type)
grasp_poses = randomize(list(generator))
if obj_type in CYLINDERS:
# TODO: filter first
grasp_poses = (multiply(grasp_pose, Pose(euler=Euler(
yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses))
for i, grasp_pose in enumerate(grasp_poses):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose)
with BodySaver(body):
grasp.get_attachment().assign()
with BodySaver(world.robot):
grasp.grasp_width = close_until_collision(
world.robot, world.gripper_joints, bodies=[body])
#print(get_joint_positions(world.robot, world.arm_joints)[-1])
#draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link)
#grasp.get_attachment().assign()
#wait_for_user()
##for value in get_joint_limits(world.robot, world.arm_joints[-1]):
#for value in [-1.8973, 0, +1.8973]:
# set_joint_position(world.robot, world.arm_joints[-1], value)
# grasp.get_attachment().assign()
# wait_for_user()
if use_width and (grasp.grasp_width is None):
continue
yield grasp
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 compute_grasps(world,
name,
grasp_length=(HAND_LENGTH - 0.02),
pre_distance=0.1,
max_attempts=INF):
body = world.get_body(name)
reference_pose = get_reference_pose(name)
# TODO: add z offset in the world frame
pre_direction = pre_distance * TOP_DIRECTION
ty = get_type(name)
if is_obj_type(name, 'block'):
generator = get_top_grasps(body,
under=False,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
max_width=np.inf)
elif is_obj_type(name, 'cup'):
#pre_direction = pre_distance*get_unit_vector([1, 0, -2]) # -x, -y, -z
pre_direction = pre_distance * get_unit_vector([3, 0, -1
]) # -x, -y, -z
# Cannot pick if top_offset is too large(0.03 <=)
top_offset = 3 * FINGER_WIDTH / 4 if ty in CUP_WITH_LIP else FINGER_WIDTH / 4
generator = get_side_cylinder_grasps(body,
under=False,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
top_offset=top_offset,
max_width=np.inf)
elif is_obj_type(name, 'stirrer'):
generator = get_top_cylinder_grasps(body,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=grasp_length,
max_width=np.inf)
elif is_obj_type(name, 'bowl'):
generator = get_edge_cylinder_grasps(body,
tool_pose=TOOL_POSE,
body_pose=reference_pose,
grasp_length=0.02)
elif is_obj_type(name, 'spoon'):
generator = get_spoon_grasps(name, body)
else:
raise NotImplementedError(name)
effort = 25 if ty in (OLIVE_CUPS + THREE_D_CUPS) else 50
for index, grasp_pose in enumerate(
islice(generator, None if max_attempts is INF else max_attempts)):
with BodySaver(world.robot):
grasp_width = compute_grasp_width(world.robot, 'left', body,
grasp_pose)
#print(index, grasp_width)
if grasp_width is not None:
yield Grasp(name, index, grasp_pose, pre_direction, grasp_width,
effort)
def get_grasp_presses(world, knob, pre_distance=APPROACH_DISTANCE):
knob_link = link_from_name(world.kitchen, knob)
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
for i, grasp_pose in enumerate(
get_top_presses(world.kitchen,
link=knob_link,
tool_pose=TOOL_POSE,
top_offset=FINGER_EXTENT[0] / 2 + 5e-3)):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, knob, TOP_GRASP, i, grasp_pose, pregrasp_pose)
yield grasp
def partial_forces(reaction_forces, elements):
loads, fixities, reactions = reaction_forces
reaction_from_node = {}
for node, reaction in fixities.items():
if node in fixities:
reaction_from_node[node] = [reaction]
reactions = {element: reactions[element] for element in elements}
reaction_from_node = add_reactions(reactions, reaction_from_node)
for node, reactions in reaction_from_node.items():
load_force = loads[node][:3]
total_force = np.sum(reactions, axis=0)[:3]
unit_load = get_unit_vector(load_force)
load_magnitude = np.dot(unit_load, load_force)
total_magnitude = np.dot(unit_load, total_force)
print(node, load_magnitude, total_magnitude)
def solve_collision_free(door,
obstacle,
max_iterations=100,
step_size=math.radians(5),
min_distance=2e-2,
draw=True):
joints = get_movable_joints(door)
door_link = child_link_from_joint(joints[-1])
# print(get_com_pose(door, door_link))
# print(get_link_inertial_pose(door, door_link))
# print(get_link_pose(door, door_link))
# draw_pose(get_com_pose(door, door_link))
handles = []
success = False
start_time = time.time()
for iteration in range(max_iterations):
current_conf = np.array(get_joint_positions(door, joints))
collision_infos = get_closest_points(door,
obstacle,
link1=door_link,
max_distance=min_distance)
if not collision_infos:
success = True
break
collision_infos = sorted(collision_infos,
key=lambda info: info.contactDistance)
collision_infos = collision_infos[:1] # TODO: average all these
if draw:
for collision_info in collision_infos:
handles.extend(draw_collision_info(collision_info))
wait_if_gui()
[collision_info] = collision_infos[:1]
distance = collision_info.contactDistance
print(
'Iteration: {} | Collisions: {} | Distance: {:.3f} | Time: {:.3f}'.
format(iteration, len(collision_infos), distance,
elapsed_time(start_time)))
if distance >= min_distance:
success = True
break
# TODO: convergence or decay in step size
direction = step_size * get_unit_vector(
collision_info.contactNormalOnB) # B->A (already normalized)
contact_point = collision_info.positionOnA
#com_pose = get_com_pose(door, door_link) # TODO: be careful here
com_pose = get_link_pose(door, door_link)
local_point = tform_point(invert(com_pose), contact_point)
#local_point = unit_point()
translate, rotate = compute_jacobian(door,
door_link,
point=local_point)
delta_conf = np.array([
np.dot(translate[mj], direction) # + np.dot(rotate[mj], direction)
for mj in movable_from_joints(door, joints)
])
new_conf = current_conf + delta_conf
if violates_limits(door, joints, new_conf):
break
set_joint_positions(door, joints, new_conf)
if draw:
wait_if_gui()
remove_handles(handles)
print('Success: {} | Iteration: {} | Time: {:.3f}'.format(
success, iteration, elapsed_time(start_time)))
#quit()
return success
def gen_fn(arm, obj_name, pose1, region):
# TODO: reachability test here
if region is None:
goals = push_goal_gen_fn(obj_name, pose1, surface)
elif isinstance(region, str):
goals = push_goal_gen_fn(obj_name, pose1, surface, region=region)
else:
goals = [(region,)]
if repeat:
goals = cycle(goals)
arm_joints = get_arm_joints(world.robot, arm)
open_width = get_max_limit(world.robot, get_gripper_joints(world.robot, arm)[0])
body = world.bodies[obj_name]
for goal_pos2d, in islice(goals, max_samples):
pose2 = get_end_pose(pose1, goal_pos2d)
body_path = list(interpolate_poses(pose1, pose2))
if cartesian_path_collision(body, body_path, set(obstacles) - {surface_body}) or \
cartesian_path_unsupported(body, body_path, surface_body):
continue
set_pose(body, pose1)
push_direction = np.array(point_from_pose(pose2)) - np.array(point_from_pose(pose1))
backoff_tform = Pose(-backoff_distance * get_unit_vector(push_direction)) # World coordinates
feature = get_push_feature(world, arm, obj_name, pose1, goal_pos2d)
for parameter in parameter_fn(world, feature):
push_contact = next(iter(sample_push_contact(world, feature, parameter, under=False)))
gripper_path = [multiply(pose, invert(multiply(TOOL_POSE, push_contact))) for pose in body_path]
set_gripper_position(world.robot, arm, open_width)
for _ in range(max_attempts):
start_conf = solve_inverse_kinematics(world.robot, arm, gripper_path[0], obstacles=obstacles)
if start_conf is None:
continue
set_pose(body, pose1)
body_saver = BodySaver(body)
#attachment = create_attachment(world.robot, arm, body)
push_path = plan_waypoint_motion(world.robot, arm, gripper_path[-1],
obstacles=obstacles, #attachments=[attachment],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if push_path is None:
continue
pre_backoff_pose = multiply(backoff_tform, gripper_path[0])
pre_approach_pose = multiply(pre_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[0])
pre_path = plan_waypoints_motion(world.robot, arm, [pre_backoff_pose, pre_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if pre_path is None:
continue
pre_path = pre_path[::-1]
post_backoff_pose = multiply(backoff_tform, gripper_path[-1])
post_approach_pose = multiply(post_backoff_pose, approach_tform)
set_joint_positions(world.robot, arm_joints, push_path[-1])
post_path = plan_waypoints_motion(world.robot, arm, [post_backoff_pose, post_approach_pose],
obstacles=obstacles, attachments=[],
self_collisions=collisions, disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(pre_path[0])
set_joint_positions(world.robot, arm_joints, pre_conf)
robot_saver = BodySaver(world.robot)
post_conf = Conf(post_path[-1])
control = Control({
'action': 'push',
'objects': [obj_name],
'feature': feature,
'parameter': None,
'context': Context(
savers=[robot_saver, body_saver],
attachments={}),
'commands': [
ArmTrajectory(arm, pre_path),
Push(arm, obj_name),
ArmTrajectory(arm, push_path),
Detach(arm, obj_name),
ArmTrajectory(arm, post_path),
],
})
yield (pose2, pre_conf, post_conf, control)
break
import numpy as np
import math
from collections import namedtuple
from itertools import islice
from control_tools.common import get_arm_prefix
from plan_tools.common import get_reference_pose, is_obj_type, get_type, CUP_WITH_LIP
from plan_tools.samplers.generators import TOOL_FRAMES, TOOL_POSE, set_gripper_position
from pybullet_tools.pr2_utils import get_gripper_joints, get_top_grasps, get_side_cylinder_grasps, \
get_top_cylinder_grasps, get_edge_cylinder_grasps, close_until_collision
from pybullet_tools.utils import get_unit_vector, multiply, Pose, link_from_name, Attachment, get_link_pose, set_pose, \
approximate_as_prism, Euler, Point, BodySaver, INF
from dimensions.cups.dimensions import OLIVE_CUPS, THREE_D_CUPS
TOP_DIRECTION = get_unit_vector([1, 0, 0])
# TODO: unify these properties
# TODO: compute thickness using the bounding box
Spoon = namedtuple('Spoon', ['length', 'height', 'diameter', 'thickness'])
SPOON_THICKNESSES = {
'grey_spoon': 0.003,
'orange_spoon': 0.004,
'green_spoon': 0.005,
}
SPOON_DIAMETERS = {
'grey_spoon': 0.045,
'orange_spoon': 0.055,
'green_spoon': 0.065,
def get_press_gen_fn(world, max_attempts=25, collisions=True):
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacle_bodies = [world.bodies[surface]
for surface in world.get_fixed()] if collisions else []
pre_direction = 0.15 * get_unit_vector([-1, 0, 0])
collision_buffer = 0.0 # Because of contact with the table
def gen_fn(arm, button):
gripper_joint = get_gripper_joints(world.robot, arm)[0]
closed_width, open_width = get_joint_limits(world.robot, gripper_joint)
pose = world.initial_poses[button]
body = world.bodies[button]
presses = cycle(get_top_presses(body, tool_pose=TOOL_POSE))
for attempt in range(max_attempts):
try:
press = next(presses)
except StopIteration:
break
set_gripper_position(world.robot, arm, closed_width)
tool_pose = multiply(pose, invert(press))
grip_conf = solve_inverse_kinematics(
world.robot,
arm,
tool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer)
if grip_conf is None:
continue
pretool_pose = multiply(tool_pose, Pose(point=pre_direction))
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacle_bodies,
collision_buffer=collision_buffer,
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
post_conf = Conf(post_path[-1])
pre_conf = post_conf
pre_path = post_path[::-1]
control = Control({
'action':
'press',
'objects': [],
'context':
Context( # TODO: robot might be at the wrong conf
savers=[BodySaver(world.robot)
], # TODO: start with open instead
attachments={}),
'commands': [
GripperCommand(arm, closed_width),
ArmTrajectory(arm, pre_path),
ArmTrajectory(arm, post_path),
GripperCommand(arm, open_width),
],
})
yield (pre_conf, post_conf, control)
# TODO: continue exploration
return gen_fn