def setup(sim=False, viewer=None, debug=True): # load the robot and environment for meal serving # find the openrave environment file data_base_path = find_in_workspaces(search_dirs=['share'], project=project_name, path='data', first_match_only=True) if len(data_base_path) == 0: raise Exception( 'Unable to find environment path. Did you source devel/setup.bash?' ) env_path = os.path.join(data_base_path[0], 'environments', 'table.env.xml') # Initialize logging if debug: openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug) else: openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) openravepy.misc.InitOpenRAVELogging() prpy.logger.initialize_logging() # Load the environment and robot env, robot = adapy.initialize(attach_viewer=viewer, sim=sim, env_path=env_path) # Set the active manipulator on the robot robot.arm.SetActive() # Now set everything to the right location in the environment #if using jaco, assume we have the portable mount, and robot should have a different distance to table using_jaco = robot.GetName() == 'JACO' if using_jaco: robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338], [0., 0., 1., 0.754], [0., 0., 0., 1.]]) else: robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338], [0., 0., 1., 0.795], [0., 0., 0., 1.]]) with env: robot.SetTransform(robot_pose) # Set the robot joint configurations ResetTrial(robot) load_fork_and_tool(env, robot) # add boxes for constraint to not hit user AddConstraintBoxes(env, robot) return env, robot
def __init__(self, args, endEffName="Mico"): openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() self.env, self.robot = adapy.initialize( sim=args.sim, attach_viewer=args.viewer, env_path=args.env_xml ) # make the robot go at half speed vel_limits = self.robot.GetDOFVelocityLimits() self.robot.SetDOFVelocityLimits(vel_limits * 0.5) self.manip = self.robot.SetActiveManipulator(endEffName) self.manip_rob = openravepy.interfaces.BaseManipulation(self.robot) # create the interface for basic manipulation programs self.rot = self.generate_target_rotmat() self.quat = t3d.quaternions.mat2quat(self.rot) # even though it's not obvious how we use this, we need to initialize the IKModel on self.robot ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(self.robot,iktype=openravepy.IkParameterization.Type.Transform6D) #ikmodel.autogenerate() try: # ideally this would return 0 instead of erroring if it fails, but for now a try-catch will do the trick # actually, a try-catch doesn't help. just run autogenerate whenever you change the robot ikmodel.load() except: ikmodel.autogenerate() self.dist_to_goal_publisher = rospy.Publisher(distance_to_goal_topic, Float64, queue_size=10)
def setUp(self): import openravepy if not self.is_setup: openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal) self.is_setup = True self.env = openravepy.Environment() with self.env: self.env.Load('data/wamtest2.env.xml') self.robot = self.env.GetRobot('BarrettWAM') self.manipulator = self.robot.GetManipulator('arm') self.env.Remove(self.env.GetKinBody('floor')) self.robot.SetActiveManipulator(self.manipulator) self.robot.SetActiveDOFs(self.active_dof_indices) self.planner = self.planner_factory() for base_cls in self.__class__.__bases__: if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'): base_cls.setUp(self)
def setUp(self): if not self.is_setup: openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal) self.env = openravepy.Environment() self.robot = self.env.ReadRobotXMLFile( 'robots/barrettwam.robot.xml') self.env.Add(self.robot)
def rave_env(): if IMPORT_OPENRAVE: logger.warn("Starting openrave") orpy.RaveInitialize(load_all_plugins=True) logger.warn("Starting a new environment") env = orpy.Environment() yield env logger.warn("Destroying a new environment") env.Destroy() logger.warn("Destroying Rave runtime") orpy.RaveDestroy() else: yield None
def Initialize_Adapy(args, env_path='/environments/tablewithobjects_assisttest.env.xml'): """ Initializes robot and environment through adapy, using the specified environment path @param env_path path to OpenRAVE environment @return environment, robot """ #env_path = '/environments/tablewithobjects_assisttest.env.xml' adapy_args = {'sim':args.sim, 'attach_viewer':args.viewer, 'env_path':env_path } openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug) #openravepy.misc.InitOpenRAVELogging(); env, robot = adapy.initialize(**adapy_args) Init_Robot(robot) return env,robot
def setUp(self): import openravepy GeometryInfo = openravepy.KinBody.Link.GeometryInfo if not self.is_setup: openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal) self.is_setup = True self.env = openravepy.Environment() with self.env: self.env.Load('data/wamtest2.env.xml') self.robot = self.env.GetRobot('BarrettWAM') self.manipulator = self.robot.GetManipulator('arm') # Add sphere geometry to the robot for CHOMP. for link_name, spheres in self.orcdchomp_spheres.iteritems(): link = self.robot.GetLink(link_name) assert link is not None geometry_infos = [] for sphere in spheres: geometry_info = GeometryInfo() geometry_info._type = openravepy.GeometryType.Sphere geometry_info._t = numpy.eye(4) geometry_info._t[0:3, 3] = sphere.position geometry_info._bModifiable = False geometry_info._bVisible = False geometry_info._vGeomData = numpy.array([sphere.radius] * 3) geometry_infos.append(geometry_info) link.SetGroupGeometries('spheres', geometry_infos) # Remove the floor because it can cause spurious collisions. self.env.Remove(self.env.GetKinBody('floor')) self.robot.SetActiveManipulator(self.manipulator) self.robot.SetActiveDOFs(self.active_dof_indices) self.planner = self.planner_factory() for base_cls in self.__class__.__bases__: if base_cls != BasePlannerTest and hasattr(base_cls, 'setUp'): base_cls.setUp(self)
) parser.add_argument( '-v', '--visualize', action='store_true', help= 'Enable visualization of tree growth (only applicable for simple robot)' ) parser.add_argument('-d', '--debug', action='store_true', help='Enable debug logging') args = parser.parse_args() openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) openravepy.misc.InitOpenRAVELogging() if args.debug: openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug) env = openravepy.Environment() env.SetViewer('qtcoin') env.GetViewer().SetName('Homework 2 Viewer') # First setup the environment and the robot visualize = args.visualize if args.robot == 'herb': robot = HerbRobot(env, args.manip) planning_env = HerbEnvironment(robot) visualize = False
openrave_data_path = os.getenv('OPENRAVE_DATA', '') openrave_data_paths = openrave_data_path.split(':') if ordata_path_thispack not in openrave_data_paths: if openrave_data_path == '': os.environ['OPENRAVE_DATA'] = ordata_path_thispack else: datastr = str('%s:%s' % (ordata_path_thispack, openrave_data_path)) os.environ['OPENRAVE_DATA'] = datastr # Set database file to be in this folder only relative_ordatabase = '/database' ordatabase_path_thispack = curr_path + relative_ordatabase os.environ['OPENRAVE_DATABASE'] = ordatabase_path_thispack samples = 10 # Get rid of warnings openravepy.RaveInitialize(True, openravepy.DebugLevel.Fatal) openravepy.misc.InitOpenRAVELogging() class RoboHandler: def __init__(self): self.openrave_init() self.problem_init() print "Start Ordering Grasps" # Order grasps based on your own scoring metric #self.order_grasps() print "Finished Ordering Grasps" # Order grasps with noise print "Start Ordering Noisy Grasps" self.order_grasps_noisy() print "Finished Ordering Noisy Grasps"
type=str, help='environment XML file; defaults to an empty environment') parser.add_argument('-b', '--segway-sim', action='store_true', help='simulate base') parser.add_argument('-p', '--perception-sim', action='store_true', help='simulate perception') parser.add_argument('--debug', action='store_true', help='enable debug logging') args = parser.parse_args() openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() if args.debug: openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Debug) herbpy_args = { 'sim': args.sim, 'attach_viewer': args.viewer, 'robot_xml': args.robot_xml, 'env_path': args.env_xml, 'segway_sim': args.segway_sim, 'perception_sim': args.perception_sim } if args.sim and not args.segway_sim: herbpy_args['segway_sim'] = args.sim
def main(): """Execute demo for grasping glass.""" parser = argparse.ArgumentParser() parser.add_argument('--viewer', '-v', type=str, default='interactivemarker', help='The viewer to attach (none for no viewer)') parser.add_argument('--monitor', action='store_true', help='Display a UI to monitor progress of the planner') parser.add_argument('--planner', type=str, choices=['dfs', 'restart'], default='restart', help='The planner to use') parser.add_argument('--robot', type=str, default='herb', help='Robot to run the task on') openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) openravepy.misc.InitOpenRAVELogging() args = parser.parse_args() env, robot = herbpy.initialize() # Get the desired manipulator manipulator = robot.GetManipulator('right') if args.viewer != 'none': env.SetViewer(args.viewer) monitor = None # Create a monitor if args.monitor: monitor = magi.monitor.ActionMonitor() def signal_handler(signum, frame): """Signal handler to gracefully kill the monitor.""" monitor.stop() sys.exit(0) signal.signal(signal.SIGINT, signal_handler) signal.signal(signal.SIGTERM, signal_handler) # Create a planner if args.planner == 'restart': planner = RestartPlanner(monitor=monitor) elif args.planner == 'dfs': planner = DepthFirstPlanner(monitor=monitor, use_frustration=True) if monitor is not None: monitor.reset() # Detect objects table, glass = detect_objects(robot) try: # Create the task. action = grasp_glass_action_graph(manipulator, glass, table) # Plan the task with env: solution = planner.plan_action(env, action) # Execute the task execute_pipeline(env, solution, simulate=True, monitor=monitor) except ActionError as err: LOGGER.info('Failed to complete planning for task: %s', str(err)) raise except ExecutionError as err: LOGGER.info('Failed to execute task: %s', str(err)) raise IPython.embed() if monitor: monitor.stop()
'-v', "validate_result", is_flag=True, help="Validate DH parameters before returning") @click.option("--viewer", "enable_viewer", is_flag=True, help="Enable OpenRAVE viewer") def cli(robot, base_link, ee_link, modified_dh, validate_result, enable_viewer, env): """ Compute DH parameters of the ROBOT kinematic chain defined by BASE_LINK and EE_LINK. Example usage: `python openrave.py robots/pr2-beta-static.zae base_link \\ r_gripper_palm_link` """ if enable_viewer: env.SetViewer("qtcoin") dh = get_dh_table(robot, base_link, ee_link, modified_dh, validate_result) click.echo(dh) if enable_viewer: input("Press any key to quit...") if __name__ == '__main__': try: rave.RaveInitialize(load_all_plugins=True, level=rave.DebugLevel.Error) cli() finally: rave.RaveDestroy()
def initialize(robot_xml=None, env_path=None, attach_viewer=False, sim=True, **kw_args): import prpy, os prpy.logger.initialize_logging() # Hide TrajOpt logging. os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN') # Load plugins. if prpy.dependency_manager.is_catkin(): prpy.dependency_manager.export() else: prpy.dependency_manager.export(PACKAGE) openravepy.RaveInitialize(True) # Create the environment. env = openravepy.Environment() if env_path is not None: if not env.Load(env_path): raise Exception('Unable to load environment frompath %s' % env_path) # Load the robot model_name = None share_directories = None if robot_xml is None: if prpy.dependency_manager.is_catkin(): from catkin.find_in_workspaces import find_in_workspaces share_directories = find_in_workspaces(search_dirs=['share'], project=PACKAGE) if not share_directories: logger.error('Unable to find share directory in catkin workspace.') raise ValueError('Unable to find share directory in catkin workspace.') import os.path model_name = 'barrettsegway.robot.xml' for share_directory in share_directories: xml_path = os.path.join(share_directory, 'robots', model_name) if os.path.exists(xml_path): robot_xml = xml_path if robot_xml is None: err_str = 'Unable to find robot model with name: %s in directories %s.' % (model_name, share_directories) logger.error(err_str) raise ValueError(err_str) # Load the robot model robot = env.ReadRobotXMLFile(robot_xml) env.Add(robot) # Default arguments. keys = [ 'arm_sim', 'hand_sim', 'ft_sim', 'segway_sim' ] for key in keys: if key not in kw_args: kw_args[key] = sim from wamrobot import WAMRobot prpy.bind_subclass(robot, WAMRobot, **kw_args) if sim: dof_indices, dof_values \ = robot.configurations.get_configuration('zero') robot.SetDOFValues(dof_values, dof_indices) # Start by attempting to load or_rviz. if attach_viewer == True: attach_viewer = 'rviz' 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 = 'qtcoin' 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
#!/usr/bin/env python # -*- coding: utf-8 -*- #based on HW3 for RBE 595/CS 525 Motion Planning import time import openravepy openravepy.RaveInitialize() openravepy.RaveLoadPlugin('build/rrt-pluguin') import multiprocessing, math, time if not __openravepy_build_doc__: from openravepy import * from numpy import * def waitrobot(robot): """busy wait for robot completion""" while not robot.GetController().IsDone(): time.sleep(0.01) def tuckarms(env, robot): with env: jointnames = [ 'l_shoulder_lift_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint', 'r_shoulder_lift_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint' ] robot.SetActiveDOFs( [robot.GetJoint(name).GetDOFIndex() for name in jointnames]) robot.SetActiveDOFValues([
def setup(sim=False, viewer=None, debug=False): data_base_path = find_in_workspaces(search_dirs=['share'], project=project_name, path='data', first_match_only=True) if len(data_base_path) == 0: raise Exception( 'Unable to find environment path. Did you source devel/setup.bash?' ) env_path = os.path.join(data_base_path[0], 'environments', 'table.env.xml') # Initialize logging if debug: openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Debug) else: openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) openravepy.misc.InitOpenRAVELogging() # Load the environment env = openravepy.Environment() if not env.Load(env_path): raise IOError('Unable to load environment "{:s}".'.format(env_path)) with env: or_urdf = openravepy.RaveCreateModule(env, 'urdf') ada_name = or_urdf.SendCommand('LoadURI {:s} {:s}'.format( URDF_PATH, SRDF_PATH)) env.SetViewer('InteractiveMarker') robot = env.GetRobot(ada_name) #TODO get this from a rosparam right_handed = True # Set the active manipulator on the robot robot.arm = robot.GetManipulator('Mico') # robot.arm.SetActive() # Now set everything to the right location in the environment #if using jaco, assume we have the portable mount, and robot should have a different distance to table using_jaco = robot.GetName() == 'JACO' if using_jaco: robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338], [0., 0., 1., 0.754], [0., 0., 0., 1.]]) else: robot_pose = numpy.array([[1., 0., 0., 0.409], [0., 1., 0., 0.338], [0., 0., 1., 0.795], [0., 0., 0., 1.]]) with env: robot.SetTransform(robot_pose) #if sim is True: # startConfig = numpy.array([ 3.33066907e-16, 2.22044605e-16, 1.66608370e+00, # -1.65549603e+00, -1.94424475e-01, 1.06742772e+00, # -1.65409614e+00, 1.30780704e+00]) logger.info('Setting Initial Robot Configuration to Serving') # robot.SetDOFValues(startConfig) logger.info('Initial Config Set') # Load the fork into the robot's hand tool = env.ReadKinBodyURI('objects/kinova_tool.kinbody.xml') env.Add(tool) # Fork in end-effector #ee_in_world = robot.GetLink('j2n6a300_link_6').GetTransform() tool_in_ee = numpy.array([[-1., 0., 0., 0.], [0., 1., 0., -0.002], [0., 0., -1., -0.118], [0., 0., 0., 1.]]) ee_in_world = robot.arm.GetEndEffectorTransform() if right_handed: y_trans_tool = 0.004 else: y_trans_tool = -0.004 tool_in_ee = numpy.array([[1., 0., 0., 0.], [0., 1., 0., y_trans_tool], [0., 0., 1., -0.042], [0., 0., 0., 1.]]) rotate_tool_in_ee = rodrigues([0., 0., 0.]) rotate_tool_in_ee = rodrigues([0., 0., numpy.pi / 32.]) #rotate_tool_in_ee = rodrigues([0., 0., -np.pi/32.]) tool_in_ee[0:3, 0:3] = numpy.dot(rotate_tool_in_ee, tool_in_ee[0:3, 0:3]) tool_in_world = numpy.dot(ee_in_world, tool_in_ee) tool.SetTransform(tool_in_world) fork = env.ReadKinBodyURI('objects/fork.kinbody.xml') env.Add(fork) fork_in_hole = numpy.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., -0.03], [0., 0., 0., 1.]]) hole_in_tool = numpy.array([[0., 0., 1., 0.], [0., 1., 0., -0.0225], [-1., 0., 0., 0.0408], [0., 0., 0., 1.]]) fork_in_tool = numpy.dot(hole_in_tool, fork_in_hole) fork_in_ee = numpy.dot(tool_in_ee, fork_in_tool) fork_in_world = numpy.dot(ee_in_world, fork_in_ee) fork.SetTransform(fork_in_world) logger.info('creating fork and tool boxes') # fork_box = make_collision_box_body(fork, add_to_pos=np.array([0.0, 0.0, 0.05]), add_to_extents=np.array([0.02, 0.02, 0.1])) # tool_box = make_collision_box_body(tool, add_to_pos=np.array([0.0, 0.0, 0.04]), add_to_extents=np.array([0.055, 0.055, 0.055])) # logger.info('fork and tool boxes created') #find all finger links # finger_link_inds = [] # grab_link = None # for ind,link in enumerate(robot.GetLinks()): # if 'inger' in link.GetName(): # finger_link_inds.append(ind) # if 'end_effector' in link.GetName(): # grab_link = link # # logger.info('Grabbing tool and fork') robot.Grab(tool) robot.Grab(fork) # robot.Grab(tool_box, grablink=grab_link, linkstoignore=finger_link_inds) # robot.Grab(fork_box, grablink=grab_link, linkstoignore=finger_link_inds) return robot, env