def _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose _enable_all(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_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose _enable_all(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_holding_motion(conf1, conf2, grasp): # Sample motion while holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return _enable_all(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(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 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 solve_tamp(env): viewer = env.GetViewer() is not None problem = PROBLEM(env) robot = env.GetRobots()[0] initialize_openrave(env, ARM, min_delta=.01) 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} 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 grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:MAX_GRASPS] poses = problem.known_poses if problem.known_poses else [] open_gripper(manipulator) initial_conf = Conf(robot.GetConfigurationValues()[manipulator.GetArmIndices()]) def _enable_all(enable): # Enables or disables all bodies for collision checking for body in all_bodies: body.Enable(enable) #################### 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 _cfree_traj_pose(traj, pose): # Collision free test between a robot executing traj and an object at pose _enable_all(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_grasp_pose(traj, grasp, pose): # Collision free test between an object held at grasp while executing traj and an object at pose _enable_all(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 cfree_traj(traj, pose): # Collision free test between a robot executing traj (which may or may not involve a grasp) and an object at pose if DISABLE_TRAJ_COLLISIONS: return True return _cfree_traj_pose(traj, pose) and (traj.grasp is None or _cfree_traj_grasp_pose(traj, traj.grasp, pose)) #################### def sample_grasp_traj(pose, grasp): # Sample pregrasp config and motion plan that performs a grasp _enable_all(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_free_motion(conf1, conf2): # Sample motion while not holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return _enable_all(False) set_manipulator_conf(manipulator, conf1.value) #traj = motion_plan(env, cspace, conf2.value, self_collisions=True) traj = cspace_traj_helper(base_manip, cspace, conf2.value, max_iterations=10) if not traj: return traj.grasp = None yield [(traj,)] def sample_holding_motion(conf1, conf2, grasp): # Sample motion while holding if DISABLE_TRAJECTORIES: yield [(object(),)] # [(True,)] return _enable_all(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,)] #################### cond_streams = [ # Pick/place trajectory EasyListGenStream(inputs=[P, G], outputs=[Q, T], conditions=[], effects=[GraspMotion(P, G, Q, T)], generator=sample_grasp_traj), # Move trajectory EasyListGenStream(inputs=[Q, Q2], outputs=[T], conditions=[], effects=[FreeMotion(Q, Q2, T)], generator=sample_free_motion, order=1, max_level=0), EasyListGenStream(inputs=[Q, Q2, G], outputs=[T], conditions=[], effects=[HoldingMotion(Q, Q2, G, T)], generator=sample_holding_motion, order=1, max_level=0), # Collisions EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePose(P, P2)], test=cfree_pose, eager=True), EasyTestStream(inputs=[T, P], conditions=[], effects=[CFreeTraj(T, P)], test=cfree_traj), ] #################### constants = map(GRASP, grasps) + map(POSE, poses) initial_atoms = [ ConfEq(initial_conf), HandEmpty(), ] + [ PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems() ] goal_formula = And(ConfEq(initial_conf), *(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: env.UpdatePublishedBodies() 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, max_time=15) env.Lock() plan, universe = solve() env.Unlock() print SEPARATOR #universe.print_statistics() 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 _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) env.UpdatePublishedBodies() raw_input('%s/%s) Step?'%(j, len(path))) if viewer and plan is not None: print SEPARATOR # Resets the initial state #set_manipulator_conf(manipulator, initial_conf.value) set_manipulator_conf(manipulator, initial_conf) for obj, pose in problem.initial_poses.iteritems(): set_pose(bodies[obj], pose.value) env.UpdatePublishedBodies() if not DISABLE_TRAJECTORIES: 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() else: for i, (action, args) in enumerate(plan): raw_input('\n%s/%s) Next?'%(i, len(plan))) if action.name == 'move': _, q2, _ = args set_manipulator_conf(manipulator, q2) elif action.name == 'move_holding': _, q2, _, _, _ = args set_manipulator_conf(manipulator, q2) elif action.name == 'pick': obj, _, _, _, traj = args robot.Grab(bodies[obj]) elif action.name == 'place': obj, _, _, _, traj = args robot.Release(bodies[obj]) else: raise ValueError(action.name) env.UpdatePublishedBodies() raw_input('Finish?')