def custom_limits_from_base_limits(robot, base_limits, yaw_limit=None): x_limits, y_limits = zip(*base_limits) custom_limits = { joint_from_name(robot, 'x'): x_limits, joint_from_name(robot, 'y'): y_limits, } if yaw_limit is not None: custom_limits.update({ joint_from_name(robot, 'theta'): yaw_limit, }) return custom_limits
def open_surface_joints(world, surface_name, joint_names=ALL_JOINTS): surface = surface_from_name(surface_name) for joint_name in surface.joints: joint = joint_from_name(world.kitchen, joint_name) if joint_name in joint_names: # TODO: remove this mechanic in the future world.open_door(joint)
def parse_robot(robot_json): pose = parse_pose(robot_json) if robot_json['name'] == 'pr2': with HideOutput(True): robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True) set_group_conf(robot_id, 'base', base_values_from_pose(pose)) else: # TODO: set the z? #set_pose(robot_id, pose) raise NotImplementedError(robot_json['name']) for joint, values in robot_json['conf'].items(): [value] = values if has_joint(robot_id, joint): set_joint_position(robot_id, joint_from_name(robot_id, joint), value) else: print('Robot {} lacks joint {}'.format(robot_json['name'], joint)) if robot_json['name'] == 'pr2': set_group_conf(robot_id, 'torso', [0.2]) set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM) set_group_conf(robot_id, 'right_arm', rightarm_from_leftarm(REST_LEFT_ARM)) return robot_id
def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]): # TODO: combine this with test_arm_motion """ Drake's PR2 URDF has explicit base joints """ disabled_collisions = get_disabled_collisions(pr2) base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']] set_joint_positions(pr2, base_joints, base_start) base_joints = base_joints[:2] base_goal = base_goal[:len(base_joints)] wait_for_user('Plan Base?') base_path = plan_joint_motion(pr2, base_joints, base_goal, obstacles=obstacles, disabled_collisions=disabled_collisions) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_joint_positions(pr2, base_joints, bq) if SLEEP is None: wait_for_user('Continue?') else: time.sleep(SLEEP)
def get_surface_obstacles(world, surface_name): surface = surface_from_name(surface_name) obstacles = set() for joint_name in surface.joints: link = child_link_from_joint(joint_from_name(world.kitchen, joint_name)) obstacles.update(get_descendant_obstacles(world.kitchen, link)) # Be careful to call this before each check open_surface_joints(world, surface_name, joint_names=CABINET_JOINTS) return obstacles
def compute_door_paths(world, joint_name, door_conf1, door_conf2, obstacles=set(), teleport=False): door_paths = [] if door_conf1 == door_conf2: return door_paths door_joint = joint_from_name(world.kitchen, joint_name) door_joints = [door_joint] # TODO: could unify with grasp path door_extend_fn = get_extend_fn(world.kitchen, door_joints, resolutions=[DOOR_RESOLUTION]) door_path = [door_conf1.values] + list( door_extend_fn(door_conf1.values, door_conf2.values)) if teleport: door_path = [door_conf1.values, door_conf2.values] # TODO: open until collision for the drawers sign = world.get_door_sign(door_joint) pull = (sign * door_path[0][0] < sign * door_path[-1][0]) # door_obstacles = get_descendant_obstacles(world.kitchen, door_joint) for handle_grasp in get_handle_grasps(world, door_joint, pull=pull): link, grasp, pregrasp = handle_grasp handle_path = [] for door_conf in door_path: set_joint_positions(world.kitchen, door_joints, door_conf) # if any(pairwise_collision(door_obst, obst) # for door_obst, obst in product(door_obstacles, obstacles)): # return handle_path.append(get_link_pose(world.kitchen, link)) # Collide due to adjacency # TODO: check pregrasp path as well # TODO: check gripper self-collisions with the robot set_configuration(world.gripper, world.open_gq.values) tool_path = [ multiply(handle_pose, invert(grasp)) for handle_pose in handle_path ] for i, tool_pose in enumerate(tool_path): set_joint_positions(world.kitchen, door_joints, door_path[i]) set_tool_pose(world, tool_pose) # handles = draw_pose(handle_path[i], length=0.25) # handles.extend(draw_aabb(get_aabb(world.kitchen, link=link))) # wait_for_user() # for handle in handles: # remove_debug(handle) if any( pairwise_collision(world.gripper, obst) for obst in obstacles): break else: door_paths.append( DoorPath(door_path, handle_path, handle_grasp, tool_path)) return door_paths
def command_torso(self, pose, timeout=2.0, blocking=True): # Commands Torso to a certain position with ClientSaver(self.client): torso_joint = joint_from_name(self.robot, self.TORSO) set_joint_position(self.robot, torso_joint, pose) if self.execute_motor_control: control_joint(self.robot, torso_joint, pose) else: set_joint_position(self.robot, torso_joint, pose)
def get_ik_generator(robot, arm, world_pose, torso_limits=False, upper_limits=False): target_pose = multiply(invert(get_base_pose(robot)), world_pose) torso_limits = get_limits(robot, joint_from_name(robot, TORSO_JOINT), torso_limits) upper_limits = get_limits(robot, joint_from_name(robot, UPPER_JOINT[arm]), upper_limits) arm_joints = get_torso_arm_joints(robot, arm) while True: torso = random.uniform(*torso_limits) upper = random.uniform(*upper_limits) yield [ q for q in inverse_kinematics(arm, target_pose, torso, upper) if not violates_limits(robot, arm_joints, q) ]
def update_door_conf(self, name, position): joint = joint_from_name(self.world.kitchen, name) conf = FConf(self.world.kitchen, [joint], [position], init=True) if (name not in self.door_confs) or not are_confs_close( conf, self.door_confs[name], tol=1e-3): # TODO: different threshold for drawers and doors self.door_confs[name] = conf else: print('At anticipated conf for door {}'.format(name)) return self.door_confs[name]
def test_clone_arm(pr2): first_joint_name = PR2_GROUPS['left_arm'][0] first_joint = joint_from_name(pr2, first_joint_name) parent_joint = get_link_parent(pr2, first_joint) print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name, first_joint) print(get_link_descendants(pr2, first_joint)) # TODO: least common ancestor links = [first_joint] + get_link_descendants(pr2, first_joint) new_arm = clone_body(pr2, links=links, collision=False) dump_world() set_base_values(pr2, (-2, 0, 0)) wait_for_interrupt()
def gen(joint_name, door_conf1, door_conf2, *args): if door_conf1 == door_conf2: return door_joint = joint_from_name(world.kitchen, joint_name) door_paths = compute_door_paths(world, joint_name, door_conf1, door_conf2, obstacles, teleport=teleport) if not door_paths: return if learned: base_generator = cycle(load_pull_base_poses(world, joint_name)) else: _, _, _, tool_path = door_paths[0] index = int(len(tool_path) / 2) # index = 0 target_pose = tool_path[index] base_generator = uniform_pose_generator(world.robot, target_pose) safe_base_generator = inverse_reachability(world, base_generator, obstacles=obstacles, **kwargs) while True: for i in range(max_attempts): try: base_conf = next(safe_base_generator) except StopIteration: return if base_conf is None: yield None continue door_path = random.choice(door_paths) randomize = (random.random() < P_RANDOMIZE_IK) ik_outputs = next( plan_pull(world, door_joint, door_path, base_conf, randomize=randomize, collisions=collisions, teleport=teleport, **kwargs), None) if ik_outputs is not None: print('Pull succeeded after {} attempts'.format(i)) yield (base_conf, ) + ik_outputs break else: if PRINT_FAILURES: print('Pull failure') yield None
def get_ik_generator(robot, tool_pose, track_limits=False, prev_free_list=[]): from .ikfast_abb_irb6600_track import get_ik world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME)) base_from_tool = multiply(invert(world_from_base), tool_pose) base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot)) sampled_limits = get_ik_limits(robot, joint_from_name(robot, *TRACK_JOINT), track_limits) while True: if not prev_free_list: sampled_values = [random.uniform(*sampled_limits)] else: sampled_values = prev_free_list ik_joints = get_track_arm_joints(robot) confs = compute_inverse_kinematics(get_ik, base_from_ik, sampled_values) yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
def gen(joint_name, door_conf1, door_conf2, base_conf): #if door_conf1 == door_conf2: # return # TODO: check if within database convex hull door_joint = joint_from_name(world.kitchen, joint_name) obstacles = (world.static_obstacles | get_descendant_obstacles(world.kitchen, door_joint) ) # if collisions else set() base_conf.assign() world.carry_conf.assign() door_plans = [ door_plan for door_plan in compute_door_paths(world, joint_name, door_conf1, door_conf2, obstacles, teleport=teleport) if is_pull_safe(world, door_joint, door_plan) ] if not door_plans: print('Unable to open door {} at fixed config'.format(joint_name)) return max_failures = FIXED_FAILURES if world.task.movable_base else INF failures = 0 while failures <= max_failures: for i in range(max_attempts): door_path = random.choice(door_plans) # TracIK is itself stochastic randomize = (random.random() < P_RANDOMIZE_IK) ik_outputs = next( plan_pull(world, door_joint, door_path, base_conf, randomize=randomize, collisions=collisions, teleport=teleport, **kwargs), None) if ik_outputs is not None: print('Fixed pull succeeded after {} attempts'.format(i)) yield ik_outputs break # return else: if PRINT_FAILURES: print('Fixed pull failure') yield None failures += 1
def test_drake_base_motion(pr2, base_start, base_goal): # TODO: combine this with test_arm_motion """ Drake's PR2 URDF has explicit base joints """ disabled_collisions = get_disabled_collisions(pr2) base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']] set_joint_positions(pr2, base_joints, base_start) user_input('Plan Base?') base_path = plan_joint_motion(pr2, base_joints, base_goal, disabled_collisions=disabled_collisions) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_joint_positions(pr2, base_joints, bq) user_input('Continue?')
def _load_robot(self, real_world): with ClientSaver(self.client): # TODO: set the x,y,theta joints using the base pose pose = get_pose(self.robot) # base_link is origin base_pose = get_link_pose(self.robot, link_from_name(self.robot, BASE_FRAME)) set_pose(self.robot, multiply(invert(base_pose), pose)) # base_pose = real_world.controller.return_cartesian_pose(arm='l') # Because the robot might have an xyz movable_names = real_world.controller.get_joint_names() movable_joints = [ joint_from_name(self.robot, name) for name in movable_names ] movable_positions = real_world.controller.get_joint_positions( movable_names) set_joint_positions(self.robot, movable_joints, movable_positions) self.initial_config = get_configuration(self.robot) self.body_mapping[self.robot] = real_world.robot
def solve_inverse_kinematics(robot, arm, world_pose, obstacles=[], collision_buffer=COLLISION_BUFFER, max_attempts=25): arm_joints = get_arm_joints(robot, arm) custom_limits = get_pr2_safety_limits(robot) # TODO: self-collisions collision_fn = get_collision_fn(robot, arm_joints, obstacles=obstacles, attachments=[], max_distance=collision_buffer, self_collisions=False, disabled_collisions=set(), custom_limits=custom_limits) base_pose = get_link_pose(robot, link_from_name(robot, BASE_FRAME)) # != get_pose(robot) target_point, target_quat = multiply(invert(base_pose), world_pose) [torso_position] = get_group_conf(robot, 'torso') upper_joint = joint_from_name(robot, UPPER_JOINTS[arm]) lower, upper = get_custom_limits(robot, [upper_joint], custom_limits, circular_limits=CIRCULAR_LIMITS) upper_limits = list(zip(lower, upper))[0] # TODO(caelan): just attempt one IK sample for each generator to restart quickly for i, arm_conf in enumerate( islice( get_arm_ik_generator(get_arm_prefix(arm), list(target_point), list(target_quat), torso_position, upper_limits=upper_limits), max_attempts)): if not collision_fn(arm_conf): #print('Attempts:', i) return Conf(arm_conf) return None
def collect_pull(world, joint_name, args): date = get_date() #set_seed(args.seed) robot_name = get_body_name(world.robot) if is_press(joint_name): press_gen = get_press_gen_fn(world, collisions=not args.cfree, teleport=args.teleport, learned=False) else: joint = joint_from_name(world.kitchen, joint_name) open_conf = Conf(world.kitchen, [joint], [world.open_conf(joint)]) closed_conf = Conf(world.kitchen, [joint], [world.closed_conf(joint)]) pull_gen = get_pull_gen_fn(world, collisions=not args.cfree, teleport=args.teleport, learned=False) #handle_link, handle_grasp, _ = get_handle_grasp(world, joint) path = get_pull_path(robot_name, joint_name) print(SEPARATOR) print('Robot name {} | Joint name: {} | Filename: {}'.format( robot_name, joint_name, path)) entries = [] failures = 0 start_time = time.time() while (len(entries) < args.num_samples) and \ (elapsed_time(start_time) < args.max_time): if is_press(joint_name): result = next(press_gen(joint_name), None) else: result = next(pull_gen(joint_name, open_conf, closed_conf), None) # Open to closed if result is None: print('Failure! | {} / {} [{:.3f}]'.format( len(entries), args.num_samples, elapsed_time(start_time))) failures += 1 continue if not is_press(joint_name): open_conf.assign() joint_pose = get_joint_reference_pose(world.kitchen, joint_name) bq, aq1 = result[:2] bq.assign() aq1.assign() #next(at.commands[2].iterate(None, None)) base_pose = get_link_pose(world.robot, world.base_link) #handle_pose = get_link_pose(world.robot, base_link) entries.append({ 'joint_from_base': multiply(invert(joint_pose), base_pose), }) print('Success! | {} / {} [{:.3f}]'.format(len(entries), args.num_samples, elapsed_time(start_time))) if has_gui(): wait_for_user() if not entries: safe_remove(path) return None #visualize_database(joint_from_base_list) # Assuming the kitchen is fixed but the objects might be open world # TODO: could store per data point data = { 'date': date, 'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name 'base_link': get_link_name(world.robot, world.base_link), 'tool_link': get_link_name(world.robot, world.tool_link), 'kitchen_name': get_body_name(world.kitchen), 'joint_name': joint_name, 'entries': entries, 'failures': failures, 'successes': len(entries), } if not is_press(joint_name): data.update({ 'open_conf': open_conf.values, 'closed_conf': closed_conf.values, }) write_json(path, data) print('Saved', path) return data
def get_pr2_safety_limits(pr2): return { joint_from_name(pr2, name): limits for name, limits in PR2_JOINT_SAFETY_LIMITS.items() }
def pdddlstream_from_problem(belief, additional_init=[], fixed_base=True, **kwargs): world = belief.world # One world per state task = world.task # One task per world print(task) domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl')) # TODO: repackage stream outputs to avoid recomputation # Despite the base not moving, it could be re-estimated init_bq = belief.base_conf init_aq = belief.arm_conf init_gq = belief.gripper_conf carry_aq = world.carry_conf init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq # TODO: the following doesn't work. Maybe because carry_conf is used elsewhere #carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf #calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf # Don't need this now that returning to old confs #open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq #closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq open_gq = world.open_gq closed_gq = world.closed_gq constant_map = { '@world': 'world', '@gripper': 'gripper', '@stove': 'stove', '@none': None, '@rest_aq': carry_aq, #'@calibrate_aq': calibrate_aq, '@open_gq': open_gq, '@closed_gq': closed_gq, '@open': OPEN, '@closed': CLOSED, '@top': TOP_GRASP, '@side': SIDE_GRASP, '@bq0': init_bq, } top_joint = JOINT_TEMPLATE.format(TOP_DRAWER) bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER) init = [ ('BConf', init_bq), ('AtBConf', init_bq), ('AConf', init_bq, carry_aq), #('RestAConf', carry_aq), #('AConf', init_bq, calibrate_aq), ( 'Stationary', ), ('AConf', init_bq, init_aq), ('AtAConf', init_aq), ('GConf', open_gq), ('GConf', closed_gq), ('Grasp', None, None), ('AtGrasp', None, None), ('Above', top_joint, bottom_joint), ('Adjacent', top_joint, bottom_joint), ('Adjacent', bottom_joint, top_joint), ('Calibrated', ), ('CanMoveBase', ), ('CanMoveArm', ), ('CanMoveGripper', ), ] + list(task.init) + list(additional_init) for action_name, cost in ACTION_COSTS.items(): function_name = '{}Cost'.format(title_from_snake(action_name)) function = (function_name, ) init.append(Equal(function, cost)) # TODO: stove state init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \ [('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \ [('Pressed', name) for name in belief.pressed] + \ [('Cookable', name) for name in task.goal_cooked] + \ [('Cooked', name) for name in belief.cooked] + \ [('Status', status) for status in DOOR_STATUSES] + \ [('Knob', knob) for knob in KNOBS] + \ [('Joint', knob) for knob in KNOBS] + \ [('Liquid', liquid) for _, liquid in task.init_liquid] + \ [('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \ [('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \ [('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object #[('Type', obj_name, 'stove') for obj_name in STOVES] + \ #[('Camera', name) for name in world.cameras] if task.movable_base: init.append(('MovableBase', )) if fixed_base: init.append(('InitBConf', init_bq)) if task.noisy_base: init.append(('NoisyBase', )) compute_pose_kin = get_compute_pose_kin(world) compute_angle_kin = get_compute_angle_kin(world) initial_poses = {} for joint_name, init_conf in belief.door_confs.items(): if joint_name in DRAWER_JOINTS: init.append(('Drawer', joint_name)) if joint_name in CABINET_JOINTS: init.append(('Cabinet', joint_name)) joint = joint_from_name(world.kitchen, joint_name) surface_name = surface_from_joint(joint_name) init.append(('SurfaceJoint', surface_name, joint_name)) # Relies on the fact that drawers have identical surface and link names link_name = get_link_name(world.kitchen, child_link_from_joint(joint)) #link_name = str(link_name.decode('UTF-8')) #link_name = str(link_name.encode('ascii','ignore')) for conf in { init_conf, world.open_kitchen_confs[joint], world.closed_kitchen_confs[joint] }: # TODO: return to initial poses? world_pose, = compute_angle_kin(link_name, joint_name, conf) init.extend([ ('Joint', joint_name), ('Angle', joint_name, conf), ('Obstacle', link_name), ('AngleKin', link_name, world_pose, joint_name, conf), ('WorldPose', link_name, world_pose), ]) if joint in world.kitchen_joints: init.extend([ ('Sample', world_pose), #('Value', world_pose), # comment out? ]) if conf == init_conf: initial_poses[link_name] = world_pose init.extend([ ('AtAngle', joint_name, conf), ('AtWorldPose', link_name, world_pose), ]) for surface_name in ALL_SURFACES: if surface_name in OPEN_SURFACES: init.append(('Counter', surface_name)) # Fixed surface if surface_name in DRAWERS: init.append(('Drawer', surface_name)) surface = surface_from_name(surface_name) surface_link = link_from_name(world.kitchen, surface.link) parent_joint = parent_joint_from_link(surface_link) if parent_joint not in world.kitchen_joints: # TODO: attach to world frame? world_pose = RelPose(world.kitchen, surface_link, init=True) initial_poses[surface_name] = world_pose init += [ #('RelPose', surface_name, world_pose, 'world'), ('WorldPose', surface_name, world_pose), #('AtRelPose', surface_name, world_pose, 'world'), ('AtWorldPose', surface_name, world_pose), ('Sample', world_pose), #('Value', world_pose), ] init.extend([ ('CheckNearby', surface_name), #('InitPose', world_pose), ('Localized', surface_name), ]) for grasp_type in task.grasp_types: if (surface_name in OPEN_SURFACES) or has_place_database( world.robot_name, surface_name, grasp_type): init.append(('AdmitsGraspType', surface_name, grasp_type)) if belief.grasped is None: init.extend([ ('HandEmpty', ), ('GConf', init_gq), ('AtGConf', init_gq), ]) else: obj_name = belief.grasped.body_name assert obj_name not in belief.pose_dists grasp = belief.grasped init += [ # Static #('Graspable', obj_name), ('Grasp', obj_name, grasp), ('IsGraspType', obj_name, grasp, grasp.grasp_type), # Fluent ('AtGrasp', obj_name, grasp), ('Holding', obj_name), ('Localized', obj_name), ] init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) for obj_name in world.movable: obj_type = type_from_name(obj_name) if obj_type in BOWLS: init.append(('Bowl', obj_name)) else: init.append( ('Obstacle', obj_name)) # TODO: hack to place within bowls if obj_type in COOKABLE: init.append(('Cookable', obj_name)) if obj_type in POURABLE: init.append(('Pourable', obj_name)) init += [ ('Entity', obj_name), ('CheckNearby', obj_name), ] + [('Stackable', obj_name, counter) for counter in set(ALL_SURFACES) & set(COUNTERS)] # TODO: track poses over time to produce estimates for obj_name, pose_dist in belief.pose_dists.items(): dist_support = pose_dist.dist.support() localized = pose_dist.is_localized() graspable = True if localized: init.append(('Localized', obj_name)) [rel_pose] = dist_support roll, pitch, yaw = euler_from_quat( quat_from_pose(rel_pose.get_reference_from_body())) if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)): graspable = False print( '{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'. format(obj_name, roll, pitch)) if graspable: #init.append(('Graspable', obj_name)) init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) # Could also fully decompose into points (but many samples) # Could immediately add likely points for collision checking for rel_pose in (dist_support if localized else pose_dist.decompose()): surface_name = rel_pose.support if surface_name is None: # Treats as obstacle # TODO: could temporarily add to fixed world_pose = rel_pose init += [ ('WorldPose', obj_name, world_pose), ('AtWorldPose', obj_name, world_pose), ] poses = [world_pose] #raise RuntimeError(obj_name, supporting) else: surface_pose = initial_poses[surface_name] world_pose, = compute_pose_kin(obj_name, rel_pose, surface_name, surface_pose) init += [ # Static ('RelPose', obj_name, rel_pose, surface_name), ('WorldPose', obj_name, world_pose), ('PoseKin', obj_name, world_pose, rel_pose, surface_name, surface_pose), # Fluent ('AtRelPose', obj_name, rel_pose, surface_name), ('AtWorldPose', obj_name, world_pose), ] if localized: init.append(('On', obj_name, surface_name)) poses = [rel_pose, world_pose] for pose in poses: if isinstance(pose, PoseDist): init.append(('Dist', pose)) else: init.extend([('Sample', pose)]) #, ('Value', pose)]) #for body, ty in problem.body_types: # init += [('Type', body, ty)] #bodies_from_type = get_bodies_from_type(problem) #bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty] goal_formula = get_goal(belief, init) stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base, **kwargs) print('Constants:', constant_map) print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal_formula) #print('Streams:', stream_map.keys()) # DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def get_custom_limits(robot, named_limits=CUSTOM_LIMITS): return { joint_from_name(robot, joint): limits for joint, limits in named_limits.items() }
#Setup sim physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) pr2_start_orientation = p.getQuaternionFromEuler([0,0,0]) pr2_start_pose = [-.80*k,0,0] pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation, useFixedBase=True, globalScaling = 1 ) #The goal here is to get the arm positions of the actual robot using the uber controller #and simulate them in pybullet print str(uc.get_head_pose()) + ',' #Left Manipulator left_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['left']] l_joint_poses = uc.get_joint_positions('l') #Right Manipulator right_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['right']] r_joint_poses = uc.get_joint_positions('r') #Torso torso_joint = 15 torso_pose = np.array([uc.get_torso_pose()]) #Head #TODO raw_input('Start?') print print #Aligns the pybullet sim with the robot's state
def main(): # TODO: teleporting kuka arm parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() client = connect(use_gui=args.viewer) add_data_path() #planeId = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) #pr2 = p.loadURDF("pr2_description/pr2.urdf") pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf") #pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",) #useFixedBase=0,) #flags=p.URDF_USE_SELF_COLLISION) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False) initially_colliding = get_colliding_links(pr2) print len(initially_colliding) origin = (0, 0, 0) print p.getNumConstraints() # TODO: no way of controlling the base position by itself # TODO: PR2 seems to collide with itself frequently # real_time = False # p.setRealTimeSimulation(real_time) # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # while True: # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # if not real_time: # p.stepSimulation() # A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints. # GetRigidlyAttachedLinks print pr2 # for i in range (10000): # p.stepSimulation() # time.sleep(1./240.) #print get_joint_names(pr2) print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)] print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER)) print self_collision(pr2) """ print p.getNumConstraints() constraint = fixed_constraint(pr2, -1, box, -1) # table p.changeConstraint(constraint) print p.getNumConstraints() print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) p.stepSimulation() raw_input('Continue?') set_point(pr2, (-2, 0, 0)) p.stepSimulation() p.changeConstraint(constraint) print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) raw_input('Continue?') print get_point(pr2) raw_input('Continue?') """ # TODO: would be good if we could set the joint directly print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #return left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES] print set_joint_positions( pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM print set_joint_positions( pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM print get_body_name(pr2) print get_body_names() # print p.getBodyUniqueId(pr2) print get_joint_names(pr2) #for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM): # set_joint_position(pr2, joint, value) # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # #print name, joint, get_joint_position(pr2, joint), value # print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint) # set_joint_position(pr2, joint, value) # #print name, joint, get_joint_position(pr2, joint), value # for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM): # set_joint_position(pr2, joint_from_name(pr2, name), value) print p.getNumJoints(pr2) jointId = 0 print p.getJointInfo(pr2, jointId) print p.getJointState(pr2, jointId) # for i in xrange(10): # #lower, upper = BASE_LIMITS # #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower # q = np.random.uniform(*BASE_LIMITS) # theta = np.random.uniform(*REVOLUTE_LIMITS) # quat = z_rotation(theta) # print q, theta, quat, env_collision(pr2) # #set_point(pr2, q) # set_pose(pr2, q, quat) # #p.getMouseEvents() # #p.getKeyboardEvents() # raw_input('Continue?') # Stalls because waiting for input # # # TODO: self collisions # for i in xrange(10): # for name in LEFT_JOINT_NAMES: # joint = joint_from_name(pr2, name) # value = np.random.uniform(*get_joint_limits(pr2, joint)) # set_joint_position(pr2, joint, value) # raw_input('Continue?') #start = (-2, -2, 0) #set_base_values(pr2, start) # #start = get_base_values(pr2) # goal = (2, 2, 0) # p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText # print start, goal # raw_input('Plan?') # path = plan_base_motion(pr2, goal) # print path # if path is None: # return # print len(path) # for bq in path: # set_base_values(pr2, bq) # raw_input('Continue?') # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # for joint in left_joints: # print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \ # is_circular(pr2, joint), get_joint_position(pr2, joint) # # #goal = np.zeros(len(left_joints)) # goal = [] # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # goal.append(wrap_joint(pr2, joint, value)) # # path = plan_joint_motion(pr2, left_joints, goal) # print path # for q in path:s # set_joint_positions(pr2, left_joints, q) # raw_input('Continue?') print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6 movable_joints = get_movable_joints(pr2) print len(movable_joints) for joint in xrange(get_num_joints(pr2)): if is_movable(pr2, joint): print joint, get_joint_name(pr2, joint), get_joint_type( pr2, joint), get_joint_limits(pr2, joint) #joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] #set_joint_positions(pr2, joints, sample_joints(pr2, joints)) #print get_joint_positions(pr2, joints) # Need to print before the display updates? # set_base_values(pr2, (1, -1, -np.pi/4)) # movable_joints = get_movable_joints(pr2) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # print gripper_pose # print get_joint_positions(pr2, movable_joints) # p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0)) # p.stepSimulation() # raw_input('Pre2 IK') # set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM # print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Pre IK') # conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons # print conf # print get_joint_positions(pr2, movable_joints) # set_joint_positions(pr2, movable_joints, conf) # print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Post IK') # return # print pose_from_tform(TOOL_TFORM) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #gripper_pose = multiply(gripper_pose, TOOL_POSE) # #gripper_pose = get_gripper_pose(pr2) # for i, grasp_pose in enumerate(get_top_grasps(box)): # grasp_pose = multiply(TOOL_POSE, grasp_pose) # box_pose = multiply(gripper_pose, grasp_pose) # set_pose(box, *box_pose) # print get_pose(box) # raw_input('Grasp {}'.format(i)) # return torso = joint_from_name(pr2, TORSO_JOINT_NAME) torso_point, torso_quat = get_link_pose(pr2, torso) #torso_constraint = p.createConstraint(pr2, torso, -1, -1, # p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED # parentFramePosition=torso_point, # childFramePosition=torso_quat) create_inverse_reachability(pr2, box, table) ir_database = load_inverse_reachability() print len(ir_database) return link = link_from_name(pr2, LEFT_ARM_LINK) point, quat = get_link_pose(pr2, link) print point, quat p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText raw_input('Continue?') current_conf = get_joint_positions(pr2, movable_joints) #ik_conf = p.calculateInverseKinematics(pr2, link, point) #ik_conf = p.calculateInverseKinematics(pr2, link, point, quat) min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints] max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints] max_velocities = [ get_max_velocity(pr2, joint) for joint in movable_joints ] # Range of Jacobian print min_limits print max_limits print max_velocities ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf) value_from_joint = dict(zip(movable_joints, ik_conf)) print[value_from_joint[joint] for joint in joints] #print len(ik_conf), ik_conf set_joint_positions(pr2, movable_joints, ik_conf) #print len(movable_joints), get_joint_positions(pr2, movable_joints) print get_joint_positions(pr2, joints) raw_input('Finish?') p.disconnect()
def create_custom_base_limits(robot, base_limits): return {joint_from_name(robot, joint): limits for joint, limits in safe_zip(BASE_JOINTS[:2], zip(*base_limits))}
def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(get_tool_pose(robot, ARM), length=0.04) draw_pose(approach_pose, length=0.04) draw_pose(gripper_pose, length=0.04) # print(movable_joints) # print(torso_arm) # wait_for_interrupt() # TODO: gantry_x_joint # TODO: proper inverse reachability base_link = link_from_name(robot, IK_BASE_FRAMES[ARM]) base_pose = get_link_pose(robot, base_link) body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose)) y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM))) initial_y = get_joint_position(robot, y_joint) ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1] set_joint_position(robot, y_joint, ik_y) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, ARM, approach_pose) if q_approach is not None: set_joint_positions(robot, torso_arm, q_approach) else: set_joint_positions(robot, torso_arm, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for approaching fails!') continue conf = BodyConf(robot, joints=arm_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, torso_arm, q_grasp) else: conf.assign() q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for grasp fails!') continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles, self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: print('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=torso_arm), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None