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) wampy_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: wampy_args['segway_sim'] = args.sim env, robot = wampy.initialize(**wampy_args) import IPython IPython.embed()
import os import sys # exit() import openravepy import prpy import prpy.planning import wampy import roslib import rospy import numpy from tf import transformations # rotation_matrix(), concatenate_matrices() # Initialize OpenRAVE robot of type wampy.wamrobot.WAMRobot: #env, robot = wampy.initialize(sim=True, attach_viewer=True) #env, robot = wampy.initialize(sim=True, attach_viewer='qtcoin') env, robot = wampy.initialize(sim=True, attach_viewer='rviz') #env, robot = wampy.initialize(robot_xml=robot_file, sim=True, # attach_viewer='interactivemarker manipulator = robot.GetManipulator('arm') # Set 7 DOF of the arm active robot.SetActiveDOFs(manipulator.GetArmIndices()) robot.SetActiveManipulator(manipulator) # Find the pr_ordata package, which contains some object models from catkin.find_in_workspaces import find_in_workspaces package_name = 'pr_ordata' directory = 'data/objects' objects_path = find_in_workspaces(search_dirs=['share'], project=package_name,
import os import sys # exit() import openravepy import prpy import prpy.planning import wampy import roslib import rospy import numpy from tf import transformations # rotation_matrix(), concatenate_matrices() # Initialize OpenRAVE robot of type wampy.wamrobot.WAMRobot: #env, robot = wampy.initialize(sim=True, attach_viewer=True) #env, robot = wampy.initialize(sim=True, attach_viewer='qtcoin') env, robot = wampy.initialize(sim=True, attach_viewer='rviz') #env, robot = wampy.initialize(robot_xml=robot_file, sim=True, # attach_viewer='interactivemarker manipulator = robot.GetManipulator('arm') # Set 7 DOF of the arm active robot.SetActiveDOFs( manipulator.GetArmIndices() ) robot.SetActiveManipulator( manipulator ) # Find the pr_ordata package, which contains some object models from catkin.find_in_workspaces import find_in_workspaces package_name = 'pr_ordata' directory = 'data/objects' objects_path = find_in_workspaces( search_dirs=['share'],
help='robot XML file; defaults to WAM robot') parser.add_argument('--env-xml', 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) wampy_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: wampy_args['segway_sim'] = args.sim env, robot = wampy.initialize(**wampy_args) import IPython IPython.embed()
project=package_name, path=directory, first_match_only=True) if len(robots_path) == 0: print('Can\'t find directory %s/%s' % (package_name, directory)) sys.exit() else: robots_path = robots_path[0] # Use robot model of the Barett WAM arm robot_file = os.path.join(robots_path, 'barrettwam.robot.xml') # Initialize OpenRAVE robot of type wampy.wamrobot.WAMRobot: #env, robot = wampy.initialize(sim=True, attach_viewer=True) #env, robot = wampy.initialize(sim=True, attach_viewer='qtcoin') env, robot = wampy.initialize(robot_xml=robot_file, sim=True, attach_viewer='rviz') #env, robot = wampy.initialize(robot_xml=robot_file, sim=True, # attach_viewer='interactivemarker manipulator = robot.GetManipulator('arm') # Set 7 DOF of the arm active robot.SetActiveDOFs( manipulator.GetArmIndices() ) robot.SetActiveManipulator( manipulator ) # By default, the base is not at the world origin # so we move the robot to the origin trans = numpy.array([[1., 0., 0., 0.0], [0., 1., 0., 0.0], [0., 0., 1., 0.0], [0., 0., 0., 1.]])