def initialize(model_filename='jaco', envXML=None): ''' Load and configure the JACO robot. If envXML is not None, loads environment. Returns robot and environment. ''' env = openravepy.Environment() if envXML is not None: env.LoadURI(envXML) env.SetViewer('qtcoin') # Assumes the robot files are located in the data folder of the # kinova_description package in the catkin workspace. urdf_uri = 'package://iact_control/src/data/'+model_filename+'.urdf' srdf_uri = 'package://iact_control/src/data/'+model_filename+'.srdf' or_urdf = openravepy.RaveCreateModule(env, 'urdf') robot_name = or_urdf.SendCommand('load {:s} {:s}'.format(urdf_uri, srdf_uri)) robot = env.GetRobot(robot_name) bind_subclass(robot, ArchieRobot) robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])) robot.SetDOFValues(robot_starting_dofs) viewer = env.GetViewer() viewer.SetSize(500,500) cam_params = np.array([[-0.99885711, -0.01248719, -0.0461361 , -0.18887213], [ 0.02495645, 0.68697757, -0.72624996, 2.04733515], [ 0.04076329, -0.72657133, -0.68588079, 1.67818344], [ 0. , 0. , 0. , 1. ]]) viewer.SetCamera(cam_params) #viewer.SetBkgndColor([0.8,0.8,0.8]) return env, robot
def main(): global m global e e = r.Environment() m = r.RaveCreateModule(e, 'orchomp') e.SetViewer('qtcoin') e.Load("intelkitchen_robotized_herb2_nosensors.env.xml") robot = e.GetRobot("Herb2") # set the manipulator to leftarm #ikmodel = databases.inversekinematics.InverseKinematicsModel( # robot,iktype=IkParameterization.Type.Transform6D) #if not ikmodel.load(): # ikmodel.autogenerate() Tz = r.matrixFromAxisAngle([0, 0, numpy.pi / 2]) Tz[0, 3] = 0.4 Tz[1, 3] = 1.6 Tz[2, 3] = -0.01 print Tz with e: for body in e.GetBodies(): body.SetTransform(numpy.dot(Tz, body.GetTransform())) readFileCommands()
def main(): global e global m global robot global knownstates e = r.Environment() m = r.RaveCreateModule(e, 'orcdchomp') e.SetViewer('qtcoin') #e.Load( 'lab1.env.xml' ); e.Load('herb2_padded_nosensors.robot.xml') robot = e.GetRobot('Herb2') knownstates = readKnownStates("data/knownstates.data") printKnownStates(knownstates) return #robot.SetActiveManipulator( 0 ) filename = "randomstates2.data" #filename = "randomManipulatorStates_0.data" userGetRandomStates(filename) return
def __init__(self, env, robotname): """ Create a ElasticStrips Problem Instance :param env: rave.Environment :param robotname: str :return: ElasticStrips """ print('ElasticStrips.py initialize') self.module = rave.RaveCreateModule(env, 'ElasticStrips') env.AddModule(self.module, '') self.env = env self.robotname = robotname # self.problem = rave.RaveCreateProblem(env, 'CBiRRT') # env.LoadProblem(self.problem, robotname) # Build a mapping for manipulators to manipulator indices self.manip_indices = {} for index, manip in enumerate( env.GetRobot(robotname).GetManipulators()): self.manip_indices[manip] = index self.manip_indices[manip.GetName()] = index
def setUp(self): self.env = openravepy.Environment() urdf_module = openravepy.RaveCreateModule(self.env, 'urdf') with self.env: arg = 'load {:s}'.format(urdf_path) bh_name = urdf_module.SendCommand(arg) self.bh_body = self.env.GetKinBody(bh_name)
def __init__(self, env, urdf_path, srdf_path): self.env = env # Load robot module = rave.RaveCreateModule(self.env, 'urdf') robot_name = module.SendCommand('load {} {}'.format( urdf_path, srdf_path)) self.robot = self.env.GetRobot(robot_name) self.manip = AttributePassthrough(self.robot.GetManipulator, self.robot.GetManipulators)
def initialize(env_path=None, attach_viewer=False, **kw_args): from adarobot import ADARobot from util import AdaPyException, find_adapy_resource prpy.logger.initialize_logging() # Create the environment. env = openravepy.Environment() if env_path is not None: if not env.Load(env_path): raise IOError( 'Unable to load environment "{:s}".'.format(env_path)) # Use or_urdf to load ADA from URDF and SRDF. with env: or_urdf = openravepy.RaveCreateModule(env, 'urdf') ada_name = or_urdf.SendCommand('load {:s} {:s}'.format( URDF_PATH, SRDF_PATH)) robot = env.GetRobot(ada_name) if robot is None: raise AdaPyException('Failed loading ADA with or_urdf.') # Bind AdaPy-specific functionality on the robot. prpy.bind_subclass(robot, ADARobot, **kw_args) # 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 rviz failed. Falling back on qt_coin.') attach_viewer = 'qtcoin' if attach_viewer and env.GetViewer() is None: #Using qtcoin as or_rviz gives seg fault attach_viewer = 'qtcoin' env.SetViewer(attach_viewer) if env.GetViewer() is None: raise AdaPyException( 'Failed creating viewer of type "{: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() import adapy.action # register actions import adapy.tsr # register TSR libraries robot.actions = prpy.action.ActionLibrary() return env, robot
def initialize(model_filename='jaco_dynamics', visualize=True, envXML=None): ''' Load and configure the JACO robot. If envXML is not None, loads environment. Returns robot and environment. ----- NOTE: IF YOU JUST WANT TO DO COMPUTATIONS THROUGH OPENRAVE AND WANT MULTPILE INSTANCES TO OPEN, THEN HAVE TO TURN OFF QTCOIN AND ALL VIEWER FUNCTIONALITY OR IT WILL CRASH. ''' env = openravepy.Environment() if envXML is not None: env.LoadURI(envXML) if visualize: env.SetViewer('qtcoin') here = os.path.dirname(os.path.realpath(__file__)) urdf_uri = '../resources/' + model_filename + '.urdf' srdf_uri = '../resources/' + model_filename + '.srdf' or_urdf = openravepy.RaveCreateModule(env, 'urdf') if or_urdf 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.') robot_name = or_urdf.SendCommand('load {:s} {:s}'.format( urdf_uri, srdf_uri)) if robot_name is None: raise ValueError('Failed loading robot model using or_urdf.') robot = env.GetRobot(robot_name) if robot is None: raise ValueError( 'Unable to find robot with name "{:s}".'.format(robot_name)) bind_subclass(robot, ArchieRobot) robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])) robot.SetDOFValues(robot_starting_dofs) if visualize: viewer = env.GetViewer() viewer.SetSize(700, 500) cam_params = np.array( [[-0.99885711, -0.01248719, -0.0461361, -0.18887213], [0.02495645, 0.68697757, -0.72624996, 2.04733515], [0.04076329, -0.72657133, -0.68588079, 1.67818344], [0., 0., 0., 1.]]) viewer.SetCamera(cam_params) return env, robot
def start_recording(self, fname='output.mpg', codec=13, framerate=24, width=640, height=480): vname = self.env.GetViewer().GetName() cmd = 'Start %d %d %d codec %d timing simtime filename %s\nviewer %s' cmd = cmd % (width, height, framerate, codec, fname, vname) recorder = openravepy.RaveCreateModule(self.env, 'viewerrecorder') self.env.AddModule(recorder, '') recorder.SendCommand(cmd) self.recorder = recorder
def get_chomp_module(env): if args.planner == "chomp": from orcdchomp import orcdchomp chomp_module_path = "/home/jonathan/build/chomp/liborcdchomp.so" elif args.planner == "chomp2": from orcdchomp2 import orcdchomp chomp_module_path = "/home/jonathan/code/orcdchomp2/lib/liborcdchomp2.so" else: assert False openravepy.RaveLoadPlugin(chomp_module_path) m_chomp = openravepy.RaveCreateModule(env, 'orcdchomp') env.Add(m_chomp, True, 'blah_load_string') orcdchomp.bind(m_chomp) return m_chomp
def initialize(): '''Load and configure the Archie robot. Returns robot and environment.''' env = openravepy.Environment() env.SetViewer('qtcoin') # Assumes the robot files are located in the urdf folder of the # kinova_description package in the catkin workspace. #env.Load('lab.xml') urdf_uri = 'package://interactpy/jaco_dynamics.urdf' srdf_uri = 'package://interactpy/jaco_dynamics.srdf' objects_path = find_in_workspaces(project='interactpy', path='envdata', first_match_only=True)[0] or_urdf = openravepy.RaveCreateModule(env, 'urdf') robot_name = or_urdf.SendCommand('load {:s} {:s}'.format( urdf_uri, srdf_uri)) robot = env.GetRobot(robot_name) bind_subclass(robot, ArchieRobot) # Put robot in natural starting config. # Load environment #env.Load('{:s}/table.xml'.format(objects_path)) env.Load('{:s}/mug1.kinbody.xml'.format(objects_path)) # table = env.GetKinBody('table')J # table.SetTransform(np.array([[1.00000000e+00, -2.79931237e-36, 1.41282351e-14, 1.50902510e-01], # [-2.07944729e-28, 1.00000000e+00, 1.47183799e-14, -1.47385532e-02], # [-1.41282351e-14, -1.47183799e-14, 1.00000000e+00, -1.00134850e-01], # [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])) mug = env.GetKinBody('mug') # mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, 6.57676935e-01], # [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, -3.07691471e-06], # [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, 5.46909690e-01], # [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])) #mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -3.00000000e-01], # [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 1.00000000e+00], # [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01], # [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])) # mug.SetTransform(np.array([[1.00000000e+00, 0.00000000e+00, 0.00000000e+00, -0.10000000], # [0.00000000e+00, 0.00000000e+00, -1.00000000e+00, 0.20000000e+00], # [0.00000000e+00, 1.00000000e+00, 0.00000000e+00, -1.00000000e-01], # [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])) mug.SetTransform([[1, 0, 0, -0.4], [0, 0, -1, 0.3], [0, 1, 0, -0.1]]) #mug.SetTransform([[1,0,0,-0.6],[0,0,-1,-0.6],[0,1,0,.7]]) robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])) robot.SetDOFValues(robot_starting_dofs) return env, robot, mug
def initialize(self, robot): # construct a new module/planner if necessary if self.module is None: self.module = openravepy.RaveCreateModule(self.env, 'Family') if self.module is None: raise prpy.planning.base.UnsupportedPlanningError( 'Unable to create Family module.') if self.planner is None: self.planner = openravepy.RaveCreatePlanner( self.env, 'FamilyPlanner') if self.planner is None: raise prpy.planning.base.UnsupportedPlanningError( 'Unable to create FamilyPlanner planner.')
def __init__(self): super(CHOMPPlanner, self).__init__() try: from orchomp import orchomp self.module = openravepy.RaveCreateModule(self.env, 'orchomp') except ImportError: raise UnsupportedPlanningError('Unable to import orchomp.') except openravepy.openrave_exception as e: raise UnsupportedPlanningError( 'Unable to create orchomp module: %s' % e) if self.module is None: raise UnsupportedPlanningError('Failed loading module.') self.initialized = False orchomp.bind(self.module)
def initialize(): '''Load and configure the Archie robot. Returns robot and environment.''' env = openravepy.Environment() env.SetViewer('qtcoin') # Assumes the robot files are located in the urdf folder of the # kinova_description package in the catkin workspace. urdf_uri = 'package://interactpy/jaco.urdf' srdf_uri = 'package://interactpy/jaco.srdf' or_urdf = openravepy.RaveCreateModule(env, 'urdf') robot_name = or_urdf.SendCommand( 'load {:s} {:s}'.format(urdf_uri, srdf_uri)) robot = env.GetRobot(robot_name) bind_subclass(robot, ArchieRobot) # Put robot in natural starting config. # TODO(allanzhou): Decide if this is really necessary. robot.SetDOFValues([0, 3, 0, 2, 0, 4, 0, 0, 0, 0]) return env, robot
def runtrial(Tz, useArm=False, useTwoArms=False, view=False): global e global m e = r.Environment() m = r.RaveCreateModule(e, 'orchomp') if view: e.SetViewer('qtcoin') #e.Load( 'lab1.env.xml') e.Load("intelkitchen_robotized_herb2_nosensors.env.xml") robot = e.GetRobot("Herb2") """ with e: for body in e.GetBodies(): body.SetTransform(numpy.dot(Tz,body.GetTransform())) """ e.Load("herb_base.kinbody.xml") herb_body = filter(lambda x: x.GetName() == "herb_base", e.GetBodies())[0] with e: robot.SetTransform(numpy.dot(Tz, robot.GetTransform())) herb_body.SetTransform(robot.GetTransform()) #e.Load( "herb2_padded_nosensors.robot.xml" ) if useTwoArms: dof1 = list(robot.GetManipulators()[0].GetArmIndices()) dof1 += list(robot.GetManipulators()[1].GetArmIndices()) robot.SetActiveDOFs(dof1) print dof1 if useArm and not useTwoArms: robot.SetActiveDOFs(robot.GetManipulators()[0].GetArmIndices()) m.SendCommand("computedistancefield getall robot Herb2") e.Remove(herb_body) m.SendCommand("benchmark n 10000") e.Destroy() m.Destroy()
def deserialize_environment(data, env=None, purge=False, reuse_bodies=None): import openravepy if env is None: env = openravepy.Environment() if reuse_bodies is None: reuse_bodies = [] reuse_bodies_dict = { body.GetName(): body for body in reuse_bodies } reuse_bodies_set = set(reuse_bodies) # Release anything that's grabbed. for body in reuse_bodies: body.ReleaseAllGrabbed() # Remove any extra bodies from the environment. for body in env.GetBodies(): if body not in reuse_bodies_set: deserialization_logger.debug('Purging body "%s".', body.GetName()) env.Remove(body) # Create a or_ordf module on demand urdf_module_getter = UnitaryMemoizer(lambda: openravepy.RaveCreateModule(env,'urdf')) # Deserialize the kinematic structure. deserialized_bodies = [] for body_data in data['bodies']: body = reuse_bodies_dict.get(body_data['name'], None) if body is None: body = deserialize_kinbody(env, body_data, state=False, urdf_module_getter=urdf_module_getter) deserialization_logger.debug('Deserialized body "%s".', body.GetName()) deserialized_bodies.append((body, body_data)) # Restore state. We do this in a second pass to insure that any bodies that # are grabbed already exist. for body, body_data in deserialized_bodies: deserialize_kinbody_state(body, body_data['kinbody_state']) if body.IsRobot(): deserialize_robot_state(body, body_data['robot_state']) return env
def setupEnv(self, env): from types import MethodType self.env = env try: from orcdchomp import orcdchomp module = openravepy.RaveCreateModule(self.env, 'orcdchomp') except ImportError: raise UnsupportedPlanningError( 'Unable to import "orcdchomp". Is or_cdchomp installed?') if module is None: raise UnsupportedPlanningError( 'Failed loading "orcdchomp" module') if module is None: raise UnsupportedPlanningError( 'Failed loading "orcdchomp" module. Is or_cdchomp installed?') # This is a hack to prevent leaking memory. class CHOMPBindings(object): pass self.module = CHOMPBindings() self.module.module = module self.module.viewspheres =\ MethodType(orcdchomp.viewspheres, module) self.module.computedistancefield =\ MethodType(orcdchomp.computedistancefield, module) self.module.addfield_fromobsarray =\ MethodType(orcdchomp.addfield_fromobsarray, module) self.module.removefield = MethodType(orcdchomp.removefield, module) self.module.create = MethodType(orcdchomp.create, module) self.module.iterate = MethodType(orcdchomp.iterate, module) self.module.gettraj = MethodType(orcdchomp.gettraj, module) self.module.destroy = MethodType(orcdchomp.destroy, module) self.module.runchomp = MethodType(orcdchomp.runchomp, module) self.module.GetEnv = self.module.module.GetEnv # Create a DistanceFieldManager to track which distance fields are # currently loaded. self.distance_fields = DistanceFieldManager( self.module, require_cache=self.require_cache)
def main(): global m global e e = r.Environment() m = r.RaveCreateModule(e, 'orchomp') e.SetViewer('qtcoin') v = e.GetViewer() start = [ 5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43, -1.66533454e-16, -1.66533454e-16, -1.38777878e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00 ] Tz = numpy.array([[-0.81596105, 0.24022406, -0.52583264, 1.39736962], [0.57709228, 0.28460075, -0.76548476, 1.4022727], [-0.03423549, -0.9280597, -0.37085458, 1.57485259], [0., 0., 0., 1.]]) v.SetCamera(Tz) e.Load("herb2_padded_nosensors.robot.xml") robot = e.GetRobot("Herb2") with e: robot.SetName("original") e.Load("herb2_padded_nosensors.robot.xml") robot2 = e.GetRobot("Herb2") with e: robot.SetDOFValues(start) robot2.SetDOFValues(start) """ for link in robot.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency( 0.3 ) """ readFileCommands()
def __init__(self): self.env = openravepy.Environment() rospack = rospkg.RosPack() package_path = rospack.get_path('jaco_manipulation') jaco_urdf_path = package_path + '/config/jaco.urdf' jaco_srdf_path = package_path + '/config/jaco.srdf' rospy.loginfo("Loading Jaco URDF from {} and SRDF from {}...".format( jaco_urdf_path, jaco_srdf_path)) self.urdf_module = openravepy.RaveCreateModule(self.env, 'urdf') name = self.urdf_module.SendCommand("load {} {}".format( jaco_urdf_path, jaco_srdf_path)) self.jaco = self.env.GetRobot(name) # self.finger_joint_values = [0.0, 0.0, 0.0] self.finger_joint_values = [1.0, 1.0, 1.0] self.joint_names = ['j2s7s300_joint_{}'.format(i) for i in range(1, 8)] self.trajopt_num_waypoints = 30 self.dt = 0.2 #time between waypoints to be added in post processing self.cost_functions = []
def initialize(model_filename='jaco', envXML=None, viewer=True): ''' Load and configure the JACO robot. If envXML is not None, loads environment. Returns robot and environment. ----- NOTE: IF YOU JUST WANT TO DO COMPUTATIONS THROUGH OPENRAVE AND WANT MULTPILE INSTANCES TO OPEN, THEN HAVE TO TURN OFF QTCOIN AND ALL VIEWER FUNCTIONALITY OR IT WILL CRASH. ''' env = openravepy.Environment() if envXML is not None: env.LoadURI(envXML) # Assumes the robot files are located in the data folder of the # kinova_description package in the catkin workspace. urdf_uri = 'package://iact_control/src/data/' + model_filename + '.urdf' srdf_uri = 'package://iact_control/src/data/' + model_filename + '.srdf' or_urdf = openravepy.RaveCreateModule(env, 'urdf') robot_name = or_urdf.SendCommand('load {:s} {:s}'.format( urdf_uri, srdf_uri)) robot = env.GetRobot(robot_name) bind_subclass(robot, ArchieRobot) robot.SetActiveDOFs(np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])) robot.SetDOFValues(robot_starting_dofs) if viewer: env.SetViewer('qtcoin') viewer = env.GetViewer() viewer.SetSize(700, 500) cam_params = np.array( [[-0.99885711, -0.01248719, -0.0461361, -0.18887213], [0.02495645, 0.68697757, -0.72624996, 2.04733515], [0.04076329, -0.72657133, -0.68588079, 1.67818344], [0., 0., 0., 1.]]) viewer.SetCamera(cam_params) return env, robot
def main(): global m global e e = r.Environment() m = r.RaveCreateModule(e, 'orchomp') e.SetViewer('qtcoin') e.Load("intelkitchen_robotized_herb2_nosensors_2.env.xml") robot = e.GetRobot("Herb2") start = [ 3.68000000e+00, -1.90000000e+00, 0.00000000e+00, 2.20000000e+00, -2.22044605e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 2.60000000e+00, -1.90000000e+00, 0.00000000e+00, 2.20000000e+00, 0.00000000e+00, -1.11022302e-16, 0.00000000e+00, -1.66533454e-16, -1.66533454e-16, -1.38777878e-16, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00 ] robot.SetDOFValues(start) readFileCommands()
def __init__(self, path=None, xml=None, use_urdf=False): assert path is not None or xml is not None env = get_openrave_env() if use_urdf: urdf_path = path[0] srdf_path = path[1] module = openravepy.RaveCreateModule(env, 'urdf') name = module.SendCommand('load {} {}'.format( urdf_path, srdf_path)) else: name = basename(splitext(path)[0]) if xml is None: xml = Robot.__default_xml % (path, name) env.LoadData(xml) rave = env.GetRobot(name) nb_dofs = rave.GetDOF() q_min, q_max = rave.GetDOFLimits() rave.SetDOFVelocityLimits([1000.] * nb_dofs) self.has_free_flyer = False self.ik = None self.is_visible = True self.mass = sum([link.GetMass() for link in rave.GetLinks()]) self.nb_dofs = nb_dofs self.q_max = q_max self.q_max.flags.writeable = False self.q_min = q_min self.q_min.flags.writeable = False self.qd_lim = rave.GetDOFVelocityLimits() self.rave = rave self.stance = None self.tau_max = None # set by hand in child robot class self.transparency = 0. # initially opaque # self.ik = IKSolver(self)
def environment_setup(self): self.env = openravepy.Environment() InitOpenRAVELogging() module = openravepy.RaveCreateModule(self.env, 'urdf') rospack = rospkg.RosPack() with self.env: # name = module.SendCommand('load /home/peppe/python_test/sawyer.urdf /home/peppe/python_test/sawyer_base.srdf') name = module.SendCommand('loadURI ' + rospack.get_path('fredsmp_utils') + '/robots/sawyer/sawyer.urdf' + ' ' + rospack.get_path('fredsmp_utils') + '/robots/sawyer/sawyer.srdf') self.robot = self.env.GetRobot(name) time.sleep(0.5) self.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( self.robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not self.ikmodel.load(): self.ikmodel.autogenerate() manip = self.robot.GetActiveManipulator() self.robot.SetActiveDOFs(manip.GetArmIndices())
#!/usr/bin/env python import os import openravepy import numpy as np import time from openravepy.misc import InitOpenRAVELogging InitOpenRAVELogging() openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Error) env = openravepy.Environment() plugin = openravepy.RaveCreateModule(env, "urdf") env.SetViewer('qtcoin') with env: name = plugin.SendCommand('load real_sawyer_urdf_rounded.xml real_sawyer.srdf') robot = env.GetRobot(name) robot.SetActiveManipulator('arm') ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D ) if not ikmodel.load(): ikmodel.autogenerate() m = robot.GetActiveManipulator() robot.SetActiveDOFs(m.GetArmIndices())
def __init__(self, env): self.module = rave.RaveCreateModule(env, 'TraversabilityCPPUtility') env.AddModule(self.module, '') self.env = env
yamldict = yaml.safe_load(open(args.logfile)) # load environment for kbdict in yamldict['environment']['kinbodies'].values(): name = kbdict['name'] uri = kbdict['uri'] # load/add kinbody if kbdict['is_robot']: if uri.endswith('.robot.xml'): kb = env.ReadRobotXMLFile('robots/barrettwam.robot.xml') kb.SetName(name) env.Add(kb) else: print('uri:', uri) urdf,srdf = uri.split() m_urdf = openravepy.RaveCreateModule(env, 'urdf') robot_name = m_urdf.SendCommand('Load {:s} {:s}'.format(urdf,srdf)) if robot_name != name: raise RuntimeError('error, or_urdf name mismatch!') kb = env.GetRobot(robot_name) kb.SetActiveDOFs(kbdict['robot_state']['active_dof_indices']) kb.SetActiveManipulator(kbdict['robot_state']['active_manipulator']) else: kb = e.ReadKinBodyXMLFile(uri) kb.SetName(name) env.Add(kb) # transform txdict = kbdict['kinbody_state']['transform'] orpose = txdict['orientation'] + txdict['position'] kb.SetTransform(orpose) # dof values
import openravepy as rave from openravepy.misc import InitOpenRAVELogging import numpy as np InitOpenRAVELogging() env = rave.Environment() env.SetViewer('qtcoin') module = rave.RaveCreateModule(env, 'urdf') name = module.SendCommand( 'load /home/ubuntu/catkin_ws/src/j2s7s300_standalone.urdf /home/ubuntu/catkin_ws/src/jaco7dof_standalonev1.srdf' ) jaco = env.GetRobot(name) # kb = env.GetKinBody(name) #print(dir(jaco)) print(jaco.GetManipulators()) jaco.SetActiveManipulator("j2s7s300") ikmodel = rave.databases.inversekinematics.InverseKinematicsModel( jaco, iktype=rave.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() with env: Tgoal = np.array([[0, -0.1, 0.0, -0.0], [-0.1, 0, 0, 0.0, 0.0], [0.0, 0.0, -0.1, 0.0], [0.0, 0.0, 0.0, 1.0]]) sol = ikmodel.manip.FindIKSolution( Tgoal, rave.IkFilterOptions.CheckEnvCollisions) # get collision-free solution with jaco: # save robot state jaco.SetDOFValues(
def load_module(env, modname, modcmd=''): m = openravepy.RaveCreateModule(env, modname) if not m: return None env.AddModule(m, modcmd) return m
import math import numpy import openravepy import orcdchomp.orcdchomp # start openrave openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) atexit.register(openravepy.RaveDestroy) e = openravepy.Environment() atexit.register(e.Destroy) e.SetViewer('qtcoin') # load the orcdchomp module openravepy.RaveLoadPlugin('planner_plugin/build/chomp/chomp') m_chomp = openravepy.RaveCreateModule(e, 'orcdchomp') if not m_chomp: raise RuntimeError('no chomp module found!') orcdchomp.orcdchomp.bind(m_chomp) # table table = e.ReadKinBodyXMLFile('models/furniture/rolly-table.iv') e.Add(table) table.SetTransform([0.70711, 0.70711, 0, 0, 0, 0, 0]) # bottle (and its grasp) mug = e.ReadKinBodyXMLFile('models/objects/mug3.iv') e.Add(mug) mug.SetTransform([1, 0, 0, 0, 0, 0, 0.7]) T_mug_palm = numpy.array([[0, -1, 0, 0.000], [0, 0, -1, 0.075], [1, 0, 0, 0.100], [0, 0, 0, 1]])
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) atexit.register(openravepy.RaveDestroy) env = openravepy.Environment() atexit.register(env.Destroy) robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml') env.Add(robot) robot.SetActiveManipulator('arm') robot.SetActiveDOFs(robot.GetActiveManipulator().GetArmIndices()) # address this problem q_start = [-2.0, -0.5, -0.2, -0.5, 0.0, 0.0, 0.0 ] q_goal = [ 2.0, 0.5, 0.2, 0.5, 0.0, 0.0, 0.0 ] family = openravepy.RaveCreateModule(env, 'Family') env.Add(family, False, '--robot-name=BarrettWAM') # save self-check family.SendCommand('Let Self = $live') # load up a box box = openravepy.RaveCreateKinBody(env,'') box.SetName('box') aabbs = numpy.zeros((0,6)) aabb = numpy.array([[0,0,0, 0.1,0.1,0.1]]) aabbs = numpy.vstack((aabbs,aabb)) box.InitFromBoxes(aabbs,True) env.Add(box) box.SetTransform([1.,0.,0.,0., 0.,0.,0.75])