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)