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 test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_user()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_user()
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_spoon_grasps(name, body, under=False, grasp_length=0.02):
# TODO: scale thickness based on size
thickness = SPOON_THICKNESSES[get_type(name)]
# Origin is in the center of the spoon's hemisphere head
# grasp_length depends on the grasp position
center, extent = approximate_as_prism(body)
for k in range(1 + under):
rotate_y = Pose(euler=Euler(pitch=-np.pi / 2 + np.pi * k))
rotate_z = Pose(euler=Euler(yaw=-np.pi / 2))
length = (-center + extent / 2)[1] - grasp_length
translate_x = Pose(point=Point(x=length, y=-thickness / 2.))
gripper_from_spoon = multiply(translate_x, rotate_z, rotate_y)
yield gripper_from_spoon
def observe_pybullet(world):
# TODO: randomize robot's pose
# TODO: probabilities based on whether in viewcone or not
# TODO: sample from poses on table
# world_saver = WorldSaver()
visible_entities = are_visible(world)
detections = {}
assert OBS_P_FP == 0
for name in visible_entities:
# TODO: false positives
if random.random() < OBS_P_FN:
continue
body = world.get_body(name)
pose = get_pose(body)
dx, dy = np.random.multivariate_normal(mean=np.zeros(2),
cov=math.pow(OBS_POS_STD, 2) *
np.eye(2))
dyaw, = np.random.multivariate_normal(mean=np.zeros(1),
cov=math.pow(OBS_ORI_STD, 2) *
np.eye(1))
print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format(
name, dx, dy, dyaw))
noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw))
observed_pose = multiply(pose, noise_pose)
#world.get_body_type(name)
detections.setdefault(name, []).append(
observed_pose) # TODO: use type instead
#world_saver.restore()
return detections
def relative_detections(belief, detections):
world = belief.world
rel_detections = {}
world_aabb = world.get_world_aabb()
for name in detections:
if name == belief.holding:
continue
body = world.get_body(name)
for observed_pose in detections[name]:
world_z_axis = np.array([0, 0, 1])
local_z_axis = tform_point(observed_pose, world_z_axis)
if np.pi / 2 < angle_between(world_z_axis, local_z_axis):
observed_pose = multiply(observed_pose,
Pose(euler=Euler(roll=np.pi)))
if not aabb_contains_point(point_from_pose(observed_pose),
world_aabb):
continue
set_pose(body, observed_pose)
support = world.get_supporting(name)
#assert support is not None
# Could also fix as relative to the world
if support is None:
# TODO: prune if nowhere near a surface (e.g. on the robot)
relative_pose = create_world_pose(world, name, init=True)
else:
relative_pose = create_relative_pose(world,
name,
support,
init=True)
rel_detections.setdefault(name, []).append(relative_pose)
# relative_pose.assign()
return rel_detections
def test_push(visualize):
arms = [LEFT_ARM]
block_name = create_name('purpleblock', 1) # greenblock | purpleblock
world, table_body = create_world([block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
#pos = (0.6, 0, block_z)
pos = (0.5, 0.2, block_z)
world.perception.set_pose(block_name, pos, quat_from_euler(Euler(yaw=-np.pi/6)))
update_world(world, table_body)
# TODO: push into reachable region
goal_pos2d = np.array(pos[:2]) + np.array([0.025, 0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([0.025, -0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([-0.1, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0.15, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0, 0.7]) # Intentionally not feasible
draw_point(np.append(goal_pos2d, [block_z]), size=0.05, color=(1, 0, 0))
init = [
('CanPush', block_name, goal_pos2d),
]
goal = [
('InRegion', block_name, goal_pos2d),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
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 fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
def sample_stir_trajectory(world, feature, parameter):
bowl_urdf_from_center = get_urdf_from_base(
world.bodies[feature['bowl_name']])
stirrer_quat = lookup_orientation(feature['spoon_name'], STIR_QUATS)
stirrer_urdf_from_center = get_urdf_from_base(
world.bodies[feature['spoon_name']], reference_quat=stirrer_quat)
stir_pos = np.array([0., 0., parameter['stir_z']])
entry_pos = np.array([0, 0, parameter['entry_z']])
# TODO: could rotate the wrist in place
# TODO: could reverse the trajectory afterwards
# TODO: could connect up the start and end
step_size = np.pi / 16
stir_positions = [
stir_pos + parameter['stir_radius'] *
np.array([math.cos(theta), math.sin(theta), 0])
for theta in np.arange(0, parameter['revolutions'] * 2 *
np.pi, step_size)
] # Counter-clockwise
entry_pos = stir_positions[0] + (entry_pos - stir_pos)
exit_pos = stir_positions[-1] + (entry_pos - stir_pos)
initial_yaw = random.uniform(-np.pi, np.pi)
rotate_stirrer = Pose(euler=Euler(yaw=initial_yaw))
# TODO(caelan): check the ordering/inversion when references are not the identity
return [
multiply(bowl_urdf_from_center, Pose(point=point), rotate_stirrer,
invert(stirrer_urdf_from_center))
for point in ([entry_pos] + stir_positions + [exit_pos])
]
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_user()
def mirror_robot(robot1, node_points):
# TODO: place robots side by side or diagonal across
set_extrusion_camera(node_points, theta=-np.pi / 3)
#draw_pose(Pose())
centroid = np.average(node_points, axis=0)
centroid_pose = Pose(point=centroid)
#draw_pose(Pose(point=centroid))
# print(centroid)
scale = 0. # 0.15
vector = get_point(robot1) - centroid
set_pose(robot1, Pose(point=Point(*+scale * vector[:2])))
# Inner product of end-effector z with base->centroid or perpendicular to this line
# Partition by sides
robot2 = load_robot()
set_pose(
robot2,
Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi)))
# robots = [robot1]
robots = [robot1, robot2]
for robot in robots:
set_configuration(robot, DUAL_CONF)
# joint1 = get_movable_joints(robot)[0]
# set_joint_position(robot, joint1, np.pi / 8)
draw_pose(get_pose(robot), length=0.25)
return robots
def __init__(self,
end_effector,
element_bodies,
node_points,
ground_nodes,
element,
reverse,
max_directions=INF):
# TODO: more directions for ground collisions
self.end_effector = end_effector
self.element = element
self.reverse = reverse
self.max_directions = max_directions
self.element_pose = get_pose(element_bodies[element])
n1, n2 = element
length = get_distance(node_points[n1], node_points[n2])
self.translation_path = np.append(
np.arange(-length / 2, length / 2, STEP_SIZE), [length / 2])
if ORTHOGONAL_GROUND and is_ground(element, ground_nodes):
# TODO: orthogonal to the ground
self.direction_generator = cycle(
[Pose(euler=Euler(roll=0, pitch=0))])
else:
self.direction_generator = get_direction_generator(
self.end_effector, use_halton=False)
self.trajectories = []
def pose2d_on_surface(world, entity_name, surface_name, pose2d=UNIT_POSE2D):
x, y, yaw = pose2d
body = world.get_body(entity_name)
surface_aabb = compute_surface_aabb(world, surface_name)
z = stable_z_on_aabb(body, surface_aabb)
pose = Pose(Point(x, y, z), Euler(yaw=yaw))
set_pose(body, pose)
return pose
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 gen_fn(index):
brick = brick_from_index[index]
while True:
original_grasp = random.choice(brick.grasps)
theta = random.uniform(-np.pi, +np.pi)
rotation = Pose(euler=Euler(yaw=theta))
new_attach = multiply(rotation, original_grasp.attach)
grasp = Grasp(None, None, None, new_attach, None)
yield grasp,
def rotate_assembly(elements, node_points, ground_nodes, yaw=0.):
# TODO: more general affine transformations
world_from_base = Pose(point=get_base_centroid(node_points, ground_nodes))
points_base = apply_affine(invert(world_from_base), node_points)
rotation = Pose(euler=Euler(yaw=yaw))
points_world = list(map(np.array, apply_affine(multiply(world_from_base, rotation), points_base)))
#draw_model(elements, scaled_node_points, ground_nodes, color=RED)
#wait_if_gui()
return points_world
def rotate_problem(problem_path, roll=np.pi):
tform = Pose(euler=Euler(roll=roll))
json_data = affine_extrusion(problem_path, tform)
path = 'rotated.json' # TODO: create folder
with open(path, 'w') as f:
json.dump(json_data, f, indent=2, sort_keys=True)
problem_path = path
# TODO: rotate the whole robot as well
# TODO: could also use the z heuristic when upside down
return problem_path
def pour_path_from_parameter(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
cup_body = world.get_body(cup_name)
cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)
#####
obj_type = type_from_name(cup_name)
if obj_type in [MUSTARD]:
initial_pitch = final_pitch = -np.pi
radius = 0
else:
initial_pitch = 0 # different if mustard
final_pitch = -3 * np.pi / 4
radius = bowl_d / 2
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0 # meters
#axis_in_cup_center_z = -cup_h/2.
axis_in_cup_center_z = 0. # meters
#axis_in_cup_center_z = +cup_h/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = final_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))
pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
base_from_pivot = Pose(
Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))
#####
assert -np.pi <= final_pitch <= initial_pitch
pitches = [initial_pitch]
if final_pitch != initial_pitch:
pitches = list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + pitches
cup_path_in_bowl = []
for pitch in pitches:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
rotate_pivot, invert(base_from_pivot),
invert(Pose(point=cup_center))))
return cup_path_in_bowl
def gen_fn(arm, bowl_name, bowl_pose, stirrer_name, grasp):
if bowl_name == stirrer_name:
return
bowl_body = world.bodies[bowl_name]
attachment = get_grasp_attachment(world, arm, grasp)
feature = get_stir_feature(world, bowl_name, stirrer_name)
parameter_generator = parameter_fn(world, feature)
if revisit:
parameter_generator = cycle(parameter_generator)
for parameter in parameter_generator:
for _ in range(max_attempts):
set_gripper_position(world.robot, arm, grasp.grasp_width)
set_pose(bowl_body, bowl_pose)
stirrer_path_bowl = sample_stir_trajectory(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
stirrer_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in stirrer_path_bowl
]
#visualize_cartesian_path(world.get_body(stirrer_name), stirrer_path)
arm_path = plan_attachment_motion(
world.robot,
arm,
attachment,
stirrer_path,
obstacles=set(obstacles) - {bowl_body},
self_collisions=collisions,
disabled_collisions=disabled_collisions,
collision_buffer=collision_buffer,
attachment_collisions=False)
if arm_path is None:
continue
pre_conf = Conf(arm_path[0])
post_conf = Conf(arm_path[-1])
control = Control({
'action':
'stir',
'objects': [bowl_name, stirrer_name],
'feature':
feature,
'parameter':
parameter,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={stirrer_name: attachment}),
'commands': [
ArmTrajectory(arm, arm_path),
],
})
yield (pre_conf, post_conf, control)
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
def pour_path_from_parameter(world, feature, parameter, cup_yaw=None):
cup_body = world.get_body(feature['cup_name'])
#cup_urdf_from_center = get_urdf_from_center(cup_body, reference_quat=get_liquid_quat(feature['cup_name']))
ref_from_urdf = (unit_point(), get_liquid_quat(feature['cup_name']))
cup_center_in_ref, _ = approximate_as_prism(cup_body,
body_pose=ref_from_urdf)
cup_center_in_ref[:
2] = 0 # Assumes the xy pour center is specified by the URDF (e.g. for spoons)
cup_urdf_from_center = multiply(invert(ref_from_urdf),
Pose(point=cup_center_in_ref))
# TODO: allow some deviation around cup_yaw for spoons
if cup_yaw is None:
cup_yaw = random.choice([0, np.pi]) if 'spoon' in feature['cup_name'] \
else random.uniform(-np.pi, np.pi)
z_rotate_cup = Pose(euler=Euler(yaw=cup_yaw))
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
base_from_pivot = Pose(
Point(x=parameter['axis_in_cup_x'], z=parameter['axis_in_cup_z']))
initial_pitch = 0
final_pitch = parameter['pitch']
assert -np.pi <= final_pitch <= initial_pitch
cup_path_in_bowl = []
for pitch in list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + [initial_pitch]:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(bowl_from_pivot, rotate_pivot, invert(base_from_pivot),
z_rotate_cup, invert(cup_urdf_from_center)))
cup_times = constant_velocity_times(cup_path_in_bowl)
# TODO: check for collisions here?
return cup_path_in_bowl, cup_times
def get_direction_generator(end_effector, **kwargs):
world_from_base = get_pose(end_effector.robot)
lower = [-np.pi / 2, -np.pi / 2]
upper = [+np.pi / 2, +np.pi / 2]
for [roll, pitch] in interval_generator(lower, upper, **kwargs):
##roll = random.uniform(0, np.pi)
#roll = np.pi/4
#pitch = random.uniform(0, 2*np.pi)
#return Pose(euler=Euler(roll=np.pi / 2 + roll, pitch=pitch))
#roll = random.uniform(-np.pi/2, np.pi/2)
#pitch = random.uniform(-np.pi/2, np.pi/2)
pose = Pose(euler=Euler(roll=roll, pitch=pitch))
yield pose
def test_shelves(visualize):
arms = [LEFT_ARM]
# TODO: smaller (2) shelves
# TODO: sample with respect to the link it will supported by
# shelves.off | shelves_points.off | tableShelves.off
# import os
# data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/'
# mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off'))
# draw_mesh(mesh)
# wait_for_interrupt()
start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4
end_link = 'shelf1'
shelf_name = 'two_shelves'
#shelf_name = 'three_shelves'
cup_name = create_name('bluecup', 1)
world, table_body = create_world([shelf_name, cup_name], visualize=visualize)
cup_x = 0.65
#shelf_x = 0.8
shelf_x = 0.75
initial_poses = {
shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))),
#cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
shelf_body = world.perception.sim_bodies[shelf_name]
#print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)])
#print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])])
#shelf_link = None
shelf_link = link_from_name(shelf_body, start_link)
cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON
world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat())
update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5]))
init = [
('Stackable', cup_name, shelf_name, end_link),
]
goal = [
('On', cup_name, shelf_name, end_link),
#('On', cup_name, TABLE_NAME, TOP),
#('Holding', cup_name),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def create_elements_bodies(node_points,
elements,
color=apply_alpha(RED, alpha=1),
diameter=ELEMENT_DIAMETER,
shrink=ELEMENT_SHRINK):
# TODO: could scale the whole environment
# TODO: create a version without shrinking for transit planning
# URDF_USE_IMPLICIT_CYLINDER
element_bodies = []
for (n1, n2) in elements:
p1, p2 = node_points[n1], node_points[n2]
height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0)
#if height == 0: # Cannot keep this here
# continue
center = (p1 + p2) / 2
# extents = (p2 - p1) / 2
delta = p2 - p1
x, y, z = delta
phi = np.math.atan2(y, x)
theta = np.math.acos(z / np.linalg.norm(delta))
quat = quat_from_euler(Euler(pitch=theta, yaw=phi))
# p1 is z=-height/2, p2 is z=+height/2
# Much smaller than cylinder
# Also faster, C_shape 177.398 vs 400
body = create_box(diameter,
diameter,
height,
color=color,
mass=STATIC_MASS)
set_color(body, color)
set_point(body, center)
set_quat(body, quat)
#dump_body(body, fixed=True)
# Visually, smallest diameter is 2e-3
# The geometries and bounding boxes seem correct though
# TODO: create_cylinder takes in a radius not diameter
#body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS)
#print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height))
#print(get_aabb_extent(get_aabb(body)).round(6).tolist())
#print(get_visual_data(body))
#print(get_collision_data(body))
#set_point(body, center)
#set_quat(body, quat)
#draw_aabb(get_aabb(body))
element_bodies.append(body)
#wait_for_user()
return element_bodies
def test_stir(visualize):
# TODO: change the stirrer coordinate frame to be on its side
arms = [LEFT_ARM]
#spoon_name = 'spoon' # spoon.urdf is very messy and has a bad bounding box
#spoon_quat = multiply_quats(quat_from_euler(Euler(pitch=-np.pi/2 - np.pi/16)), quat_from_euler(Euler(yaw=-np.pi/8)))
#spoon_name, spoon_quat = 'stirrer', quat_from_euler(Euler(roll=-np.pi/2, yaw=np.pi/2))
spoon_name, spoon_quat = 'grey_spoon', quat_from_euler(Euler(yaw=-np.pi/2)) # grey_spoon | orange_spoon | green_spoon
# *_spoon points in +y
#bowl_name = 'bowl'
bowl_name = 'whitebowl'
items = [spoon_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_quat(world.get_body(spoon_name), spoon_quat) # get_reference_pose(spoon_name)
initial_positions = {
#spoon_name: [0.5, -0.2, 0],
spoon_name: [0.3, 0.5, 0],
bowl_name: [0.6, 0.1, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
#draw_aabb(get_aabb(body))
#wait_for_interrupt()
#enable_gravity()
#for i in range(10):
# simulate_for_sim_duration(sim_duration=0.05, frequency=0.1)
# print(get_quat(world.perception.sim_bodies[spoon_name]))
# raw_input('Continue?')
update_world(world, table_body)
init_holding = hold_item(world, arms[0], spoon_name)
assert init_holding is not None
# TODO: add the concept of a recipe
init = [
('Contains', bowl_name, COFFEE),
('Contains', bowl_name, SUGAR),
]
goal = [
#('Holding', spoon_name),
('Mixed', bowl_name),
]
task = Task(init=init, init_holding=init_holding, goal=goal,
arms=arms, reset_arms=False)
return world, task
def sample_scoop_trajectory(world, feature, parameter, collisions=True):
bowl_body = world.get_body(feature['bowl_name'])
bowl_urdf_from_center = get_urdf_from_base(bowl_body) # get_urdf_from_base
spoon_body = world.get_body(feature['spoon_name'])
spoon_quat = lookup_orientation(feature['spoon_name'], STIR_QUATS)
spoon_urdf_from_center = get_urdf_from_base(
spoon_body, reference_quat=spoon_quat) # get_urdf_from_base
if RELATIVE_SCOOP:
parameter = scale_parameter(feature,
parameter,
RELATIVE_SCOOP_SCALING,
descale=True)
#entry_z = parameter['entry_z'] # Ends up colliding quite frequently
entry_z = feature['bowl_height'] + TOP_OFFSET_Z
exit_z = parameter['exit_z'] + feature[
'bowl_height'] + feature['spoon_length'] / 2
entry_pos = np.array([0., parameter['start_scoop_y'], entry_z])
start_pos = np.array(
[0., parameter['start_scoop_y'], parameter['scoop_z']])
end_pos = np.array([0., parameter['end_scoop_y'], parameter['scoop_z']])
exit_pos = np.array([0., parameter['exit_y'], exit_z])
spoon_path_in_bowl = interpolate_pose_waypoints(
[Pose(point=point) for point in [entry_pos, start_pos, end_pos]] +
[(Pose(exit_pos, Euler(roll=-np.pi / 2)))])
# TODO(caelan): check the ordering/inversion when references are not the identity
spoon_path = [
multiply(bowl_urdf_from_center, spoon_pose_in_bowl,
invert(spoon_urdf_from_center))
for spoon_pose_in_bowl in spoon_path_in_bowl
]
#draw_pose(get_pose(bowl_body))
#set_pose(spoon_body, multiply(get_pose(bowl_body), bowl_urdf_from_center,
# Pose(point=start_pos), invert(spoon_urdf_from_center)))
#set_pose(bowl_body, Pose())
#wait_for_user()
collision_path = [spoon_path[0], spoon_path[-1]]
#collision_path = [spoon_path[-1]]
if collisions and bowl_path_collision(bowl_body, spoon_body,
collision_path):
return None
return spoon_path
def pose_from_pose2d(self, pose2d, surface):
#assert surface in self.poses_from_surface
#reference_pose = self.poses_from_surface[surface][0]
body = self.world.get_body(self.name)
surface_aabb = compute_surface_aabb(self.world, surface)
world_from_surface = get_surface_reference_pose(
self.world.kitchen, surface)
if DIM == 2:
x, y = pose2d[:DIM]
yaw = np.random.uniform(*CIRCULAR_LIMITS)
else:
x, y, yaw = pose2d
z = stable_z_on_aabb(
body,
surface_aabb) + Z_EPSILON - point_from_pose(world_from_surface)[2]
point = Point(x, y, z)
surface_from_body = Pose(point, Euler(yaw=yaw))
set_pose(body, multiply(world_from_surface, surface_from_body))
return create_relative_pose(self.world, self.name, surface)
def transform_json(problem):
original_path = get_extrusion_path(problem)
#new_path = '{}_transformed.json'.format(problem)
new_path = 'transformed.json'.format(problem) # TODO: create folder
#pose = Pose(euler=Euler(roll=np.pi / 2.))
yaw = np.pi / 4.
pose = Pose(euler=Euler(yaw=yaw))
tform = tform_from_pose(pose)
tform = rotation_matrix(angle=yaw, direction=[0, 0, 1])
scale = SCALE_ASSEMBLY.get(problem, DEFAULT_SCALE)
#tform = scale*np.identity(4)
tform = scale_matrix(scale, origin=None, direction=None)
json_data = affine_extrusion(original_path, tform)
write_json(new_path, json_data)
# TODO: rotate the whole robot as well
# TODO: could also use the z heuristic when upside down
return new_path
def _initialize_environment(self):
# wall to fridge: 4cm
# fridge to goal: 1.5cm
# hitman to range: 3.5cm
# range to indigo: 3.5cm
self.environment_poses = read_json(POSES_PATH)
root_from_world = get_link_pose(self.kitchen, self.world_link)
for name, world_from_part in self.environment_poses.items():
if name in ['range']:
continue
visual_path = os.path.join(KITCHEN_LEFT_PATH,
'{}.obj'.format(name))
collision_path = os.path.join(KITCHEN_LEFT_PATH,
'{}_collision.obj'.format(name))
mesh_path = None
for path in [collision_path, visual_path]:
if os.path.exists(path):
mesh_path = path
break
if mesh_path is None:
continue
body = load_pybullet(mesh_path, fixed_base=True)
root_from_part = multiply(root_from_world, world_from_part)
if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']:
(pos, quat) = root_from_part
# TODO: still not totally aligned
pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005])
root_from_part = (pos, quat)
self.environment_bodies[name] = body
set_pose(body, root_from_part)
# TODO: release bounding box or convex hull
# TODO: static object nonconvex collisions
if TABLE_NAME in self.environment_bodies:
body = self.environment_bodies[TABLE_NAME]
_, (w, l, _) = approximate_as_prism(body)
_, _, z = get_point(body)
new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z),
Euler(yaw=np.pi / 2))
set_pose(body, new_pose)
