def main(argv):
try:
opts, args = getopt.getopt(argv, 'h:v', ['help', 'viewer'])
except getopt.GetoptError as err:
print str(err)
print HELP_MESSAGE
return
viewer = False
for opt, arg in opts:
if opt in ('-h', '--help'):
print HELP_MESSAGE
return
elif opt in ('-v', '--viewer'):
viewer = True
env = Environment()
try:
execute = lambda: script(env)
if viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def main(argv):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
env = Environment()
try:
execute = lambda: solve_tamp(env)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
print 'Done!'
def main(_):
parser = argparse.ArgumentParser()
parser.add_argument('-focus',
action='store_true',
help='use focused algorithm.')
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
env = Environment()
try:
execute = lambda: solve_tamp(env, args.focus)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
print 'Done!'
def main(or_viewer=False, hpn_viewer=False):
#exp = Experiment({'tableIkea1' : (hu.Pose(1.3, 0.0, 0.0, math.pi/2.0), TINY_VAR)},
# {'objA' : (hu.Pose(1.1, 0.0, ikZ, 0.0), TINY_VAR)},
# ['tableIkea1Top', 'tableIkea1Left'])
#PlanTest(exp)
#raw_input('Here') # NOTE - cannot run both viewers
assert not or_viewer or not hpn_viewer
print 'Window:', not debug('noW')
env = Environment() # NOTE - need this for load_problem
#execute = lambda: solve_belief(env, no_window=not hpn_viewer)
execute = lambda: solve_deterministic(env, no_window=not hpn_viewer)
#execute = lambda: openrave_test(env)
#execute = lambda: grasp_test(env, no_window=not hpn_viewer)
try:
if or_viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def main(argv):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('--problem', help='problem name.', default='simple')
parser.add_argument('-fd', action='store_true', help='FastDownward (a dummy flag).')
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
parser.add_argument('-display', action='store_true', help='display solution.')
parser.add_argument('-focus', action='store_true', help='focused.')
parser.add_argument('-movie', action='store_true', help='record a movie.')
args = parser.parse_args()
if args.problem not in list_problems():
print args.problem, 'is not a valid problem'
return
env = Environment() # NOTE - need this for load_problem
problem = load_problem(args.problem)
try:
execute = lambda: solve_tamp(env, problem, args.display, incremental=not args.focus, focused=args.focus, movie=args.movie)
if args.viewer: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
def main(argv):
HELP_MESSAGE = 'python generate_problem.py -p <problem>'
try:
opts, args = getopt.getopt(argv, 'hp:v',
['help', 'problem=', 'visualize'])
except getopt.GetoptError:
print HELP_MESSAGE
return
problem = None
visualize = False
for opt, arg in opts:
if opt in ('-h', '--help'):
print HELP_MESSAGE
return
elif opt in ('-p', '--problem'):
problem = arg
elif opt in ('-v', '--visualize'):
visualize = not visualize
if problem is None:
print HELP_MESSAGE
return
if not hasattr(generate, problem):
print problem, 'is not a valid problem'
return
env = Environment()
try:
execute = lambda: save(env, getattr(generate, problem))
if visualize: execute_viewer(env, execute)
else: execute()
finally:
if env.GetViewer() is not None:
env.GetViewer().quitmainloop()
RaveDestroy()
class OpenRAVEViewer(Viewer):
_viewer = None
def __init__(self, env=None):
assert OpenRAVEViewer._viewer == None
if env == None:
self.env = Environment()
else:
self.env = env
self.env.SetViewer('qtcoin')
self.name_to_rave_body = {}
OpenRAVEViewer._viewer = self
def clear(self):
for b in self.name_to_rave_body.itervalues():
b.delete()
self.name_to_rave_body = {}
@staticmethod
def create_viewer():
# if reset and OpenRAVEViewer._viewer != None:
# ## close the old viewer to avoid a threading error
# OpenRAVEViewer._viewer = None
if OpenRAVEViewer._viewer == None:
return OpenRAVEViewer()
OpenRAVEViewer._viewer.clear()
return OpenRAVEViewer._viewer
def record_plan(self, plan, outf, res=(640, 480), cam=None):
"""
creates a video of a plan and stores it in outf
"""
obj_list = []
horizon = plan.horizon
v = self.env.GetViewer()
if osp.exists('.video_images'):
shutil.rmtree('.video_images')
os.makedirs('.video_images')
for p in plan.params.itervalues():
if not p.is_symbol():
obj_list.append(p)
for t in range(horizon):
self.draw(obj_list, t)
time.sleep(0.1)
if cam is None:
cam = v.GetCameraTransform()
im = v.GetCameraImage(res[0], res[1], cam, [640, 640, 320, 240])
scipy.misc.imsave(
'.video_images/frame_' + str('%05d' % t) + '.png', im)
outfname = "{}.mp4".format(outf)
if osp.exists(outfname):
os.remove(outfname)
arglist = [
'avconv', '-f', 'image2', '-r', '10', "-i",
".video_images/frame_%05d.png", "-f", "mp4", "-bf", "1", "-r",
"30", "{}.mp4".format(outf)
]
subprocess.call(arglist)
def initialize_from_workspace(self, workspace):
pass
def draw(self, objList, t, transparency=0.7):
"""
This function draws all the objects from the objList at timestep t
objList : list of parameters of type Object
t : timestep of the trajectory
"""
for obj in objList:
self._draw_rave_body(obj, obj.name, t, transparency)
def draw_traj(self, objList, t_range):
"""
This function draws the trajectory of objects from the objList
objList : list of parameters of type Object
t_range : range of timesteps to draw
"""
for t in t_range:
for obj in objList:
name = "{0}-{1}".format(obj.name, t)
self._draw_rave_body(obj, name, t)
def _draw_rave_body(self, obj, name, t, transparency=0.7):
rotation = np.array([[0], [0], [0]])
assert isinstance(obj, Object)
if name not in self.name_to_rave_body:
self.name_to_rave_body[name] = OpenRAVEBody(
self.env, name, obj.geom)
if isinstance(obj.geom, PR2):
self.name_to_rave_body[name].set_dof(obj.backHeight[:, t],
obj.lArmPose[:, t],
obj.lGripper[:, t],
obj.rArmPose[:, t],
obj.rGripper[:, t])
if isinstance(obj.geom, Can) or isinstance(
obj.geom, Box) or isinstance(obj.geom, Table):
rotation = obj.rotation[:, t]
assert not np.any(np.isnan(rotation))
assert not np.any(np.isnan(obj.pose[:, t]))
self.name_to_rave_body[name].set_pose(obj.pose[:, t], rotation)
self.name_to_rave_body[name].set_transparency(transparency)
def animate_plan(self, plan, delay=.1):
obj_list = []
horizon = plan.horizon
for p in plan.params.itervalues():
if not p.is_symbol():
obj_list.append(p)
for t in range(horizon):
self.draw(obj_list, t)
time.sleep(delay)
def draw_plan(self, plan):
horizon = plan.horizon
self.draw_plan_range(plan, (0, horizon - 1))
def draw_plan_range(self, plan, (start, end)):
obj_list = self._get_plan_obj_list(plan)
self.draw_traj(obj_list, range(start, end + 1))
def initialize(robot_xml=None,
env_path=None,
viewer='rviz',
sim=True,
**kw_args):
prpy.logger.initialize_logging()
#Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
#Load plugins
prpy.dependency_manager.export()
RaveInitialize(True)
#Create the environment
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError(
'Unable to load environment from path {:s}'.format(env_path))
#Load the URDF file into OpenRave.
urdf_module = RaveCreateModule(env, 'urdf')
if urdf_module is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
'built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
urdf_uri = 'package://fetchpy/robot_description/fetch.gazebo.urdf'
srdf_uri = 'package://fetchpy/robot_description/fetch.srdf'
args = 'Load {:s} {:s}'.format(urdf_uri, srdf_uri)
fetch_name = urdf_module.SendCommand(args)
if fetch_name is None:
raise ValueError('Failed loading FETCH model using or_urdf.')
robot = env.GetRobot(fetch_name)
if robot is None:
raise ValueError(
'Unable to find robot with name "{:s}".'.format(fetch_name))
#Default to FCL.
collision_checker = RaveCreateCollisionChecker(env, 'fcl')
if collision_checker is not None:
env.SetCollisionChecker(collision_checker)
else:
collision_checker = env.GetCollisionChecker()
logger.warning(
'Failed creating "fcl", defaulting to the default OpenRAVE'
' collision checker. Did you install or_fcl?')
#Enable Baking if it is supported
try:
result = collision_checker.SendCommand('BakeGetType')
is_baking_suported = (result is not None)
except openrave_exception:
is_baking_suported = False
if is_baking_suported:
robot_checker_factory = BakedRobotCollisionCheckerFactory()
else:
robot_checker_factory = SimpleRobotCollisionCheckerFactory()
logger.warning(
'Collision checker does not support baking. Defaulting to'
' the slower SimpleRobotCollisionCheckerFactory.')
#Default arguments
keys = [
'arm_sim', 'arm_torso_sim', 'gripper_sim', 'head_sim', 'torso_sim',
'base_sim', 'talker_sim', 'perception_sim', 'whole_body_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
prpy.bind_subclass(robot,
FETCHRobot,
robot_checker_factory=robot_checker_factory,
**kw_args)
if sim:
dof_indices, dof_values = robot.configurations.get_configuration(
'straight')
robot.SetDOFValues(dof_values, dof_indices)
#Start by attempting to load or_rviz.
viewers = ['qtcoin', 'rviz']
if viewer is None:
viewer = 'rviz'
if viewer not in viewers:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
if viewer == 'rviz':
env.SetViewer(viewer)
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
viewer == 'qtcoin'
if viewer == 'qtcoin':
env.SetViewer(viewer)
if viewer and env.GetViewer() is None:
env.SetViewer(viewer)
if env.GetViewer() is None:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
# if attach_viewer == 'True':
# attach_viewer = 'qtcoin'
# env.SetViewer(attach_viewer)
# # Fall back on qtcoin if loading or_rviz failed
# if env.GetViewer() is None:
# logger.warning('Loading the RViz viewer failed. Do you have or_interactive'
# ' marker installed? Falling back on qtcoin.')
# attach_viewer = 'rviz'
# if attach_viewer and env.GetViewer() is None:
# env.SetViewer(attach_viewer)
# if env.GetViewer() is None:
# raise Exception('Failed creating viewer of type "{0:s}".'.format(attach_viewer))
#Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot