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 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)