def fn(robot_name, obj_name, pose, grasp): # TODO: if gripper/block in collision, return robot = task.mbp.GetModelInstanceByName(robot_name) joints = get_movable_joints(task.mbp, robot) collision_pairs = set(product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed)) collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph, joints, collision_pairs=collision_pairs) # TODO: while holding grasp_pose = pose.transform.multiply(grasp.transform.inverse()) gripper_path = list(interpolate_translation(grasp_pose, approach_vector)) attempts = 0 last_success = 0 while (attempts - last_success) < max_failures: attempts += 1 waypoints = plan_workspace_motion(task.mbp, joints, gripper_frame, gripper_path, initial_guess=initial_guess, collision_fn=collision_fn) if waypoints is None: continue path = plan_waypoints_joint_motion(joints, waypoints, collision_fn=collision_fn) if path is None: continue #path = refine_joint_path(joints, path) traj = Trajectory(Conf(joints, q) for q in path) #print(attempts - last_success) last_success = attempts return traj.path[-1], traj
def load_station(time_step=0.0): # https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc object_file_path = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") #object_file_path = FOAM_BRICK_PATH station = ManipulationStation(time_step) station.AddCupboard() mbp = station.get_mutable_multibody_plant() scene_graph = station.get_mutable_scene_graph() object = AddModelFromSdfFile( file_name=object_file_path, model_name="object", plant=mbp, scene_graph=scene_graph) station.Finalize() robot = mbp.GetModelInstanceByName('iiwa') gripper = mbp.GetModelInstanceByName('gripper') initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions = dict(zip(get_movable_joints(mbp, robot), initial_conf)) initial_poses = { object: create_transform(translation=[.6, 0, 0]), } task = Task(mbp, scene_graph, robot, gripper, movable=[object], surfaces=[], initial_positions=initial_positions, initial_poses=initial_poses, goal_on=[]) return mbp, scene_graph, task
def get_open_trajectory(plant, gripper): gripper_joints = get_movable_joints(plant, gripper) gripper_extend_fn = get_extend_fn(gripper_joints) gripper_closed_conf = get_close_wsg50_positions(plant, gripper) gripper_path = list( gripper_extend_fn(gripper_closed_conf, get_open_wsg50_positions(plant, gripper))) gripper_path.insert(0, gripper_closed_conf) return Trajectory(Conf(gripper_joints, q) for q in gripper_path)
def fn(robot_name, door_name, dq1, dq2): robot = task.mbp.GetModelInstanceByName(robot_name) robot_joints = get_movable_joints(task.mbp, robot) collision_pairs = set(product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed)) collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph, robot_joints, collision_pairs=collision_pairs) door_body = task.mbp.GetBodyByName(door_name) door_joints = dq1.joints extend_fn = get_extend_fn(door_joints, resolutions=step_size*np.ones(len(door_joints))) door_joint_path = [dq1.positions] + list(extend_fn(dq1.positions, dq2.positions)) # TODO: check for collisions door_cartesian_path = [] for robot_conf in door_joint_path: set_joint_positions(door_joints, context, robot_conf) door_cartesian_path.append(get_body_pose(context, door_body)) shape, index = 'cylinder', 1 # Second grasp is np.pi/2, corresponding to +y #shape, index = 'box', 0 # left_door TODO: right_door for i in range(2): handle_aabb, handle_from_box, handle_shape = box_from_geom[int(door_body.model_instance()), door_name, i] if handle_shape == shape: break else: raise RuntimeError(shape) [gripper_from_box] = list(get_box_grasps(handle_aabb, orientations=[index], pitch_range=(pitch, pitch), grasp_length=grasp_length)) gripper_from_obj = gripper_from_box.multiply(handle_from_box.inverse()) pull_cartesian_path = [body_pose.multiply(gripper_from_obj.inverse()) for body_pose in door_cartesian_path] #start_path = list(interpolate_translation(pull_cartesian_path[0], approach_vector)) #end_path = list(interpolate_translation(pull_cartesian_path[-1], approach_vector)) for _ in range(max_attempts): pull_joint_waypoints = plan_workspace_motion(task.mbp, robot_joints, gripper_frame, reversed(pull_cartesian_path), collision_fn=collision_fn) if pull_joint_waypoints is None: continue pull_joint_waypoints = pull_joint_waypoints[::-1] rq1 = Conf(robot_joints, pull_joint_waypoints[0]) rq2 = Conf(robot_joints, pull_joint_waypoints[-1]) combined_joints = robot_joints + door_joints combined_waypoints = [list(rq) + list(dq) for rq, dq in zip(pull_joint_waypoints, door_joint_path)] pull_joint_path = plan_waypoints_joint_motion(combined_joints, combined_waypoints, collision_fn=lambda q: False) if pull_joint_path is None: continue traj = Trajectory(Conf(combined_joints, combined_conf) for combined_conf in pull_joint_path) return rq1, rq2, traj
def fn(robot_name, conf1, conf2, fluents=[]): robot = task.mbp.GetModelInstanceByName(robot_name) joints = get_movable_joints(task.mbp, robot) moving = bodies_from_models(task.mbp, [robot, gripper]) obstacles = set(task.fixed_bodies()) attachments = [] for fact in fluents: predicate = fact[0] if predicate == 'atconf': name, conf = fact[1:] conf.assign(context) obstacles.update(conf.bodies) elif predicate == 'atpose': name, pose = fact[1:] pose.assign(context) obstacles.update(pose.bodies) elif predicate == 'atgrasp': robot, name, grasp = fact[1:] attachments.append(grasp) moving.update(grasp.bodies) else: raise ValueError(predicate) obstacles -= moving #print(sorted(body.name() for body in moving)) #print(sorted(body.name() for body in obstacles)) #print(attachments) # Can make separate predicate for the frame something is in at a particular time collision_pairs = set(product(moving, obstacles)) if collisions else set() collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph, joints, collision_pairs=collision_pairs, attachments=attachments) open_wsg50_gripper(task.mbp, context, gripper) set_joint_positions(joints, context, conf1.positions) path = plan_joint_motion(joints, conf1.positions, conf2.positions, collision_fn=collision_fn, restarts=7, iterations=75, smooth=100) if path is None: return None #path = refine_joint_path(joints, path) traj = Trajectory(Conf(joints, q) for q in path) return traj,
def fn(robot_name, conf1, conf2, fluents=[]): robot = task.mbp.GetModelInstanceByName(robot_name) joints = get_movable_joints(task.mbp, robot) if teleport: traj = Trajectory([conf1, conf2]) return (traj, ) moving = bodies_from_models(task.mbp, [robot, gripper]) obstacles = set(task.fixed_bodies()) attachments = parse_fluents(fluents, context, obstacles) for grasp in attachments: moving.update(grasp.bodies) obstacles -= moving collision_pairs = set(product(moving, obstacles)) if collisions else set() collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph, joints, collision_pairs=collision_pairs, attachments=attachments) #weights = np.ones(len(joints)) #weights = np.array([sum(weights[i:]) for i in range(len(weights))]) distance_fn = None # TODO: adjust the resolutions open_wsg50_gripper(task.mbp, context, gripper) path = plan_joint_motion(joints, conf1.positions, conf2.positions, distance_fn=distance_fn, collision_fn=collision_fn, restarts=10, iterations=75, smooth=50) if path is None: return None traj = Trajectory([Conf(joints, q) for q in path], attachments=attachments) return (traj, )
def fn(robot_name, obj_name, obj_pose, obj_grasp): # TODO: if gripper/block in collision, return robot = task.mbp.GetModelInstanceByName(robot_name) joints = get_movable_joints(task.mbp, robot) collision_pairs = set( product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed)) collision_fn = get_collision_fn( task.diagram, task.diagram_context, task.mbp, task.scene_graph, joints, collision_pairs=collision_pairs) # TODO: while holding gripper_pose = obj_pose.transform.multiply( obj_grasp.transform.inverse()) gripper_path = list( interpolate_translation(gripper_pose, approach_vector, step_size=step_size)) attempts = 0 last_success = 0 while (attempts - last_success) < max_failures: attempts += 1 obj_pose.assign(context) path = plan_frame_motion(task.plant, joints, gripper_frame, gripper_path, initial_guess=initial_guess, collision_fn=collision_fn) if path is None: continue print('IK attempts: {}'.format(attempts)) traj = Trajectory([Conf(joints, q) for q in path], attachments=[obj_grasp]) conf = traj.path[-1] yield (conf, traj) last_success = attempts
def get_pddlstream_problem(task, context, collisions=True): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} plant = task.mbp robot = task.robot robot_name = get_model_name(plant, robot) world = plant.world_frame() # mbp.world_body() robot_joints = get_movable_joints(plant, robot) robot_conf = Conf(robot_joints, get_configuration(plant, context, robot)) init = [ ('Robot', robot_name), ('CanMove', robot_name), ('Conf', robot_name, robot_conf), ('AtConf', robot_name, robot_conf), ('HandEmpty', robot_name), ] goal_literals = [] if task.reset_robot: goal_literals.append(('AtConf', robot_name, robot_conf), ) for obj in task.movable: obj_name = get_model_name(plant, obj) #obj_frame = get_base_body(mbp, obj).body_frame() obj_pose = Pose(plant, world, obj, get_world_pose(plant, context, obj)) # get_relative_transform init += [ ('Graspable', obj_name), ('Pose', obj_name, obj_pose), #('InitPose', obj_name, obj_pose), ('AtPose', obj_name, obj_pose) ] for surface in task.surfaces: init += [('Stackable', obj_name, surface)] # TODO: detect already stacked for surface in task.surfaces: surface_name = get_model_name(plant, surface.model_index) if 'sink' in surface_name: init += [('Sink', surface)] if 'stove' in surface_name: init += [('Stove', surface)] for door in task.doors: door_body = plant.tree().get_body(door) door_name = door_body.name() door_joints = get_parent_joints(plant, door_body) door_conf = Conf(door_joints, get_joint_positions(door_joints, context)) init += [ ('Door', door_name), ('Conf', door_name, door_conf), ('AtConf', door_name, door_conf), ] for positions in [get_door_positions(door_body, DOOR_OPEN)]: conf = Conf(door_joints, positions) init += [('Conf', door_name, conf)] if task.reset_doors: goal_literals += [('AtConf', door_name, door_conf)] for obj, transform in task.goal_poses.items(): obj_name = get_model_name(plant, obj) obj_pose = Pose(plant, world, obj, transform) init += [('Pose', obj_name, obj_pose)] goal_literals.append(('AtPose', obj_name, obj_pose)) for obj in task.goal_holding: goal_literals.append( ('Holding', robot_name, get_model_name(plant, obj))) for obj, surface in task.goal_on: goal_literals.append(('On', get_model_name(plant, obj), surface)) for obj in task.goal_cooked: goal_literals.append(('Cooked', get_model_name(plant, obj))) goal = And(*goal_literals) print('Initial:', init) print('Goal:', goal) stream_map = { #'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)), 'sample-reachable-pose': from_gen_fn( get_reachable_pose_gen_fn(task, context, collisions=collisions)), 'sample-grasp': from_gen_fn(get_grasp_gen_fn(task)), 'plan-ik': from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)), 'plan-motion': from_fn(get_motion_fn(task, context, collisions=collisions)), 'plan-pull': from_fn(get_pull_fn(task, context, collisions=collisions)), 'TrajPoseCollision': get_collision_test(task, context, collisions=collisions), 'TrajConfCollision': get_collision_test(task, context, collisions=collisions), } #stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def load_manipulation(time_step=0.0): source = True if source: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") #IIWA7_PATH = FindResourceOrThrow( # "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf") CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf") #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") goal_shelf = 'bottom' mbp = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa', scene_graph=scene_graph, plant=mbp) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=mbp) # TODO: sdf frame/link error amazon_table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='amazon_table', scene_graph=scene_graph, plant=mbp) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=mbp) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=mbp) # left_door, left_door_hinge, cylinder weld_gripper(mbp, robot, gripper) weld_to_world(mbp, robot, create_transform()) weld_to_world(mbp, amazon_table, create_transform( translation=[dx_table_center_to_robot_base, 0, -dz_table_top_robot_base])) weld_to_world(mbp, cupboard, create_transform( translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) mbp.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper' 'top', ] goal_surface = VisualElement(cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #VisualElement(amazon_table, 'amazon_table', 0), goal_surface, ] door_names = [ 'left_door', 'right_door', ] doors = [mbp.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/8 initial_positions = { mbp.GetJointByName('left_door_hinge'): -door_position, #mbp.GetJointByName('right_door_hinge'): door_position, mbp.GetJointByName('right_door_hinge'): np.pi, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update(zip(get_movable_joints(mbp, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.4, 0, 0]), } goal_on = [ (brick, goal_surface), ] task = Task(mbp, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_on=goal_on) return mbp, scene_graph, task
def load_manipulation(time_step=0.0, use_meshcat=True, use_controllers=True, use_external=False): if not use_external: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/cupboard.sdf") IIWA7_PATH = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") goal_shelf = 'shelf_lower' #goal_shelf = 'shelf_upper' else: AMAZON_TABLE_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/amazon_table_simplified.sdf" ) CUPBOARD_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/manipulation_station/cupboard.sdf" ) IIWA7_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/iiwa7/iiwa7_no_collision.sdf" ) FOAM_BRICK_PATH = FindResourceOrThrow( "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf" ) goal_shelf = 'bottom' #IIWA7_PATH = os.path.join(MODELS_DIR, "iiwa_description/iiwa7/iiwa7_with_box_collision.sdf") plant = MultibodyPlant(time_step=time_step) scene_graph = SceneGraph() dx_table_center_to_robot_base = 0.3257 dz_table_top_robot_base = 0.0127 dx_cupboard_to_table_center = 0.43 + 0.15 dz_cupboard_to_table_center = 0.02 cupboard_height = 0.815 cupboard_x = dx_table_center_to_robot_base + dx_cupboard_to_table_center cupboard_z = dz_cupboard_to_table_center + cupboard_height / 2.0 - dz_table_top_robot_base robot = AddModelFromSdfFile(file_name=IIWA7_PATH, model_name='iiwa', scene_graph=scene_graph, plant=plant) gripper = AddModelFromSdfFile(file_name=WSG50_SDF_PATH, model_name='gripper', scene_graph=scene_graph, plant=plant) # TODO: sdf frame/link error table = AddModelFromSdfFile(file_name=AMAZON_TABLE_PATH, model_name='table', scene_graph=scene_graph, plant=plant) cupboard = AddModelFromSdfFile(file_name=CUPBOARD_PATH, model_name='cupboard', scene_graph=scene_graph, plant=plant) brick = AddModelFromSdfFile(file_name=FOAM_BRICK_PATH, model_name='brick', scene_graph=scene_graph, plant=plant) weld_gripper(plant, robot, gripper) weld_to_world(plant, robot, create_transform()) weld_to_world( plant, table, create_transform(translation=[ dx_table_center_to_robot_base, 0, -dz_table_top_robot_base ])) weld_to_world( plant, cupboard, create_transform(translation=[cupboard_x, 0, cupboard_z], rotation=[0, 0, np.pi])) plant.Finalize(scene_graph) shelves = [ 'bottom', 'shelf_lower', 'shelf_upper', 'top', ] goal_surface = Surface(plant, cupboard, 'top_and_bottom', shelves.index(goal_shelf)) surfaces = [ #Surface(plant, table, 'amazon_table', 0), #goal_surface, ] door_names = ['left_door', 'right_door'] #door_names = [] doors = [plant.GetBodyByName(name).index() for name in door_names] door_position = DOOR_CLOSED # ~np.pi/2 #door_position = DOOR_OPEN #door_position = np.pi #door_position = np.pi/2 #door_position = np.pi/8 initial_positions = { plant.GetJointByName('left_door_hinge'): -door_position, #plant.GetJointByName('right_door_hinge'): door_position, plant.GetJointByName('right_door_hinge'): np.pi / 2, } initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions.update( zip(get_movable_joints(plant, robot), initial_conf)) initial_poses = { #brick: create_transform(translation=[0.6, 0, 0]), brick: create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]), #brick: create_transform(translation=[0.2, -0.3, 0], rotation=[0, 0, np.pi/8]), } goal_poses = { brick: create_transform(translation=[0.8, 0.2, 0.2927], rotation=[0, 0, np.pi / 8]), } goal_holding = [ #brick, ] goal_on = [ #(brick, goal_surface), ] diagram, state_machine = build_diagram(plant, scene_graph, robot, gripper, meshcat=use_meshcat, controllers=use_controllers) task = Task(diagram, plant, scene_graph, robot, gripper, movable=[brick], surfaces=surfaces, doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_poses=goal_poses, goal_holding=goal_holding, goal_on=goal_on, reset_robot=True, reset_doors=False) return task, diagram, state_machine
def load_station(time_step=0.0, **kwargs): # https://github.com/RobotLocomotion/drake/blob/master/bindings/pydrake/examples/manipulation_station_py.cc #object_file_path = FindResourceOrThrow( # "drake/external/models_robotlocomotion/ycb_objects/061_foam_brick.sdf") object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") # RuntimeError: Error control wants to select step smaller than minimum allowed (1e-14) station = ManipulationStation(time_step, IiwaCollisionModel.kBoxCollision) plant = station.get_mutable_multibody_plant() scene_graph = station.get_mutable_scene_graph() robot = plant.GetModelInstanceByName('iiwa') gripper = plant.GetModelInstanceByName('gripper') station.AddCupboard() brick = AddModelFromSdfFile(file_name=object_file_path, model_name="brick", plant=plant, scene_graph=scene_graph) station.Finalize() door_names = ['left_door', 'right_door'] doors = [plant.GetBodyByName(name).index() for name in door_names] initial_conf = [0, 0.6 - np.pi / 6, 0, -1.75, 0, 1.0, 0] #initial_conf[1] += np.pi / 6 initial_positions = { plant.GetJointByName('left_door_hinge'): -DOOR_CLOSED, #plant.GetJointByName('left_door_hinge'): -np.pi / 2, plant.GetJointByName('right_door_hinge'): np.pi / 2, } initial_positions.update( zip(get_movable_joints(plant, robot), initial_conf)) initial_poses = { brick: create_transform(translation=[0.3, 0, 0], rotation=[0, 0, np.pi / 2]), } goal_poses = { brick: create_transform(translation=[0.8, 0.2, 0.2927], rotation=[0, 0, 5 * np.pi / 4]), } diagram, state_machine = build_manipulation_station(station) task = Task(diagram, plant, scene_graph, robot, gripper, movable=[brick], doors=doors, initial_positions=initial_positions, initial_poses=initial_poses, goal_poses=goal_poses, reset_robot=True, reset_doors=False) task.station = station return task, diagram, state_machine
def open_wsg50_gripper(mbp, context, model_index): set_joint_positions(get_movable_joints(mbp, model_index), context, get_open_wsg50_positions(mbp, model_index))
def close_wsg50_gripper(mbp, context, model_index): # 0.05 set_joint_positions(get_movable_joints(mbp, model_index), context, get_close_wsg50_positions(mbp, model_index))
def fn(robot_name, door_name, door_conf1, door_conf2): robot = task.mbp.GetModelInstanceByName(robot_name) robot_joints = get_movable_joints(task.mbp, robot) collision_pairs = set( product(bodies_from_models(task.mbp, [robot, task.gripper]), fixed)) collision_fn = get_collision_fn(task.diagram, task.diagram_context, task.mbp, task.scene_graph, robot_joints, collision_pairs=collision_pairs) door_body = task.mbp.GetBodyByName(door_name) door_joints = door_conf1.joints gripper_from_door = get_door_grasp(door_body, box_from_geom) extend_fn = get_extend_fn(door_joints, resolutions=step_size * np.ones(len(door_joints))) door_joint_path = [door_conf1.positions] + list( extend_fn(door_conf1.positions, door_conf2.positions)) # TODO: check for collisions door_body_path = get_body_path(door_body, context, door_joints, door_joint_path) pull_cartesian_path = [ door_pose.multiply(gripper_from_door.inverse()) for door_pose in door_body_path ] #start_path = list(interpolate_translation(pull_cartesian_path[0], approach_vector)) #end_path = list(interpolate_translation(pull_cartesian_path[-1], approach_vector)) for _ in range(max_attempts): # TODO: could solve for kinematic solution of robot and door together pull_joint_waypoints = plan_workspace_motion( task.mbp, robot_joints, gripper_frame, reversed(pull_cartesian_path), collision_fn=collision_fn) if pull_joint_waypoints is None: continue pull_joint_waypoints = pull_joint_waypoints[::-1] combined_joints = robot_joints + door_joints combined_waypoints = [ list(rq) + list(dq) for rq, dq in zip(pull_joint_waypoints, door_joint_path) ] pull_joint_path = plan_waypoints_joint_motion( combined_joints, combined_waypoints, collision_fn=lambda q: False) if pull_joint_path is None: continue # TODO: approach & retreat path? robot_conf1 = Conf(robot_joints, pull_joint_waypoints[0]) robot_conf2 = Conf(robot_joints, pull_joint_waypoints[-1]) traj = Trajectory( Conf(combined_joints, combined_conf) for combined_conf in pull_joint_path) return (robot_conf1, robot_conf2, traj)