def plan(robot, block, fixed, teleport): grasp_gen = get_grasp_gen(robot, 'top') ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport) free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport) holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport) pose0 = BodyPose(block) conf0 = BodyConf(robot) saved_world = WorldSaver() for grasp, in grasp_gen(block): saved_world.restore() result1 = ik_fn(block, pose0, grasp) if result1 is None: continue conf1, path2 = result1 pose0.assign() result2 = free_motion_fn(conf0, conf1) if result2 is None: continue path1, = result2 result3 = holding_motion_fn(conf1, conf0, block, grasp) if result3 is None: continue path3, = result3 return Command(path1.body_paths + path2.body_paths + path3.body_paths) return None
def plan(robot, block, fixed, teleport): grasp_gen = get_grasp_gen(robot, 'top', TOOL_FRAME) ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) movable_joints = get_track_arm_joints(robot) if USE_IKFAST else get_movable_joints(robot) pose0 = BodyPose(block) conf0 = BodyConf(robot, joints=movable_joints) saved_world = WorldSaver() for grasp, in grasp_gen(block): saved_world.restore() result1 = ik_fn(block, pose0, grasp) if result1 is None: continue conf1, path2 = result1 pose0.assign() result2 = free_motion_fn(conf0, conf1) if result2 is None: continue path1, = result2 result3 = holding_motion_fn(conf1, conf0, block, grasp) if result3 is None: continue path3, = result3 return Command(path1.body_paths + path2.body_paths + path3.body_paths) return None
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(gripper_pose, length=0.04) draw_pose(approach_pose, length=0.04) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, approach_pose) if q_approach is not None: set_joint_positions(robot, movable_joints, q_approach) else: set_joint_positions(robot, movable_joints, 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): continue conf = BodyConf(robot, joints=movable_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, movable_joints, q_grasp) else: q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): 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, conf.joints, q_grasp, obstacles=obstacles, \ self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: user_input('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=movable_joints), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
def plan(robot, block, fixed, teleport): grasp_gen = get_grasp_gen(robot, 'bottom', get_tool_frame(ARM)) #'top' ik_fn = get_ik_fn(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) free_motion_fn = get_free_motion_gen(robot, fixed=([block] + fixed), teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) holding_motion_fn = get_holding_motion_gen(robot, fixed=fixed, teleport=teleport, self_collisions=ENABLE_SELF_COLLISION) #arm_joints = get_torso_arm_joints(robot, ARM) arm_joints = joints_from_names(robot, get_arm_joint_names(ARM)) pose0 = BodyPose(block) conf0 = BodyConf(robot, joints=arm_joints) saved_world = WorldSaver() for grasp, in grasp_gen(block): saved_world.restore() result1 = ik_fn(block, pose0, grasp) if result1 is None: print('ik fn fails!') continue conf1, path2 = result1 pose0.assign() result2 = free_motion_fn(conf0, conf1) if result2 is None: print("free motion plan fails!") continue path1, = result2 result3 = holding_motion_fn(conf1, conf0, block, grasp) if result3 is None: print('holding motion fails!') continue path3, = result3 return Command(path1.body_paths + path2.body_paths + path3.body_paths) return None
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
def ss_from_problem(robot, movable=[], bound='shared', teleport=False, movable_collisions=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) rigid = [body for body in get_bodies() if body != robot] fixed = [body for body in rigid if body not in movable] print('Robot:', robot) print('Movable:', movable) print('Fixed:', fixed) conf = BodyConf(robot, get_configuration(robot)) initial_atoms = [ HandEmpty(), CanMove(), IsConf(conf), AtConf(conf), initialize(TotalCost(), 0), ] for body in movable: pose = BodyPose(body, get_pose(body)) initial_atoms += [ IsMovable(body), IsPose(body, pose), AtPose(body, pose) ] for surface in fixed: initial_atoms += [Stackable(body, surface)] if is_placement(body, surface): initial_atoms += [IsSupported(pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: initial_atoms.append(Sink(body)) if 'stove' in name: initial_atoms.append(Stove(body)) body = movable[0] goal_literals = [ AtConf(conf), #Holding(body), #On(body, fixed[0]), #On(body, fixed[2]), #Cleaned(body), Cooked(body), ] PlacedCollision = Predicate([T, O, P], domain=[IsTraj(T), IsPose(O, P)], fn=get_movable_collision_test(), bound=False) actions = [ Action(name='pick', param=[O, P, G, Q, T], pre=[ IsKin(O, P, G, Q, T), HandEmpty(), AtPose(O, P), AtConf(Q), ~Unsafe(T) ], eff=[HasGrasp(O, G), CanMove(), ~HandEmpty(), ~AtPose(O, P)]), Action( name='place', param=[O, P, G, Q, T], pre=[IsKin(O, P, G, Q, T), HasGrasp(O, G), AtConf(Q), ~Unsafe(T)], eff=[HandEmpty(), CanMove(), AtPose(O, P), ~HasGrasp(O, G)]), # Can just do move if we check holding collisions Action(name='move_free', param=[Q, Q2, T], pre=[ IsFreeMotion(Q, Q2, T), HandEmpty(), CanMove(), AtConf(Q), ~Unsafe(T) ], eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]), Action(name='move_holding', param=[Q, Q2, O, G, T], pre=[ IsHoldingMotion(Q, Q2, O, G, T), HasGrasp(O, G), CanMove(), AtConf(Q), ~Unsafe(T) ], eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]), Action( name='clean', param=[O, O2], # Wirelessly communicates to clean pre=[Stackable(O, O2), Sink(O2), ~Cooked(O), On(O, O2)], eff=[Cleaned(O)]), Action( name='cook', param=[O, O2], # Wirelessly communicates to cook pre=[Stackable(O, O2), Stove(O2), Cleaned(O), On(O, O2)], eff=[Cooked(O), ~Cleaned(O)]), ] axioms = [ Axiom(param=[O, G], pre=[IsGrasp(O, G), HasGrasp(O, G)], eff=Holding(O)), Axiom(param=[O, P, O2], pre=[IsPose(O, P), IsSupported(P, O2), AtPose(O, P)], eff=On(O, O2)), ] if movable_collisions: axioms += [ Axiom(param=[T, O, P], pre=[IsPose(O, P), PlacedCollision(T, O, P), AtPose(O, P)], eff=Unsafe(T)), ] streams = [ GenStream(name='grasp', inp=[O], domain=[IsMovable(O)], fn=get_grasp_gen(robot, grasp_name), out=[G], graph=[IsGrasp(O, G)], bound=bound), # TODO: test_support GenStream(name='support', inp=[O, O2], domain=[Stackable(O, O2)], fn=get_stable_gen(fixed=fixed), out=[P], graph=[IsPose(O, P), IsSupported(P, O2)], bound=bound), FnStream(name='inverse_kin', inp=[O, P, G], domain=[IsPose(O, P), IsGrasp(O, G)], fn=get_ik_fn(robot, fixed=fixed, teleport=teleport), out=[Q, T], graph=[IsKin(O, P, G, Q, T), IsConf(Q), IsTraj(T)], bound=bound), FnStream(name='free_motion', inp=[Q, Q2], domain=[IsConf(Q), IsConf(Q2)], fn=get_free_motion_gen(robot, teleport=teleport), out=[T], graph=[IsFreeMotion(Q, Q2, T), IsTraj(T)], bound=bound), FnStream(name='holding_motion', inp=[Q, Q2, O, G], domain=[IsConf(Q), IsConf(Q2), IsGrasp(O, G)], fn=get_holding_motion_gen(robot, teleport=teleport), out=[T], graph=[IsHoldingMotion(Q, Q2, O, G, T), IsTraj(T)], bound=bound), ] return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=TotalCost())