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 main():
# TODO: move to pybullet-planning for now
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(args)
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1)
floor = create_floor()
with HideOutput():
robot = load_pybullet(get_model_path(ROOMBA_URDF),
fixed_base=True,
scale=2.0)
for link in get_all_links(robot):
set_color(robot, link=link, color=ORANGE)
robot_z = stable_z(robot, floor)
set_point(robot, Point(z=robot_z))
#set_base_conf(robot, rover_confs[i])
data_path = add_data_path()
shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
fixed_base=True) # TODO: returns a tuple
dump_body(shelf)
#draw_aabb(get_aabb(shelf))
wait_for_user()
disconnect()
def create_world(items=[], **kwargs):
state = {name: unit_pose() for name in items}
with HideOutput():
world = ROSWorld(sim_only=True, state=state, **kwargs) # state=[]
table_body = add_table(world)
create_floor()
return world, table_body
def __init__(self, task, use_robot=True, visualize=False, **kwargs):
self.task = task
self.real_world = None
self.visualize = visualize
self.client_saver = None
self.client = connect(use_gui=visualize, **kwargs)
print('Planner connected to client {}.'.format(self.client))
self.robot = None
with ClientSaver(self.client):
with HideOutput():
if use_robot:
self.robot = load_pybullet(os.path.join(
get_models_path(), PR2_URDF),
fixed_base=True)
#dump_body(self.robot)
#compute_joint_weights(self.robot)
self.world_name = 'world'
self.world_pose = Pose(unit_pose())
self.bodies = {}
self.fixed = []
self.surfaces = []
self.items = []
#self.holding_liquid = []
self.initial_config = None
self.initial_poses = {}
self.body_mapping = {}
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def main(display='execute'): # control | execute | step
# One of the arm's gantry carriage is fixed when the other is moving.
connect(use_gui=True)
set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
draw_pose(unit_pose(), length=1.0)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
# floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
#link = link_from_name(robot, 'gantry_base_link')
#print(get_aabb(robot, link))
block_x = -0.2
#block_y = 1 if ARM == 'right' else 13.5
#block_x = 10
block_y = 5.
# set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
# set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
# set_default_camera()
dump_world()
#print(get_camera())
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
if (command is None) or (display is None):
print('Unable to find a plan! Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
print('{}?'.format(display))
wait_for_interrupt()
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def test_grasps(robot, block):
for arm in ARMS:
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
tool_pose = get_link_pose(robot, tool_link)
#handles = draw_pose(tool_pose)
#grasps = get_top_grasps(block, under=True, tool_pose=unit_pose())
grasps = get_side_grasps(block, under=True, tool_pose=unit_pose())
for i, grasp_pose in enumerate(grasps):
block_pose = multiply(tool_pose, grasp_pose)
set_pose(block, block_pose)
close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm),
closed_conf=get_closed_positions(robot, arm))
handles = draw_pose(block_pose)
wait_if_gui('Grasp {}'.format(i))
remove_handles(handles)
def plan_water_motion(body,
joints,
end_conf,
attachment,
obstacles=None,
attachments=[],
self_collisions=True,
disabled_collisions=set(),
max_distance=MAX_DISTANCE,
weights=None,
resolutions=None,
reference_pose=unit_pose(),
custom_limits={},
**kwargs):
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles, {attachment} | set(attachments),
self_collisions,
disabled_collisions,
max_distance=max_distance,
custom_limits=custom_limits)
def water_test(q):
if attachment is None:
return False
set_joint_positions(body, joints, q)
attachment.assign()
attachment_pose = get_pose(attachment.child)
pose = multiply(attachment_pose,
reference_pose) # TODO: confirm not inverted
roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
#if violation: # TODO: check whether different confs can be waypoints for each object
# print(q, violation)
# wait_for_user()
return violation
invalid_fn = lambda q, **kwargs: water_test(q) or collision_fn(q, **kwargs)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, invalid_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
invalid_fn, **kwargs)
def draw_names(world, **kwargs):
# TODO: adjust the colors?
handles = []
if not DEBUG:
return handles
with ClientSaver(world.perception.client):
for name, body in world.perception.sim_bodies.items():
#add_body_name(body, **kwargs)
with PoseSaver(body):
set_pose(body, unit_pose())
lower, upper = get_aabb(
body
) # TODO: multi-link bodies doesn't seem to update when moved
handles.append(
add_text(name, position=upper, parent=body,
**kwargs)) # parent_link=0,
#handles.append(draw_pose(get_pose(body)))
#handles.extend(draw_base_limits(get_base_limits(world.pr2), color=(1, 0, 0)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_VIS_DISTANCE, color=(0, 0, 1)))
#handles.extend(draw_circle(get_point(world.pr2), MAX_REG_DISTANCE, color=(0, 0, 1)))
#from plan_tools.debug import test_voxels
#test_voxels(world)
return handles
def get_reference_pose(name): # TODO: bluecup_rotated
for model, quat in STABLE_QUATS.items():
if is_obj_type(name, model):
return (unit_point(), quat)
return unit_pose()
def get_world_from_reference(self):
if self.reference_body is None:
return unit_pose()
self.assign()
return get_link_pose(self.reference_body, self.reference_link)
def set_tool_pose(world, tool_pose):
#root_from_urdf = multiply(invert(get_pose(world.gripper, BASE_LINK)), # Previously 0?
# get_pose(world.gripper))
root_from_urdf = unit_pose()
tool_from_root = get_tool_from_root(world.robot)
set_pose(world.gripper, multiply(tool_pose, tool_from_root, root_from_urdf))
MODELS_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.pardir, 'models/')
VISUAL = True
if VISUAL:
FRANKA_CARTER_PATH = os.path.join(MODELS_PATH, 'panda_arm_hand_on_carter_visual.urdf')
else:
FRANKA_CARTER_PATH = os.path.join(MODELS_PATH, 'panda_arm_hand_on_carter_collision.urdf')
DEBUG = True
BASE_JOINTS = ['x', 'y', 'theta']
WHEEL_JOINTS = ['left_wheel', 'right_wheel']
FRANKA_CARTER = 'franka_carter'
FRANKA_TOOL_LINK = 'right_gripper' # right_gripper | panda_wrist_end_pt | panda_forearm_end_pt
TOOL_POSE = unit_pose()
# +z: pointing, +y: left finger
FINGER_EXTENT = np.array([0.02, 0.01, 0.02]) # 2cm x 1cm x 2cm
FRANKA_GRIPPER_LINK = 'panda_link7' # panda_link7 | panda_link8 | panda_hand
################################################################################
TOP_GRASP = 'top'
SIDE_GRASP = 'side' # TODO: allow normal side grasps for cabinets?
UNDER_GRASP = 'under' # TODO: for franka_carter
GRASP_TYPES = [
TOP_GRASP,
SIDE_GRASP,
]
APPROACH_DISTANCE = 0.075 # 0.075 | 0.1
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
#ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB)
#ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box'))
#print(ycb_path)
#load_pybullet(ycb_path)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
#set_euler(table, Euler(yaw=np.pi/2))
with HideOutput(False):
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
#robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf')
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
#robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table)
set_point(block1, Point(z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=+0.25, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, z=block_z))
blocks = [block1, block2, block3]
add_line(Point(x=-TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
Point(x=+TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
color=RED)
set_camera_pose(camera_point=Point(y=-1, z=block_z + 1),
target_point=Point(z=block_z))
wait_for_user()
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
y_grasp, x_grasp = get_top_grasps(block1,
tool_pose=unit_pose(),
grasp_length=0.03,
under=False)
grasp = y_grasp # fingers won't collide
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user('Finish?')
disconnect()
def plan_extrusion(args_list,
viewer=False,
verify=False,
verbose=False,
watch=False):
results = []
if not args_list:
return results
# TODO: setCollisionFilterGroupMask
if not verbose:
sys.stdout = open(os.devnull, 'w')
problems = {args.problem for args in args_list}
assert len(problems) == 1
[problem] = problems
# TODO: change dir for pddlstream
extrusion_path = get_extrusion_path(problem)
if False:
extrusion_path = transform_json(problem)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path,
verbose=True)
elements = sorted(element_from_id.values())
#node_points = transform_model(problem, elements, node_points, ground_nodes)
connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR)
with LockRenderer(lock=True):
draw_pose(unit_pose(), length=1.)
json_data = read_json(extrusion_path)
draw_pose(parse_origin_pose(json_data))
draw_model(elements, node_points, ground_nodes)
obstacles, robot = load_world()
color = apply_alpha(BLACK, alpha=0) # 0, 1
#color = None
element_bodies = dict(
zip(elements,
create_elements_bodies(node_points, elements, color=color)))
set_extrusion_camera(node_points)
#if viewer:
# label_nodes(node_points)
saver = WorldSaver()
wait_if_gui()
#visualize_stiffness(extrusion_path)
#debug_elements(robot, node_points, node_order, elements)
initial_position = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
checker = None
plan = None
for args in args_list:
if args.stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
saver.restore()
#initial_conf = get_joint_positions(robot, get_movable_joints(robot))
with LockRenderer(lock=not viewer):
start_time = time.time()
plan, data = None, {}
with timeout(args.max_time):
with Profiler(num=10, cumulative=False):
plan, data = solve_extrusion(robot,
obstacles,
element_from_id,
node_points,
element_bodies,
extrusion_path,
ground_nodes,
args,
checker=checker)
runtime = elapsed_time(start_time)
sequence = recover_directed_sequence(plan)
if verify:
max_translation, max_rotation = compute_plan_deformation(
extrusion_path, recover_sequence(plan))
valid = check_plan(extrusion_path, sequence)
print('Valid:', valid)
safe = validate_trajectories(element_bodies, obstacles, plan)
print('Safe:', safe)
data.update({
'safe': safe,
'valid': valid,
'max_translation': max_translation,
'max_rotation': max_rotation,
})
data.update({
'runtime':
runtime,
'num_elements':
len(elements),
'ee_distance':
compute_sequence_distance(node_points,
sequence,
start=initial_position,
end=initial_position),
'print_distance':
get_print_distance(plan, teleport=True),
'distance':
get_print_distance(plan, teleport=False),
'sequence':
sequence,
'parameters':
get_global_parameters(),
'problem':
args.problem,
'algorithm':
args.algorithm,
'heuristic':
args.bias,
'plan_extrusions':
not args.disable,
'use_collisions':
not args.cfree,
'use_stiffness':
args.stiffness,
})
print(data)
results.append((args, data))
if True:
data.update({
'assembly': read_json(extrusion_path),
'plan': extract_plan_data(plan), # plan | trajectories
})
plan_file = '{}_solution.json'.format(args.problem)
plan_path = os.path.join('solutions', plan_file)
write_json(plan_path, data)
reset_simulation()
disconnect()
if watch and (plan is not None):
args = args_list[-1]
animate = not (args.disable or args.ee_only)
connect(use_gui=True, shadows=SHADOWS,
color=BACKGROUND_COLOR) # TODO: avoid reconnecting
obstacles, robot = load_world()
display_trajectories(
node_points,
ground_nodes,
plan, #time_step=None, video=True,
animate=animate)
reset_simulation()
disconnect()
if not verbose:
sys.stdout.close()
return results