def create_problem(goal, obstacles=(), distance=.25, digits=3): """ Creates a Probabilistic Roadmap (PRM) motion planning problem. :return: a :class:`.FTSProblem` """ R_Q = 'R_Q' CONF, REGION = VarType(), VarType(domain=[goal] if is_region(goal) else []) Q, R = Par(CONF), Par(REGION) IsEdge = ConType([CONF, CONF], test=lambda q1, q2: is_collision_free( (q1, q2), obstacles)) # CFree Contained = ConType([CONF, REGION]) rename_variables(locals()) # Trick to make debugging easier state_vars = [Var(R_Q, CONF)] control_vars = [] ########## clauses = [ Clause([IsEdge(X[R_Q], nX[R_Q])], name='move'), ] ########## samplers = [ Sampler([Q], gen=lambda: ([(sample(), )] for _ in inf_sequence()), inputs=[]), Sampler([Contained(Q, R)], gen=lambda r: ([(sample_box(r), )] for _ in inf_sequence()), inputs=[R]), ] ########## initial_state = [Eq(X[R_Q], (0, 0))] goal_constraints = [] if is_region(goal): goal_constraints.append(Contained(X[R_Q], goal)) else: goal_constraints.append(Eq(X[R_Q], goal)) return FTSProblem(state_vars, control_vars, clauses, samplers, initial_state, goal_constraints)
def tamp_problem(oracle): DO_ARM_MOTION = True DO_COLLISIONS = True CHECK_BASE = True problem = oracle.problem for obj, pose in oracle.initial_poses.iteritems( ): # NOTE - would be nice to undo this pose.obj = obj for obj in problem.goal_poses: if problem.goal_poses[obj] == 'initial': problem.goal_poses[obj] = oracle.initial_poses[obj] elif problem.goal_poses[ obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial) raise NotImplementedError() # TODO - don't want to overwrite obj problem.goal_poses[obj] = oracle.initial_poses[ problem.goal_poses[obj]] problem.goal_poses[obj].obj = obj ########## def region_contains(p, r): # TODO - this is kind of strange return isinstance(p, Pose) and oracle.region_contains(r, p.obj, p) def collision_free(p, pap): if not DO_COLLISIONS: return True if pap is None: return True #if p is None: return True if isinstance(p, GraspTform): return True if p.obj == pap.obj: return True if pap.obj is None: return True # TODO - trajectory collisions here holding = ObjGrasp(pap.obj, pap.grasp) #holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp) #assert pose.obj != holding.object_name # NOTE - they cannot be the same for this check if not DO_ARM_MOTION: return not oracle.holding_collision(pap.grasp_config, p.obj, p, holding) return not oracle.traj_holding_collision(pap.approach_config, pap.trajs, p.obj, p, holding) ########## def sample_grasp(o): grasps = get_grasps(oracle, o) for grasp in grasps: grasp.obj = o yield grasps max_failures = 50 # 10 | 20 | 40 max_calls = 1 # 1 | INF def sample_ik(o, g, p): #saver = oracle.state_saver() oracle.set_all_object_poses( {o: p}) # TODO - saver for the initial state as well? if oracle.approach_collision(o, p, g): return for i in range(max_calls): pap = PickAndPlace(oracle.get_geom_hash(o), p, g) if not pap.sample(oracle, o, max_failures=max_failures, sample_vector=DO_ARM_MOTION, sample_arm=DO_ARM_MOTION, check_base=CHECK_BASE): break pap.obj = o yield [pap] #saver.Restore() def sample_region(o, r, max_samples=2): oracle.set_all_object_poses({o: oracle.initial_poses[o]}) poses = random_region_placements(oracle, o, [r], region_weights=True) for pose in islice(poses, max_samples): pose.obj = o yield [pose] NUM_POSES = 2 AVOID_INITIAL = True def sample_pose(o): #with oracle.state_saver(): while True: if AVOID_INITIAL: oracle.set_all_object_poses(oracle.initial_poses) else: oracle.set_all_object_poses({o: oracle.initial_poses[o]}) poses = random_region_placements(oracle, o, oracle.get_counters(), region_weights=True) #poses = cached_region_placements(self.oracle, self.obj, self.oracle.get_counters(), order=None, random=True) fixed_poses = [] for pose in islice(poses, NUM_POSES): pose.obj = o #yield pose fixed_poses.append(pose) yield fixed_poses # Exception IndexError: 'vector' in <generator object sample_pose at 0x127d0dbe0> ignored ########## def test_goal(s): state = dict(s) for obj, pose in problem.goal_poses.iteritems(): if state[(P, obj)] != pose: return False for obj, region in problem.goal_regions.iteritems(): if not region_contains(state[(P, obj)], region): return False return True def get_successor(state, obj, pose, holding): new_state = state.copy() new_state[(H, obj)] = holding new_state[(P, obj)] = pose return frozenset(new_state.items()) # TODO - how many successors to sample? # TODO - can either automatically sample goal or make special sampler # TODO - how do I signal to the algorithm that I achieve a goal? Only do implicitly def forward_dynamics(s): state = dict(s) holding = None for obj in oracle.get_objects(): if state[(H, obj)]: holding = obj # TODO - I should just make an alternating cycle of these generators or something while True: if holding is None: # TODO - why does this termainte on the first state sometimes????? obj = choice(oracle.get_objects()) pose = state[(P, obj)] #for grasp in islice(sample_grasp(obj), 1): for grasp in next(sample_grasp(obj)): for traj in next(sample_ik(obj, grasp, pose)): if all( collision_free(state[(P, obst)], traj) for obst in oracle.get_objects()): yield [(traj, get_successor(state, obj, grasp, True))] else: obj = holding grasp = state[(P, obj)] pose_generators = [sample_pose(obj)] if obj in problem.goal_poses: pose_generators.append(iter([problem.goal_poses[obj]])) if obj in problem.goal_regions: pose_generators.append( sample_region(obj, problem.goal_regions[obj])) # TODO - I should cycle using this for generator in pose_generators: # NOTE - this is generous #for pose in islice(generator, 1): for pose in next(generator): for traj in next(sample_ik(obj, grasp, pose)): if all( collision_free(state[(P, obst)], traj) for obst in oracle.get_objects()): yield [(traj, get_successor(state, obj, pose, False))] #for pose in islice(sample_pose(obj), 1): # for traj in sample_ik(obj, grasp, pose): # if all(collision_free(state[(P, obst)], traj) for obst in oracle.get_objects()): # yield [(traj, get_successor(state, obj, pose, False))] ########## Transition = ConType([STATE, CONTROL, STATE], name='Transition') #GoalPose = ConType([OBJ, STATE], name='GoalPose') #GoalRegion = ConType([OBJ, STATE], name='GoalRegion') Goal = ConType([STATE], name='Goal', test=test_goal) state_vars = [Var(S, STATE)] control_vars = [Var(A, CONTROL)] ########## actions = [ Action([Transition(X[S], U[A], nX[S])], name='transition'), ] ########## samplers = [ Sampler([Transition(s1, a, s2)], gen=forward_dynamics, inputs=[s1], name='forward'), ] ########## S0 = frozenset([((H, obj), False) for obj in oracle.get_objects()] + \ [((P, obj), pose) for obj, pose in oracle.initial_poses.iteritems()]) initial_state = [Eq(X[S], S0)] goal_constraints = [Goal(X[S])] #for obj, pose in problem.goal_poses.iteritems(): # goal_constraints.append(GoalPose(obj, pose, X[S])) #for obj, region in problem.goal_regions.iteritems(): # goal_constraints.append(GoalRegion(obj, region, X[S])) return ConstraintProblem(state_vars, control_vars, actions, samplers, initial_state, goal_constraints)
def get_problem(env, problem, robot, manipulator, base_manip, bodies, initial_conf): all_bodies = bodies.values() assert len({body.GetKinematicsGeometryHash() for body in all_bodies }) == 1 # NOTE - assuming all objects has the same geometry body1 = all_bodies[-1] # Generic object 1 body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2 # TODO - bug that when n=1, it automatically passes holding collisions because only one body used grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else [] poses = problem.known_poses if problem.known_poses else [] # Limits the number of poses #needed_poses = list(set(problem.initial_poses.values() + problem.goal_poses.values())) #poses = needed_poses + list(set(poses) - set(needed_poses))[:9] cfree_traj = cfree_traj_fn(env, manipulator, body1, body2, all_bodies) sample_grasp_traj = sample_grasp_traj_fn(env, manipulator, body1, all_bodies) sample_free_motion = sample_free_motion_fn(manipulator, base_manip, all_bodies) sample_holding_motion = sample_holding_motion_fn(manipulator, base_manip, body1, all_bodies) # OBJ, BOOL = VarType(domain=list(bodies)), VarType(domain=[True, False]) # POSE, CONF, TRAJ = VarType(domain=poses+grasps), VarType(), VarType() # # Stable = ConType([POSE], satisfying=[(p,) for p in poses]) # Grasp = ConType([POSE], satisfying=[(g,) for g in grasps]) # Motion = ConType([CONF, CONF, TRAJ]) # MotionH = ConType([CONF, CONF, POSE, TRAJ]) # GraspMotion = ConType([POSE, POSE, CONF, TRAJ]) # CFree = ConType([TRAJ, POSE], test=cfree_traj) # # O, P, G = Par(OBJ), Par(POSE), Par(POSE) # Q, Q2, T = Par(CONF), Par(CONF), Par(TRAJ) # # state_vars = [Var('R_Q', CONF), Var('O_P', POSE, args=[OBJ]), Var('O_H', BOOL, args=[OBJ])] # control_vars = [Var('R_T', TRAJ)] # # transition = [ # Clause([Stable(X['O_P', O]), Grasp(nX['O_P', O]), # Eq(X['O_H', O], False), Eq(nX['O_H', O], True), # GraspMotion(X['O_P', O], nX['O_P', O], X['R_Q'], U['R_T'])] + # [Eq(X['O_H', obj], False) for obj in bodies] + # [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='pick'), # Clause([Grasp(X['O_P', O]), Stable(nX['O_P', O]), # Eq(X['O_H', O], True), Eq(nX['O_H', O], False), # GraspMotion(nX['O_P', O], X['O_P', O], X['R_Q'], U['R_T'])] + # [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='place'), # Clause([Motion(X['R_Q'], nX['R_Q'], U['R_T'])] + # [Eq(X['O_H', obj], False) for obj in bodies] + # [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='move'), # Clause([MotionH(X['R_Q'], nX['R_Q'], X['O_P', O], U['R_T']), Eq(X['O_H', O], True)] + # [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='move_holding')] # # samplers = [ # Sampler([GraspMotion(P, G, Q, T)], gen=sample_grasp_traj, inputs=[P, G], domain=[Stable(P), Grasp(G)]), # Sampler([Motion(Q, Q2, T)], gen=sample_free_motion, inputs=[Q, Q2]), # Sampler([MotionH(Q, Q2, G, T)], gen=sample_holding_motion, inputs=[Q, Q2, G], domain=[Grasp(G)])] # # initial_state = [Eq(X['R_Q'], initial_conf)] + [ # Eq(X['O_P', obj], pose) for obj, pose in problem.initial_poses.iteritems() # ] + [Eq(X['O_H', obj], False) for obj in problem.initial_poses] # # goal_constraints = [Eq(X['R_Q'], initial_conf)] + [ # Eq(X['O_P', obj], pose) for obj, pose in problem.goal_poses.iteritems()] # # return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints) OBJ, BOOL = VarType(domain=list(bodies)), VarType(domain=[True, False]) POSE, CONF, TRAJ = VarType(domain=poses + grasps), VarType(), VarType() Stable = ConType([POSE], satisfying=[(p, ) for p in poses]) Grasp = ConType([POSE], satisfying=[(g, ) for g in grasps]) Motion = ConType([CONF, CONF, TRAJ]) MotionH = ConType([CONF, CONF, POSE, TRAJ]) CFree = ConType([TRAJ, POSE], test=cfree_traj) state_vars = [ Var('R_Q', CONF), Var('O_P', POSE, args=[OBJ]), Var('O_H', BOOL, args=[OBJ]) ] control_vars = [Var('R_T', TRAJ)] O, G, Q, Q2, T = Par(OBJ), Par(POSE), Par(CONF), Par(CONF), Par(TRAJ) transition = [ Clause([ Stable(X['O_P', O]), Grasp(nX['O_P', O]), Eq(X['O_H', O], False), Eq(nX['O_H', O], True) ] + [Eq(X['O_H', obj], False) for obj in bodies] + [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='pick'), Clause([ Grasp(X['O_P', O]), Stable(nX['O_P', O]), Eq(X['O_H', O], True), Eq(nX['O_H', O], False) ] + [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='place'), Clause([Motion(X['R_Q'], nX['R_Q'], U['R_T'])] + [Eq(X['O_H', obj], False) for obj in bodies] + [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='move'), Clause([ MotionH(X['R_Q'], nX['R_Q'], X['O_P', O], U['R_T']), Eq(X['O_H', O], True) ] + [CFree(U['R_T'], X['O_P', obj]) for obj in bodies], name='move_holding') ] samplers = [ Sampler([Motion(Q, Q2, T)], gen=sample_free_motion, inputs=[Q, Q2]), Sampler([MotionH(Q, Q2, G, T)], gen=sample_holding_motion, inputs=[Q, Q2, G], domain=[Grasp(G)]) ] #### initial_state = [Eq(X['R_Q'], initial_conf)] + [ Eq(X['O_P', obj], pose) for obj, pose in problem.initial_poses.iteritems() ] + [Eq(X['O_H', obj], False) for obj in problem.initial_poses] goal_constraints = [Eq(X['R_Q'], initial_conf)] + [ Eq(X['O_P', obj], pose) for obj, pose in problem.goal_poses.iteritems() ] return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints)
def get_problem(env, problem, robot, manipulator, base_manip, bodies, initial_conf): all_bodies = bodies.values() assert len({body.GetKinematicsGeometryHash() for body in all_bodies }) == 1 # Assuming all objects has the same geometry body1 = all_bodies[-1] # Generic object 1 body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2 grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else [] poses = problem.known_poses if problem.known_poses else [] cfree_pose = cfree_pose_fn(env, body1, body2) cfree_traj = cfree_traj_fn(env, manipulator, body1, body2, all_bodies) # Variables R_Q, O_P, O_H, R_T, = 'R_Q', 'O_P', 'O_H', 'R_T' # Types OBJ = VarType(domain=list(bodies)) CONF, TRAJ = VarType(), VarType() BOOL = VarType(domain=[True, False]) POSE = VarType(domain=poses + grasps) # Trajectory constraints FreeMotion = ConType([CONF, CONF, TRAJ]) HoldingMotion = ConType([CONF, CONF, POSE, TRAJ]) GraspMotion = ConType([POSE, POSE, CONF, TRAJ]) Stable = ConType([POSE], satisfying=[(p, ) for p in poses]) Grasp = ConType([POSE], satisfying=[(g, ) for g in grasps]) # Static constraints CFreePose = ConType([POSE, POSE], test=cfree_pose) CFreeTraj = ConType([TRAJ, POSE], test=cfree_traj) O = Par(OBJ) P, G = Par(POSE), Par(POSE) O2, P2 = Par(OBJ), Par(POSE) Q, Q2 = Par(CONF), Par(CONF) T = Par(TRAJ) rename_variables(locals()) state_vars = [ Var(R_Q, CONF), Var(O_P, POSE, args=[OBJ]), Var(O_H, BOOL, args=[OBJ]), ] control_vars = [Var(R_T, TRAJ)] #################### transition = [ Clause([ Stable(X[O_P, O]), Grasp(nX[O_P, O]), Eq(X[O_H, O], False), Eq(nX[O_H, O], True), GraspMotion(X[O_P, O], nX[O_P, O], X[R_Q], U[R_T]) ] + [Eq(X[O_H, obj], False) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='pick'), Clause([ Grasp(X[O_P, O]), Stable(nX[O_P, O]), Eq(X[O_H, O], True), Eq(nX[O_H, O], False), GraspMotion(nX[O_P, O], X[O_P, O], X[R_Q], U[R_T]) ] + [CFreePose(nX[O_P, O], X[O_P, obj]) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='place'), Clause([FreeMotion(X[R_Q], nX[R_Q], U[R_T])] + [Eq(X[O_H, obj], False) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move'), Clause([ HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T]), Eq(X[O_H, O], True) ] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move_holding') ] #################### sample_grasp_traj = sample_grasp_traj_fn(env, manipulator, body1, all_bodies) sample_free_motion = sample_free_motion_fn(manipulator, base_manip, all_bodies) sample_holding_motion = sample_holding_motion_fn(manipulator, base_manip, body1, all_bodies) samplers = [ # Pick/place trajectory Sampler([GraspMotion(P, G, Q, T)], gen=sample_grasp_traj, inputs=[P, G], domain=[Stable(P), Grasp(G)]), # Move trajectory Sampler([FreeMotion(Q, Q2, T)], gen=sample_free_motion, inputs=[Q, Q2], order=1, max_level=0), Sampler([HoldingMotion(Q, Q2, G, T)], gen=sample_holding_motion, inputs=[Q, Q2, G], domain=[Grasp(G)], order=1, max_level=0), ] #################### initial_state = [ Eq(X[R_Q], initial_conf), ] + [ Eq(X[O_P, obj], pose) for obj, pose in problem.initial_poses.iteritems() ] + [Eq(X[O_H, obj], False) for obj in problem.initial_poses] goal_constraints = [Eq(X[R_Q], initial_conf)] + [ Eq(X[O_P, obj], pose) for obj, pose in problem.goal_poses.iteritems() ] return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints)
def create_problem(): """ Creates the 1D task and motion planning FTSProblem problem. :return: a :class:`.FTSProblem` """ blocks = ['block%i' % i for i in range(2)] num_poses = pow(10, 10) initial_config = 0 # the initial robot configuration is 0 initial_poses = {block: i for i, block in enumerate(blocks) } # the initial pose for block i is i goal_poses = {block: i + 1 for i, block in enumerate(blocks) } # the goal pose for block i is i+1 #goal_poses = {blocks[0]: 1} #################### # TODO - rethink the naming... R_Q, B_P, B_H = 'R_Q', 'B_P', 'B_H' R_T = 'R_T' # NOTE - these should really just be param type CONF = VarType() BOOL = VarType(domain=[True, False]) POSE = VarType() BLOCK = VarType(domain=blocks) # TODO - VarType vs ParamType? TRAJ = VarType() B, Q, P = Par(BLOCK), Par(CONF), Par(POSE) T, Q2 = Par(TRAJ), Par(CONF) LegalKin = ConType([POSE, CONF]) CollisionFree = ConType([POSE, POSE], test=lambda p1, p2: None in (p1, p2) or p1 != p2) Motion = ConType([CONF, TRAJ, CONF]) rename_variables(locals()) # Trick to make debugging easier # NOTE - can make a holding variable for each object or just one holding variable # TODO - maybe declare the variables upfront state_vars = [ Var(R_Q, CONF), Var(B_P, POSE, args=[BLOCK]), Var(B_H, BOOL, args=[BLOCK]) ] control_vars = [Var(R_T, TRAJ)] ########## # NOTE - didn't I have some kind of bug where things had to be effects for this to work? # NOTE - this is because I have to write things in the form where we only mention things that change.... # THus, I need the identify constraint or something transition = [ Clause([ LegalKin(X[B_P, B], X[R_Q]), Eq(nX[B_P, B], None), Eq(X[B_H, B], False), Eq(nX[B_H, B], True) ] + [Eq(X[B_H, block], False) for block in blocks], name='pick'), #Clause([LegalKin(X[B_P, B], X[R_Q]), Eq(nX[B_P, B], None), Eq(nX[B_H, B], True)] + # [Eq(X[B_H, block], False) for block in blocks], name='pick'), # NOTE - this makes bool free params internally Clause([ LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_P, B], None), Eq(X[B_H, B], True), Eq(nX[B_H, B], False) ] + [CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks], name='place'), #Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_P, B], None), Eq(nX[B_H, B], False)], # + ##Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_H, B], True), Eq(nX[B_H, B], False)], # + # [CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks], name='place'), #Clause([Unconstrained(nX[R_Q])], name='move'), # NOTE - only write what changes Clause([Motion(X[R_Q], U[R_T], nX[R_Q])], name='move'), ] ########## # TODO - expand so we don't need to return lists of lists samplers = [ Sampler([P], gen=lambda: ([(p, )] for p in xrange(num_poses)), inputs=[]), Sampler([LegalKin(P, Q)], gen=lambda p: [[(p, )]] if p is not None else [], inputs=[P]), Sampler([Motion(Q, T, Q2)], gen=lambda q1, q2: [[((q1, q2), )]], inputs=[Q, Q2]), # TODO - avoid this much nesting ] ########## initial_state = [Eq(X[R_Q], initial_config)] + \ [Eq(X[B_H, block], False) for block in blocks] + \ [Eq(X[B_P, block], pose) for block, pose in initial_poses.iteritems()] goal_constraints = [ Eq(X[B_P, block], pose) for block, pose in goal_poses.iteritems() ] #goal_constraints = [Eq(X[R_Q], 1)] return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints)
def create_problem(): """ Creates the 1D task and motion planning FTSProblem problem. :return: a :class:`.FTSProblem` """ blocks = ['block%i' % i for i in range(2)] num_poses = pow(10, 10) initial_config = 0 # the initial robot configuration is 0 initial_poses = {block: i for i, block in enumerate( blocks)} # the initial pose for block i is i # the goal pose for block i is i+1 goal_poses = {block: i + 1 for i, block in enumerate(blocks)} #################### R_Q, B_P, B_H = 'R_Q', 'B_P', 'B_H' R_T = 'R_T' CONF = VarType() BOOL = VarType(domain=[True, False]) POSE = VarType() BLOCK = VarType(domain=blocks) TRAJ = VarType() B, Q, P = Par(BLOCK), Par(CONF), Par(POSE) T, Q2 = Par(TRAJ), Par(CONF) LegalKin = ConType([POSE, CONF]) CollisionFree = ConType([POSE, POSE], test=lambda p1, p2: None in (p1, p2) or p1 != p2) Motion = ConType([CONF, TRAJ, CONF]) rename_variables(locals()) # Trick to make debugging easier state_vars = [Var(R_Q, CONF), Var(B_P, POSE, args=[BLOCK]), Var(B_H, BOOL, args=[BLOCK])] control_vars = [Var(R_T, TRAJ)] ########## transition = [ Clause([LegalKin(X[B_P, B], X[R_Q]), Eq(nX[B_P, B], None), Eq(X[B_H, B], False), Eq(nX[B_H, B], True)] + [Eq(X[B_H, block], False) for block in blocks], name='pick'), Clause([LegalKin(nX[B_P, B], X[R_Q]), Eq(X[B_P, B], None), Eq(X[B_H, B], True), Eq(nX[B_H, B], False)] + [CollisionFree(X[B_P, block], nX[B_P, B]) for block in blocks], name='place'), Clause([Motion(X[R_Q], U[R_T], nX[R_Q])], name='move'), ] ########## samplers = [ Sampler([P], gen=lambda: ([(p,)] for p in xrange(num_poses)), inputs=[]), Sampler([LegalKin(P, Q)], gen=lambda p: [[(p,)]] if p is not None else [], inputs=[P]), Sampler([Motion(Q, T, Q2)], gen=lambda q1, q2: [[((q1, q2),)]], inputs=[Q, Q2]), ] ########## initial_state = [Eq(X[R_Q], initial_config)] + \ [Eq(X[B_H, block], False) for block in blocks] + \ [Eq(X[B_P, block], pose) for block, pose in initial_poses.iteritems()] goal_constraints = [Eq(X[B_P, block], pose) for block, pose in goal_poses.iteritems()] return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints)
def get_problem(env, problem, robot, manipulator, base_manip, bodies, initial_conf): all_bodies = bodies.values() assert len({body.GetKinematicsGeometryHash() for body in all_bodies }) == 1 # NOTE - assuming all objects has the same geometry body1 = all_bodies[-1] # Generic object 1 body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2 # TODO - bug that when n=1, it automatically passes holding collisions because only one body used grasps = problem.known_grasps[:MAX_GRASPS] if problem.known_grasps else [] poses = problem.known_poses if problem.known_poses else [] # Limits the number of poses #needed_poses = list(set(problem.initial_poses.values() + problem.goal_poses.values())) #poses = needed_poses + list(set(poses) - set(needed_poses))[:9] cfree_pose = cfree_pose_fn(env, body1, body2) cfree_traj = cfree_traj_fn(env, robot, manipulator, body1, body2, all_bodies) # Variables R_Q, O_P, O_H, R_T, = 'R_Q', 'O_P', 'O_H', 'R_T' #R_H = 'R_H' # Types OBJ = VarType(domain=list(bodies)) CONF, TRAJ = VarType(), VarType() #POSE, GRASP = VarType(domain=poses), VarType(domain=grasps) BOOL = VarType(domain=[True, False]) #HELD = VarType(domain=[None]+list(bodies)) POSE = VarType(domain=poses + grasps) # TODO - need to combine these # Trajectory constraints FreeMotion = ConType([CONF, CONF, TRAJ]) #HoldingMotion = ConType([CONF, CONF, GRASP, TRAJ]) #GraspMotion = ConType([POSE, GRASP, CONF, TRAJ]) HoldingMotion = ConType([CONF, CONF, POSE, TRAJ]) GraspMotion = ConType([POSE, POSE, CONF, TRAJ]) # TODO - can also just make these separate variables Stable = ConType([POSE], satisfying=[(p, ) for p in poses]) Grasp = ConType([POSE], satisfying=[(g, ) for g in grasps]) # Static constraints CFreePose = ConType([POSE, POSE], test=cfree_pose) CFreeTraj = ConType([TRAJ, POSE], test=cfree_traj) O = Par(OBJ) #P, G = Par(POSE), Par(GRASP) P, G = Par(POSE), Par(POSE) O2, P2 = Par(OBJ), Par(POSE) Q, Q2 = Par(CONF), Par(CONF) T = Par(TRAJ) rename_variables(locals()) state_vars = [ Var(R_Q, CONF), Var(O_P, POSE, args=[OBJ]), Var(O_H, BOOL, args=[OBJ]), #Var(R_H, HELD), ] control_vars = [Var(R_T, TRAJ)] #################### # TODO - could use fact that poses and grasp satisfy constraints to replace holding transition = [ Clause( [ Stable(X[O_P, O]), Grasp( nX[O_P, O]), # NOTE - not necessary but results in slowdown... Eq(X[O_H, O], False), Eq(nX[O_H, O], True), GraspMotion(X[O_P, O], nX[O_P, O], X[R_Q], U[R_T]) ] + [Eq(X[O_H, obj], False) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='pick'), # No need to test placement collisions Clause([ Grasp(X[O_P, O]), Stable(nX[O_P, O]), Eq(X[O_H, O], True), Eq(nX[O_H, O], False), GraspMotion(nX[O_P, O], X[O_P, O], X[R_Q], U[R_T]) ] + [CFreePose(nX[O_P, O], X[O_P, obj]) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='place'), # TODO - warning if you mix T and R_T #Clause([Eq(X[R_H], None), FreeMotion(X[R_Q], nX[R_Q], U[R_T])] + # [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move'), Clause( [FreeMotion(X[R_Q], nX[R_Q], U[R_T])] + #[Unconstrained(nX[R_Q])] + [Eq(X[O_H, obj], False) for obj in bodies] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move'), #Clause([Eq(X[R_H], O), HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T])] + # [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move_holding') Clause([ HoldingMotion(X[R_Q], nX[R_Q], X[O_P, O], U[R_T]), Eq(X[O_H, O], True) ] + [CFreeTraj(U[R_T], X[O_P, obj]) for obj in bodies], name='move_holding') ] #################### sample_grasp_traj = sample_grasp_traj_fn(env, robot, manipulator, body1, all_bodies) sample_free_motion = sample_free_motion_fn(manipulator, base_manip, all_bodies) sample_holding_motion = sample_holding_motion_fn(robot, manipulator, base_manip, body1, all_bodies) samplers = [ # Pick/place trajectory Sampler([GraspMotion(P, G, Q, T)], gen=sample_grasp_traj, inputs=[P, G], domain=[Stable(P), Grasp(G)]), # Move trajectory Sampler([FreeMotion(Q, Q2, T)], gen=sample_free_motion, inputs=[Q, Q2], order=1, max_level=0), Sampler([HoldingMotion(Q, Q2, G, T)], gen=sample_holding_motion, inputs=[Q, Q2, G], domain=[Grasp(G)], order=1, max_level=0), ] #################### initial_state = [ Eq(X[R_Q], initial_conf), #Eq(X[R_H], None), ] + [ Eq(X[O_P, obj], pose) for obj, pose in problem.initial_poses.iteritems() ] + [Eq(X[O_H, obj], False) for obj in problem.initial_poses] goal_constraints = [Eq(X[R_Q], initial_conf)] + [ Eq(X[O_P, obj], pose) for obj, pose in problem.goal_poses.iteritems() ] #goal_constraints = [Eq(X[O_H, list(problem.goal_poses)[0]], True)] #goal_constraints = [Eq(X[R_Q], initial_conf), Eq(X[O_H, list(problem.goal_poses)[0]], True)] return FTSProblem(state_vars, control_vars, transition, samplers, initial_state, goal_constraints)