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
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))
from openravepy import RaveSetDebugLevel, DebugLevel, Environment, RaveDestroy from openravepy.misc import SetViewerUserThread from collections import deque from random import sample as random_sample #from tamp.learning_tamp import * from random import random from manipulation.primitives.utils import get_env from manipulation.primitives.transforms import * from openravepy import * from manipulation.bodies.robot import open_gripper, get_manip_trans from manipulation.bodies.robot import manip_from_pose_grasp import copy env=Environment() ENV_FILENAME = '../..//env/just_empty.xml' env.SetViewer('qtcoin') env.Load(ENV_FILENAME) def translate_point( target_transform,point): if len(point) == 3: point = np.concatenate([point, np.array([1])]) elif len(point) != 4: print 'Invalid dimension' return transformed_point = trans_dot( target_transform, point) # equation 2.23 in Murray return transformed_point[:-1] # define object and place it (0,0,0) width = 0.07 length = 0.03 height = 0.1