def visualize_solution(env, problem, initial_conf, robot, manipulator, bodies, plan): def execute_traj(traj): #for j, conf in enumerate(traj.path()): #for j, conf in enumerate([traj.end()]): path = list(sample_manipulator_trajectory(manipulator, traj.traj())) for j, conf in enumerate(path): set_manipulator_conf(manipulator, conf) raw_input('%s/%s) Step?'%(j, len(path))) # Resets the initial state set_manipulator_conf(manipulator, initial_conf.value) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move': _, _, traj = args execute_traj(traj) elif action.name == 'move_holding': _, _, traj, _, _ = args execute_traj(traj) elif action.name == 'pick': obj, _, _, _, traj = args execute_traj(traj.reverse()) robot.Grab(bodies[obj]) execute_traj(traj) elif action.name == 'place': obj, _, _, _, traj = args execute_traj(traj.reverse()) robot.Release(bodies[obj]) execute_traj(traj) else: raise ValueError(action.name) env.UpdatePublishedBodies()
def grasp_env_cfree(mt, g): enable_all(False) # TODO - base config? body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): return False return True
def cfree_mtraj_pose(mt, p): enable_all(False) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) if env.CheckCollision(body2): return False return True
def _cfree_traj_pose(traj, pose): enable_all(all_bodies, False) body2.Enable(True) set_pose(body2, pose.value) for conf in traj.path(): set_manipulator_conf(manipulator, conf) if env.CheckCollision(robot, body2): return False return True
def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose enable_all(all_bodies, False) body2.Enable(True) set_pose(body2, pose.value) for conf in traj.path(): set_manipulator_conf(manipulator, conf) if env.CheckCollision(robot, body2): return False return True
def cfree_mtraj_grasp(mt, g): enable_all(False) body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): print 'cfree_mtraj_grasp' return False return True
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions #print manipulator.CheckEndEffectorCollision(manip_trans) if grasp_config is not None: yield [Config(grasp_config)]
def grasp_pose_cfree(mt, g, p): enable_all(False) # TODO - base config? body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): return False return True
def get_values(self, universe, dependent_atoms=set(), **kwargs): mq1, mq2 = map(get_value, self.inputs) collision_atoms = filter( lambda atom: atom.predicate in [CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms) collision_params = {atom: atom.args[0] for atom in collision_atoms} grasp = None for atom in collision_atoms: if atom.predicate is CFreeMTrajGrasp: assert grasp is None # Can't have two grasps _, grasp = map(get_value, atom.args) placed = [] for atom in collision_atoms: if atom.predicate is CFreeMTrajPose: _, pose = map(get_value, atom.args) placed.append(pose) #placed, grasp = [], None #print grasp, placed if placed or grasp: assert len(placed) <= len(all_bodies) enable_all(False) for b, p in zip(all_bodies, placed): b.Enable(True) set_pose(b, p.value) if grasp: assert grasp is None or len(placed) <= len(all_bodies) - 1 set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans)) robot.Grab(body1) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if grasp: robot.Release(body1) if mt: self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps... # TODO - could always hash this trajectory for the current set of constraints bound_collision_atoms = [ atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms ] #bound_collision_atoms = [] return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms raise ValueError() enable_all(False) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if mt: return [ManipMotion(mq1, mq2, mt)] return []
def dantam(env): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') set_default_robot_config(env.GetRobots()[0]) set_point(env.GetRobots()[0], (-1.5, 0, 0)) m, n = 3, 3 #m, n = 5, 5 n_obj = 8 side_dim = .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) pose_indices = list(product(range(m), range(n))) colors = {} for r, c in pose_indices: color = np.zeros(4) color[2-r] = 1. colors[(r, c)] = color + float(c)/(n-1)*np.array([1, 0, 0, 0]) poses = {} z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r, c in pose_indices: x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) poses[(r, c)] = Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z]))) initial_indices = randomize(pose_indices[:]) initial_poses = {} goal_poses = {} for i, indices in enumerate(pose_indices[:n_obj]): name = 'block%d-%d'%indices color = colors[indices] initial_poses[name] = poses[initial_indices[i]] goal_poses[name] = poses[indices] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, initial_poses[name].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=poses.values())
def _cfree_traj_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose enable_all(all_bodies, False) body1.Enable(True) body2.Enable(True) set_pose(body2, pose.value) for conf in traj.path(): set_manipulator_conf(manipulator, conf) manip_trans = manipulator.GetTransform() set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans)) if env.CheckCollision(body1, body2): return False return True
def sample_grasp_traj(pose, grasp, base_conf): enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector) if grasp_config is None: return yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_grasp_traj(pose, grasp): enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_trans) if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) robot.Grab(body1) grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return enable_all(all_bodies, False) body1.Enable(True) set_manipulator_conf(manipulator, conf1.value) manip_trans = manipulator.GetTransform() set_pose(body1, object_trans_from_manip_trans(manip_trans, grasp.grasp_trans)) robot.Grab(body1) #traj = motion_plan(env, cspace, conf2.value, self_collisions=True) traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10) robot.Release(body1) if not traj: return traj.grasp = grasp yield [(traj,)]
def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp enable_all(all_bodies, False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) # Grasp configuration if grasp_conf is None: return if DISABLE_TRAJECTORIES: yield [(Conf(grasp_conf), object())] return set_manipulator_conf(manipulator, grasp_conf) robot.Grab(body1) grasp_traj = vector_traj_helper(env, robot, approach_vector) # Trajectory from grasp configuration to pregrasp #grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp pregrasp_conf = Conf(grasp_traj.end()) # Pregrasp configuration yield [(pregrasp_conf, grasp_traj)]
def sample_grasp_traj(pose, grasp): enable_all(all_bodies, False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_conf = solve_inverse_kinematics(env, manipulator, manip_trans) if grasp_conf is None: return if DISABLE_TRAJECTORIES: yield [(Conf(grasp_conf), object())] return set_manipulator_conf(manipulator, grasp_conf) robot.Grab(body1) grasp_traj = vector_traj_helper(env, robot, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp pregrasp_conf = Conf(grasp_traj.end()) yield [(pregrasp_conf, grasp_traj)]
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi/2)) initialize_openrave(env, ARM) manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()} assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] open_gripper2(manipulator) initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot))) def enable_all(enable): for body in all_bodies: body.Enable(enable) #################### def cfree_pose_pose(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def cfree_gtraj_pose(gt, p): return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p) #################### def cfree_mtraj_grasp(mt, g): enable_all(False) body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): print 'cfree_mtraj_grasp' return False return True def cfree_mtraj_pose(mt, p): enable_all(False) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) if env.CheckCollision(robot, body2): print 'cfree_mtraj_pose' return False return True def cfree_mtraj_grasp_pose(mt, g, p): enable_all(False) body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): print 'cfree_mtraj_grasp_pose' return False return True #################### def sample_grasp_traj(pose, grasp): enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_trans) if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) robot.Grab(body1) grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp yield [(Config(grasp_traj.end()), grasp_traj)] def sample_manip_motion(mq1, mq2): enable_all(False) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if not mt: return yield [(mt,)] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[], effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj), # Move trajectory EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0), # Pick/place collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)], test=cfree_pose_pose, eager=True), EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)], test=cfree_gtraj_pose), # Move collisions EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)], test=cfree_mtraj_pose), EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)], test=cfree_mtraj_grasp), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)], test=cfree_mtraj_grasp_pose), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ManipEq(initial_manip), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager', max_time=10) #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i+1, action, args else: print 'Failure' #################### def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?'%(j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state set_manipulator_values(manipulator, initial_manip.value) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move': mq1, mq2, mt = args step_path(mt) elif action.name == 'pick': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')
def cfree_pose(pose1, pose2): # Collision free test between an object at pose1 and an object at pose2 body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2)
def rearrangement_problem(env,obj_poses=None): # import random # seed = 50 # random.seed(seed) # np.random.seed(seed) MIN_DELTA = .01 # .01 | .02 ENV_FILENAME = ENVIRONMENTS_DIR+'/single_table.xml' env.Load(ENV_FILENAME) robot = env.GetRobots()[0] TABLE_NAME = 'table1' NUM_OBJECTS = 8 WIDTH = .05 # .07 | .1 TARGET_WIDTH = 0.07 OBJ_HEIGHT = 0.2 objects = [box_body(env, WIDTH, WIDTH, .2, name='obj%s'%i, \ color=(0, (i+.5)/NUM_OBJECTS, 0)) for i in range(NUM_OBJECTS)] target_obj = box_body(env, TARGET_WIDTH, TARGET_WIDTH, .2, name='target_obj', \ color=(1, 1.5, 0)) #TODO: Place obstacle that prevent you to reach from top OBST_X = 0 OBST_WIDTH = 1. OBST_COLOR = (1, 0, 0) OBST_HEIGHT = 0.4 OBST_TRANSPARENCY = .25 place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst1', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.0, OBST_X-(OBST_WIDTH-.05)/2, 0), TABLE_NAME) place_body(env, box_body(env, .4, .05, OBST_HEIGHT, name='obst2', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.0, OBST_X+(OBST_WIDTH-.05)/2, 0), TABLE_NAME) place_body(env, box_body(env, .05, OBST_WIDTH, OBST_HEIGHT, name='obst3', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (.225, OBST_X, 0), TABLE_NAME) # roof OBST_Z = OBST_HEIGHT place_xyz_body(env, box_body(env, .4, OBST_WIDTH, .01, name='obst4', color=OBST_COLOR, transparency=OBST_TRANSPARENCY), (0, OBST_X, OBST_Z,0), TABLE_NAME) # I think this can be done once and for all REST_TORSO = [.15] robot.SetDOFValues(REST_TORSO, [robot.GetJointIndex('torso_lift_joint')]) l_model = databases.linkstatistics.LinkStatisticsModel(robot) if not l_model.load(): l_model.autogenerate() l_model.setRobotWeights() min_delta = 0.01 l_model.setRobotResolutions(xyzdelta=min_delta) # xyzdelta is the minimum Cartesian distance of an object extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in env.GetBodies()])).T robot.SetAffineTranslationLimits(*extrema) X_PERCENT = 0.0 # define intial region for movable objects init_region = create_region(env, 'init_region', ((-0.8, 0.8), (-0.7, 0.6)), TABLE_NAME, color=np.array((1, 0, 1, .5))) #init_region.draw(env) # define target object region target_obj_region = create_region(env, 'target_obj_region', ((-0.2, 0.6), (-0.5,0.5)), \ TABLE_NAME, color=np.array((0, 0, 0, 0.9))) target_obj_region.draw(env) # place object if obj_poses is not None: for obj in objects: set_pose(obj,obj_poses[get_name(obj)]) set_quat(obj, quat_from_z_rot(0)) if env.GetKinBody(get_name(obj)) is None: env.Add(obj) set_pose( target_obj, obj_poses[get_name(target_obj)] ) set_quat( target_obj, quat_from_z_rot(0) ) if env.GetKinBody(get_name(target_obj)) is None: env.Add(target_obj) else: for obj in objects: randomly_place_region(env, obj, init_region) set_quat(obj, quat_from_z_rot(0)) randomly_place_region(env, target_obj, target_obj_region) set_quat(target_obj, quat_from_z_rot(0)) object_names = [get_name(body) for body in objects] # (tothefront,totheback), (to_the_right,to_the_left) set_xy(robot, -.75, 0) # LEFT_ARM_CONFIG = HOLDING_LEFT_ARM robot.SetDOFValues(REST_LEFT_ARM, robot.GetManipulator('leftarm').GetArmIndices()) robot.SetDOFValues(mirror_arm_config(REST_LEFT_ARM), robot.GetManipulator('rightarm').GetArmIndices()) grasps = {} for obj_name in object_names: obj = env.GetKinBody(obj_name) obj_grasps = get_grasps(env, robot, obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) \ + get_grasps(env, robot, obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH) grasps[get_name(obj)] = obj_grasps target_obj = env.GetKinBody('target_obj') target_obj_grasps = get_grasps(env, robot, target_obj, GRASP_APPROACHES.TOP, GRASP_TYPES.TOUCH)+\ get_grasps(env, robot, target_obj, GRASP_APPROACHES.SIDE, GRASP_TYPES.TOUCH) grasps['target_obj'] = target_obj_grasps initial_poses={} for obj in objects: initial_poses[get_name(obj)] = get_pose(obj) initial_poses[get_name(target_obj)] = get_pose(target_obj) return ManipulationProblem(None, object_names=object_names, table_names='table1', goal_regions=init_region,grasps=grasps, initial_poses = initial_poses)
def solve_tamp(env): viewer = env.GetViewer() is not None #problem = dantam(env) problem = dantam2(env) #problem = move_several_4(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi/2)) initialize_openrave(env, 'leftarm') manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) #USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE USE_GRASP_APPROACH = GRASP_APPROACHES.TOP #USE_GRASP_TYPE = GRASP_TYPES.TOUCH USE_GRASP_TYPE = GRASP_TYPES.GRASP bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()} assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] ################################################## def enable_all(enable): for body in all_bodies: body.Enable(enable) def collision_free(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def grasp_env_cfree(mt, g): enable_all(False) # TODO - base config? body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): return False return True def grasp_pose_cfree(mt, g, p): enable_all(False) # TODO - base config? body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): return False return True ################################################## """ class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2 def get_values(self, **kwargs): self.enumerated = True pose1, pose2 = map(get_value, self.inputs) if collision_free(pose1, pose2): return [PoseCFree(pose1, pose2)] return [] #return [Movable()] # NOTE - only make movable when it fails a collision check CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now movable = [] def get_values(self, **kwargs): self.enumerated = True pose, = map(get_value, self.inputs) results = [CheckedInitial(pose)] for obj, pose2 in problem.initial_poses.iteritems(): if obj not in self.movable and collision_free(pose, pose2): pass #results.append(PoseCFree(pose, pose2)) else: self.movable.append(obj) results.append(PoseEq(obj, pose2)) #results.append(Movable(obj)) if results: pass # NOTE - I could make this fail if there is a collision # I could prevent the binding by directly adding CheckedInitial to the universe # In general, I probably can just mutate the problem however I see fit here return results """ ################################################## # NOTE - can do pose, approach manip, true approach traj, motion plan # NOTE - can make something that produces approach trajectories def get_manip_vector(pose, grasp): manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) #enable_all(False) #if manipulator.CheckEndEffectorCollision(manip_trans): # return None return ManipVector(manip_trans, approach_vector) def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions #print manipulator.CheckEndEffectorCollision(manip_trans) if grasp_config is not None: yield [Config(grasp_config)] #traj = workspace_traj_helper(base_manip, approach_vector) def sample_grasp_traj(pose, grasp, base_conf): enable_all(False) set_base_values(robot, base_conf.value) body1.Enable(True) set_pose(body1, pose.value) manip_vector = get_manip_vector(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector) if grasp_config is None: return yield [(Config(grasp_traj.end()), grasp_traj)] ################################################## # NOTE - can either include the held object in the traj or have a special condition that not colliding def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in yield None #enable_all(False) #with robot: # set_base_values(robot, bq.value) # pass # TODO - does it make sense to make a new stream for the biasing or to continuously just pass things # I suppose I could cache the state or full plan as context class MotionStream(Stream): # TODO - maybe make this produce the correct values num = 0 #def get_values(self, **kwargs): # self.enumerated = True # mq1, mq2, bq = map(get_value, self.inputs) # #mt = None # mt = MotionStream.num # MotionStream.num += 1 # Ensures all are unique # return [ManipMotion(mq1, mq2, bq, mt)] def sample_motion_plan(self, mq1, mq2, bq): set_manipulator_values(manipulator, mq1.value) set_base_values(robot, bq.value) return motion_plan(env, cspace, mq2.value, self_collisions=True) def get_values(self, universe, dependent_atoms=set(), **kwargs): mq1, mq2, bq = map(get_value, self.inputs) collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms) collision_params = {atom: atom.args[0] for atom in collision_atoms} grasp = None for atom in collision_atoms: if atom.predicate is MTrajGraspCFree: assert grasp is None # Can't have two grasps _, grasp = map(get_value, atom.args) placed = [] for atom in collision_atoms: if atom.predicate is MTrajPoseCFree: _, pose = map(get_value, atom.args) placed.append(pose) #placed, grasp = [], None print grasp, placed if placed or grasp: assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj? enable_all(False) for b, p in zip(all_bodies, placed): b.Enable(True) set_pose(b, p.value) if grasp: assert grasp is None or len(placed) <= len(all_bodies)-1 set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans)) robot.Grab(body1) mt = self.sample_motion_plan(mq1, mq2, bq) if grasp: robot.Release(body1) if mt: self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps... # TODO - could always hash this trajectory for the current set of constraints bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms] #bound_collision_atoms = [] return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms raise ValueError() enable_all(False) mt = self.sample_motion_plan(mq1, mq2, bq) if mt: return [ManipMotion(mq1, mq2, bq, mt)] return [] ################################################## cond_streams = [ #MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses), #EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[], # effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj), ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0), #MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[], # effects=[Kin(P, G, BQ, MQ)], generator=sample_ik), EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[], effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj), #EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)], # test=collision_free, eager=True), EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)], test=collision_free, eager=True), #ClassStream(inputs=[P, P2], conditions=[], outputs=[], # effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True), EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)], test=lambda mt, p: True, eager=True), EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)], test=lambda mt, g: True, eager=True), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)], test=lambda mt, g, p: True, eager=True), #ClassStream(inputs=[P], conditions=[], outputs=[], # effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream), ] ################################################## constants = map(GRASP, grasps) + map(POSE, poses) initial_full = Config(get_full_config(robot)) initial_base = get_base_conf(initial_full) initial_manip = get_arm_conf(manipulator, initial_full) initial_atoms = [ BaseEq(initial_base), ManipEq(initial_manip), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager') #plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF #plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF from misc.profiling import run_profile, str_profile #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True) #with env: env.Lock() (plan, universe), prof = run_profile(solve) env.Unlock() print SEPARATOR universe.print_domain_statistics() universe.print_statistics() print SEPARATOR print str_profile(prof) print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i+1, action, args else: print 'Failure' ################################################## def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?'%(j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state open_gripper2(manipulator) set_base_values(robot, initial_base.value) set_manipulator_values(manipulator, initial_manip.value) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move_arm': mq1, mq2, bq, mt = args #set_manipulator_values(manipulator, mq2.value) step_path(mt) elif action.name == 'pick': #o, p, q, mq, bq = args o, p, g, mq, bq, gt = args step_path(gt.reverse()) #grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': #o, p, q, mq, bq = args o, p, g, mq, bq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) #open_gripper2(manipulator) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')
def dantam_distract(env, n_obj): # (Incremental Task and Motion Planning: A Constraint-Based Approach) assert REARRANGEMENT env.Load(ENVIRONMENTS_DIR + 'empty.xml') m, n = 3, 3 #m, n = 5, 5 side_dim = .07 # .05 | .07 height_dim = .1 box_dims = (side_dim, side_dim, height_dim) separation = (side_dim, side_dim) #separation = (side_dim/2, side_dim/2) coordinates = list(product(range(m), range(n))) assert n_obj <= len(coordinates) obj_coordinates = sample(coordinates, n_obj) length = m*(box_dims[0] + separation[0]) width = n*(box_dims[1] + separation[1]) height = .7 table = box_body(env, length, width, height, name='table', color=get_color('tan1')) set_point(table, (0, 0, 0)) env.Add(table) robot = env.GetRobots()[0] set_default_robot_config(robot) set_base_values(robot, (-1.5, 0, 0)) #set_base_values(robot, (0, width/2 + .5, math.pi)) #set_base_values(robot, (.35, width/2 + .35, math.pi)) #set_base_values(robot, (.35, width/2 + .35, 3*math.pi/4)) poses = [] z = get_point(table)[2] + height + BODY_PLACEMENT_Z_OFFSET for r in range(m): row = [] x = get_point(table)[0] - length/2 + (r+.5)*(box_dims[0] + separation[0]) for c in range(n): y = get_point(table)[1] - width/2 + (c+.5)*(box_dims[1] + separation[1]) row.append(Pose(pose_from_quat_point(unit_quat(), np.array([x, y, z])))) poses.append(row) initial_poses = {} goal_poses = {} # TODO - randomly assign goal poses here for i, (r, c) in enumerate(obj_coordinates): row_color = np.zeros(4) row_color[2-r] = 1. if i == 0: name = 'goal%d-%d'%(r, c) color = BLUE goal_poses[name] = poses[m/2][n/2] else: name = 'block%d-%d'%(r, c) color = RED initial_poses[name] = poses[r][c] obj = box_body(env, *box_dims, name=name, color=color) set_pose(obj, poses[r][c].value) env.Add(obj) #for obj in randomize(objects): # randomly_place_body(env, obj, [get_name(table)]) known_poses = list(flatten(poses)) #known_poses = list(set(flatten(poses)) - {poses[r][c] for r, c in obj_coordinates}) # TODO - put the initial poses here return ManipulationProblem(function_name(inspect.stack()), object_names=initial_poses.keys(), table_names=[get_name(table)], goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses)
def cfree_pose(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2)
def get_aabb(self, body_name): body = self.env.GetKinBody(body_name) with body: # NOTE - before I didn't have this and it worked for rlplan set_pose(body, unit_pose()) return aabb_from_body(self.env.GetKinBody(body_name))
def solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] set_base_values(robot, (-.75, .2, -math.pi / 2)) initialize_openrave(env, ARM) manipulator = robot.GetActiveManipulator() cspace = CSpace.robot_arm(manipulator) base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None) bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names} geom_hashes = { body.GetKinematicsGeometryHash() for body in bodies.values() } assert len( geom_hashes) == 1 # NOTE - assuming all objects has the same geometry all_bodies = bodies.values() body1 = all_bodies[-1] body2 = all_bodies[-2] if len(bodies) >= 2 else body1 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1] poses = problem.known_poses if problem.known_poses else [] open_gripper2(manipulator) initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot))) def enable_all(enable): for body in all_bodies: body.Enable(enable) #################### def cfree_pose_pose(pose1, pose2): body1.Enable(True) set_pose(body1, pose1.value) body2.Enable(True) set_pose(body2, pose2.value) return not env.CheckCollision(body1, body2) def cfree_gtraj_pose(gt, p): return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose( gt, gt.grasp, p) #################### def cfree_mtraj_grasp(mt, g): enable_all(False) body1.Enable(True) for conf in mt.path(): set_manipulator_values(manipulator, conf) # NOTE - can also grab set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1): return False return True def cfree_mtraj_pose(mt, p): enable_all(False) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) if env.CheckCollision(body2): return False return True def cfree_mtraj_grasp_pose(mt, g, p): enable_all(False) body1.Enable(True) body2.Enable(True) set_pose(body2, p.value) for conf in mt.path(): set_manipulator_values(manipulator, conf) set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans)) if env.CheckCollision(body1, body2): return False return True #################### def sample_grasp_traj(pose, grasp): enable_all(False) body1.Enable(True) set_pose(body1, pose.value) manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp) grasp_config = inverse_kinematics_helper(env, robot, manip_trans) if grasp_config is None: return set_manipulator_values(manipulator, grasp_config) robot.Grab(body1) grasp_traj = workspace_traj_helper(base_manip, approach_vector) robot.Release(body1) if grasp_traj is None: return grasp_traj.grasp = grasp yield [(Config(grasp_traj.end()), grasp_traj)] class MotionStream(Stream): def get_values(self, universe, dependent_atoms=set(), **kwargs): mq1, mq2 = map(get_value, self.inputs) collision_atoms = filter( lambda atom: atom.predicate in [CFreeMTrajGrasp, CFreeMTrajPose], dependent_atoms) collision_params = {atom: atom.args[0] for atom in collision_atoms} grasp = None for atom in collision_atoms: if atom.predicate is CFreeMTrajGrasp: assert grasp is None # Can't have two grasps _, grasp = map(get_value, atom.args) placed = [] for atom in collision_atoms: if atom.predicate is CFreeMTrajPose: _, pose = map(get_value, atom.args) placed.append(pose) #placed, grasp = [], None #print grasp, placed if placed or grasp: assert len(placed) <= len(all_bodies) enable_all(False) for b, p in zip(all_bodies, placed): b.Enable(True) set_pose(b, p.value) if grasp: assert grasp is None or len(placed) <= len(all_bodies) - 1 set_pose( body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans)) robot.Grab(body1) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if grasp: robot.Release(body1) if mt: self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps... # TODO - could always hash this trajectory for the current set of constraints bound_collision_atoms = [ atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms ] #bound_collision_atoms = [] return [ManipMotion(mq1, mq2, mt)] + bound_collision_atoms raise ValueError() enable_all(False) set_manipulator_values(manipulator, mq1.value) mt = motion_plan(env, cspace, mq2.value, self_collisions=True) if mt: return [ManipMotion(mq1, mq2, mt)] return [] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[], effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj), # Move trajectory ClassStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[], effects=[ManipMotion(MQ, MQ2, MT)], StreamClass=MotionStream, order=1, max_level=0), # Pick/place collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)], test=cfree_pose_pose, eager=True), EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)], test=cfree_gtraj_pose), # Move collisions EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)], test=cfree_mtraj_pose), EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)], test=cfree_mtraj_grasp), EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)], test=cfree_mtraj_grasp_pose), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ManipEq(initial_manip), HandEmpty(), ] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()] goal_formula = And( ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems())) stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants) if viewer: raw_input('Start?') search_fn = get_fast_downward('eager') #plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF #plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF #plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF from misc.profiling import run_profile, str_profile #solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False) #with env: env.Lock() (plan, universe), prof = run_profile(solve) env.Unlock() print SEPARATOR universe.print_domain_statistics() universe.print_statistics() print SEPARATOR print str_profile(prof) print SEPARATOR plan = convert_plan(plan) if plan is not None: print 'Success' for i, (action, args) in enumerate(plan): print i + 1, action, args else: print 'Failure' #################### def step_path(traj): #for j, conf in enumerate(traj.path()): for j, conf in enumerate([traj.end()]): set_manipulator_values(manipulator, conf) raw_input('%s/%s) Step?' % (j, len(traj.path()))) if viewer and plan is not None: print SEPARATOR # Resets the initial state set_manipulator_values(manipulator, initial_manip.value) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?' % (i, len(plan))) if action.name == 'move': mq1, mq2, mt = args step_path(mt) elif action.name == 'pick': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Grab(bodies[o]) step_path(gt) elif action.name == 'place': o, p, g, mq, gt = args step_path(gt.reverse()) robot.Release(bodies[o]) step_path(gt) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')