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 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 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 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 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 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 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 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 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 test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def main(display='control'): # control | execute | step
connect(use_gui=True)
disable_real_time()
# KUKA_IIWA_URDF | DRAKE_IIWA_URDF
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
# floor = load_model('models/short_floor.urdf')
floor = p.loadURDF("plane.urdf")
block = load_model(
"models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_user()
disconnect()
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 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 create_door(width=0.08,
length=1,
height=2,
mass=1,
handle=True,
frame=True,
**kwargs):
# TODO: hinge, cylinder on end, sliding door, knob
# TODO: explicitly disable self collisions (happens automatically)
geometry = get_box_geometry(width, length, height)
hinge = 0 # -width/2
com_point = Point(x=hinge, y=-length / 2., z=height / 2.)
door_collision, door_visual = create_shape(geometry,
pose=Pose(com_point),
**kwargs)
door_link = LinkInfo(
mass=mass,
collision_id=door_collision,
visual_id=door_visual,
#point=com_point,
inertial_point=com_point, # TODO: be careful about the COM
parent=0,
joint_type=p.JOINT_REVOLUTE,
joint_axis=[0, 0, 1])
links = [door_link]
if handle:
links.append(create_handle(width, length, height))
if frame:
links.append(create_frame(width, length, height))
body = create_multi_body(base_link=LinkInfo(), links=links)
#draw_circle(center=unit_point(), diameter=width/2., parent=body, parent_link=0)
#set_joint_limits(body, link=0, lower=-PI, upper=PI)
return body
def add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
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 lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z):
assert min_spoon_z <= max_spoon_z
bowl_body = world.get_body(bowl_name)
bowl_urdf_from_center = get_urdf_from_base(
bowl_body) # get_urdf_from_center
spoon_body = world.get_body(spoon_name)
spoon_quat = lookup_orientation(spoon_name, STIR_QUATS)
spoon_urdf_from_center = get_urdf_from_base(
spoon_body, reference_quat=spoon_quat) # get_urdf_from_center
# Keeping the orientation consistent for generalization purposes
# TODO: what happens when the base isn't flat (e.g. bowl)
bowl_pose = get_pose(bowl_body)
stir_z = None
for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005):
bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)),
invert(spoon_urdf_from_center))
set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer))
#wait_for_user()
if pairwise_collision(bowl_body, spoon_body):
# Want to be careful not to make contact with the base
break
stir_z = z
return stir_z
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
draw_global_system()
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera(distance=2)
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
wait_if_gui('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_if_gui()
disconnect()
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 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 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_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 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 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 main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
floor_x = 2
set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
# set_default_camera()
dump_world()
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
print('Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
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 move_occluding(world):
# Prevent obstruction by other objects
# TODO: this is a bit of a hack due to pybullet
world.set_base_conf([-5.0, 0, 0])
for joint in world.kitchen_joints:
joint_name = get_joint_name(world.kitchen, joint)
if joint_name in DRAWERS:
world.open_door(joint)
else:
world.close_door(joint)
for name in world.movable:
set_pose(world.get_body(name), Pose(Point(z=-5.0)))
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 get_bowl_from_pivot(world, feature, parameter):
bowl_body = world.get_body(feature['bowl_name'])
bowl_urdf_from_center = get_urdf_from_top(
bowl_body) # get_urdf_from_base | get_urdf_from_center
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
bowl_base_from_pivot = Pose(
Point(x=parameter['axis_in_bowl_x'], z=parameter['axis_in_bowl_z']))
return multiply(bowl_urdf_from_center, bowl_base_from_pivot)
