def iterate(self, world, attachments):
joints = get_gripper_joints(world.robot, self.arm)
start_conf = get_joint_positions(world.robot, joints)
end_conf = [self.position] * len(joints)
extend_fn = get_extend_fn(world.robot, joints)
path = [start_conf] + list(extend_fn(start_conf, end_conf))
for positions in path:
set_joint_positions(world.robot, joints, positions)
yield positions
def gen_fn(arm, obj_name, pose):
open_width = get_max_limit(world.robot,
get_gripper_joints(world.robot, arm)[0])
obj_body = world.bodies[obj_name]
#grasps = cycle([(grasp,)])
grasps = cycle(grasp_gen_fn(obj_name))
for attempt in range(max_attempts):
try:
grasp, = next(grasps)
except StopIteration:
break
# TODO: if already successful for a grasp, continue
set_pose(obj_body, pose)
set_gripper_position(world.robot, arm, open_width)
robot_saver = BodySaver(
world.robot) # TODO: robot might be at the wrong conf
body_saver = BodySaver(obj_body)
pretool_pose = multiply(pose, invert(grasp.pregrasp_pose))
tool_pose = multiply(pose, invert(grasp.grasp_pose))
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_pose,
obstacles=obstacles)
if grip_conf is None:
continue
#attachment = Attachment(world.robot, tool_link, get_grasp_pose(grasp), world.bodies[obj])
# Attachments cause table collisions
post_path = plan_waypoint_motion(
world.robot,
arm,
pretool_pose,
obstacles=obstacles,
attachments=[], #attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pick',
'objects': [obj_name],
'context':
Context(savers=[robot_saver, body_saver], attachments={}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
GripperCommand(arm, grasp.grasp_width,
effort=grasp.effort),
Attach(arm, obj_name, effort=grasp.effort),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (grasp, pre_conf, post_conf, control)
def compute_grasp_width(robot, arm, body, grasp_pose, **kwargs):
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
body_pose = multiply(tool_pose, grasp_pose)
set_pose(body, body_pose)
return close_until_collision(robot,
gripper_joints,
bodies=[body],
max_distance=0.0,
**kwargs)
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)
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
def set_gripper_position(robot, arm, position):
gripper_joints = get_gripper_joints(robot, arm)
set_joint_positions(robot, gripper_joints,
[position] * len(gripper_joints))