def parse_object(obj, mesh_directory): name = obj.find('name').text mesh_filename = obj.find('geom').text geom = get_mesh_geometry(os.path.join(mesh_directory, mesh_filename)) pose = parse_pose(obj.find('pose')) movable = parse_boolean(obj.find('moveable')) color = (.75, .75, .75, 1) if 'red' in name: color = (1, 0, 0, 1) elif 'green' in name: color = (0, 1, 0, 1) elif 'blue' in name: color = (0, 0, 1, 1) elif movable: # TODO: assign new color #size = 2 * MAX_INT #size = 255 #n = id(obj) % size #n = hash(obj) % size #h = float(n) / size h = random.random() r, g, b = colorsys.hsv_to_rgb(h, .75, .75) color = (r, g, b, 1) print(name, mesh_filename, movable, color) collision_id, visual_id = create_shape(geom, color=color) body_id = p.createMultiBody(baseMass=STATIC_MASS, baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id, physicsClientId=CLIENT) set_pose(body_id, pose) return body_id
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 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 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 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 create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) robot_saver = BodySaver(robot) gripper_from_base_list = [] grasps = GET_GRASPS[grasp_type](body) start_time = time.time() while len(gripper_from_base_list) < num_samples: box_pose = sample_placement(body, table) set_pose(body, box_pose) grasp_pose = random.choice(grasps) gripper_pose = multiply(box_pose, invert(grasp_pose)) for attempt in range(max_attempts): robot_saver.restore() base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.))) #set_base_values(robot, base_conf) set_group_conf(robot, 'base', base_conf) if pairwise_collision(robot, table): continue grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT) #conf = inverse_kinematics(robot, link, gripper_pose) if (grasp_conf is None) or pairwise_collision(robot, table): continue gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) #wait_if_gui() gripper_from_base_list.append(gripper_from_base) print('{} / {} | {} attempts | [{:.3f}]'.format( len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time))) wait_if_gui() break else: print('Failed to find a kinematic solution after {} attempts'.format(max_attempts)) return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
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 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 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 cartesian_path_collision(body, path, obstacles, **kwargs): for pose in path: set_pose(body, pose) if any( body_pair_collision(body, obst, **kwargs) for obst in obstacles): return True return False
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 bowl_path_collision(bowl_body, body, body_path_bowl): bowl_pose = get_pose(bowl_body) with BodySaver(body): for cup_pose_bowl in body_path_bowl: cup_pose_world = multiply(bowl_pose, cup_pose_bowl) set_pose(body, cup_pose_world) if body_pair_collision(bowl_body, body): #, collision_buffer=0.0): return True return False
def iterate_approach_path(world, pose, grasp, body=None): world_from_body = pose.get_world_from_body() grasp_pose = multiply(world_from_body, invert(grasp.grasp_pose)) approach_pose = multiply(world_from_body, invert(grasp.pregrasp_pose)) for tool_pose in interpolate_poses(grasp_pose, approach_pose): set_tool_pose(world, tool_pose) if body is not None: set_pose(body, multiply(tool_pose, grasp.grasp_pose)) yield
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model( "models/drake/pr2_description/urdf/pr2_simplified.urdf") set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) # head_link = link_from_name(pr2, HEAD_LINK) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) head_link = head_joints[-1] #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)), target_point, lineColorRGB=(1, 0, 0)) # addUserDebugText p.addUserDebugLine(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, lineColorRGB=(0, 0, 1)) # addUserDebugText # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point) set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_for_user() remove_body(detect_cone) remove_body(view_cone) disconnect()
def gen_fn(arm, obj_name, pose): open_width = get_max_limit(world.robot, get_gripper_joints(world.robot, arm)[0]) obj_body = world.bodies[obj_name] #grasps = cycle([(grasp,)]) grasps = cycle(grasp_gen_fn(obj_name)) for attempt in range(max_attempts): try: grasp, = next(grasps) except StopIteration: break # TODO: if already successful for a grasp, continue set_pose(obj_body, pose) set_gripper_position(world.robot, arm, open_width) robot_saver = BodySaver( world.robot) # TODO: robot might be at the wrong conf body_saver = BodySaver(obj_body) pretool_pose = multiply(pose, invert(grasp.pregrasp_pose)) tool_pose = multiply(pose, invert(grasp.grasp_pose)) grip_conf = solve_inverse_kinematics(world.robot, arm, tool_pose, obstacles=obstacles) if grip_conf is None: continue #attachment = Attachment(world.robot, tool_link, get_grasp_pose(grasp), world.bodies[obj]) # Attachments cause table collisions post_path = plan_waypoint_motion( world.robot, arm, pretool_pose, obstacles=obstacles, attachments=[], #attachments=[attachment], self_collisions=collisions, disabled_collisions=disabled_collisions) if post_path is None: continue pre_conf = Conf(post_path[-1]) pre_path = post_path[::-1] post_conf = pre_conf control = Control({ 'action': 'pick', 'objects': [obj_name], 'context': Context(savers=[robot_saver, body_saver], attachments={}), 'commands': [ ArmTrajectory(arm, pre_path, dialation=2.), GripperCommand(arm, grasp.grasp_width, effort=grasp.effort), Attach(arm, obj_name, effort=grasp.effort), ArmTrajectory(arm, post_path, dialation=2.), ], }) yield (grasp, pre_conf, post_conf, control)
def test(obj1, pose1, obj2, pose2): if not collisions or (obj1 == obj2): return False body1 = world.bodies[obj1] set_pose(body1, pose1) body2 = world.bodies[obj2] set_pose(body2, pose2) return body_pair_collision(body1, body2, collision_buffer=collision_buffer)
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 compute_grasp_width(robot, arm, body, grasp_pose, **kwargs): gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_FRAMES[arm]) tool_pose = get_link_pose(robot, tool_link) body_pose = multiply(tool_pose, grasp_pose) set_pose(body, body_pose) return close_until_collision(robot, gripper_joints, bodies=[body], max_distance=0.0, **kwargs)
def check_initial_collisions(world, obj_name, obst_names=[], **kwargs): obj_body = world.bodies[obj_name] for obst_name in obst_names: if obj_name == obst_name: continue obst_body = world.bodies[obst_name] set_pose(obst_body, world.initial_poses[obst_name]) if body_pair_collision(obj_body, obst_body, **kwargs): #print(obj_name, obst_name) return True return False
def _create_robot(self): with ClientSaver(self.client): with HideOutput(): pr2_path = os.path.join(get_models_path(), PR2_URDF) self.pr2 = load_pybullet(pr2_path, fixed_base=True) # Base link is the origin base_pose = get_link_pose(self.robot, link_from_name(self.robot, BASE_FRAME)) set_pose(self.robot, invert(base_pose)) return self.pr2
def visualize_cartesian_path(body, pose_path): for i, pose in enumerate(pose_path): set_pose(body, pose) print('{}/{}) continue?'.format(i, len(pose_path))) wait_for_user() handles = draw_pose(get_pose(body)) handles.extend(draw_aabb(get_aabb(body))) print('Finish?') wait_for_user() for h in handles: remove_debug(h)
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 load_body(self, name, pose=None, fixed_base=False): assert name not in self.sim_bodies ty = get_type(name) with ClientSaver(self.client): with HideOutput(): body = load_cup_bowl(ty) if body is None: body = load_pybullet(get_body_urdf(name), fixed_base=fixed_base) if pose is not None: set_pose(body, pose) self.sim_bodies[name] = body return body
def parse_region(region): lower = np.min(region['hull'], axis=0) upper = np.max(region['hull'], axis=0) x, y = (lower + upper) / 2. w, h = (upper - lower) # / 2. geom = get_box_geometry(w, h, 1e-3) collision_id, visual_id = create_shape(geom, pose=Pose(Point(x, y)), color=parse_color(region['color'])) #region_id = create_body(NULL_ID, visual_id) region_id = create_body(collision_id, visual_id) set_pose(region_id, parse_pose(region)) return region_id
def add_table(world, use_surface=False): # Use the table in simulation to ensure no penetration if use_surface: # TODO: note whether the convex hull image was clipped (only partial) table_mesh = rectangular_mesh(width=0.6, length=1.2) color = apply_alpha(COLORS['tan']) table_body = create_mesh(table_mesh, under=True, color=color) set_pose(table_body, TABLE_POSE) world.perception.sim_bodies[TABLE_NAME] = table_body world.perception.sim_surfaces[TABLE_NAME] = None else: table_body = world.perception.add_surface(TABLE_NAME, TABLE_POSE) return table_body
def gen_fn(index, pose, grasp): body = brick_from_index[index].body set_pose(body, pose.value) obstacles = list(obstacle_from_name.values()) # + [body] collision_fn = get_collision_fn( robot, movable_joints, obstacles=obstacles, attachments=[], self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, custom_limits=get_custom_limits(robot)) attach_pose = multiply(pose.value, invert(grasp.attach)) approach_pose = multiply(attach_pose, (approach_vector, unit_quat())) # approach_pose = multiply(pose.value, invert(grasp.approach)) for _ in range(max_attempts): if USE_IKFAST: attach_conf = sample_tool_ik(robot, attach_pose) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed attach_conf = inverse_kinematics(robot, tool_link, attach_pose) if (attach_conf is None) or collision_fn(attach_conf): continue set_joint_positions(robot, movable_joints, attach_conf) #if USE_IKFAST: # approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf) #else: approach_conf = inverse_kinematics(robot, tool_link, approach_pose) if (approach_conf is None) or collision_fn(approach_conf): continue set_joint_positions(robot, movable_joints, approach_conf) path = plan_direct_joint_motion( robot, movable_joints, attach_conf, obstacles=obstacles, self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions) if path is None: # TODO: retreat continue #path = [approach_conf, attach_conf] attachment = Attachment(robot, tool_link, grasp.attach, body) traj = MotionTrajectory(robot, movable_joints, path, attachments=[attachment]) yield approach_conf, traj break
def visualize_learned_pours(learner, bowl_type='blue_bowl', cup_type='red_cup', num_steps=None): learner.query_type = REJECTION # BEST | REJECTION | STRADDLE learner.validity_learner = None world = create_world() #add_obstacles() #environment = get_bodies() world.bodies[bowl_type] = load_cup_bowl(bowl_type) world.bodies[cup_type] = load_cup_bowl(cup_type) feature = get_pour_feature(world, bowl_type, cup_type) draw_pose(get_pose(world.get_body(bowl_type)), length=0.2) # TODO: sample different sizes print('Feature:', feature) for parameter in learner.parameter_generator(world, feature, valid=False): handles = [] print('Parameter:', parameter) valid = is_valid_pour(world, feature, parameter) score = learner.score(feature, parameter) x = learner.func.x_from_feature_parameter(feature, parameter) [prediction] = learner.predict_x(np.array([x]), noise=True) print('Valid: {} | Mean: {:.3f} | Var: {:.3f} | Score: {:.3f}'.format( valid, prediction['mean'], prediction['variance'], score)) #fraction = rescale(clip(prediction['mean'], *DEFAULT_INTERVAL), DEFAULT_INTERVAL, new_interval=(0, 1)) #color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) #set_color(world.get_body(cup_type), apply_alpha(color, alpha=0.25)) # TODO: overlay many cups at different poses bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter) #handles.extend(draw_pose(bowl_from_pivot)) handles.extend(draw_point(point_from_pose(bowl_from_pivot), color=BLACK)) # TODO: could label bowl, cup dimensions path, _ = pour_path_from_parameter(world, feature, parameter, cup_yaw=0) for p1, p2 in safe_zip(path[:-1], path[1:]): handles.append(add_line(point_from_pose(p1), point_from_pose(p2), color=RED)) if num_steps is not None: path = path[:num_steps] for i, pose in enumerate(path[::-1]): #step_handles = draw_pose(pose, length=0.1) #step_handles = draw_point(point_from_pose(pose), color=BLACK) set_pose(world.get_body(cup_type), pose) print('Step {}/{}'.format(i+1, len(path))) wait_for_user() #remove_handles(step_handles) remove_handles(handles)
def test_grasps(robot, block): for arm in ARMS: gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_LINK.format(arm)) tool_pose = get_link_pose(robot, tool_link) #handles = draw_pose(tool_pose) #grasps = get_top_grasps(block, under=True, tool_pose=unit_pose()) grasps = get_side_grasps(block, under=True, tool_pose=unit_pose()) for i, grasp_pose in enumerate(grasps): block_pose = multiply(tool_pose, grasp_pose) set_pose(block, block_pose) close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm), closed_conf=get_closed_positions(robot, arm)) handles = draw_pose(block_pose) wait_if_gui('Grasp {}'.format(i)) remove_handles(handles)