def AttachController(self, name, args, dof_indices, affine_dofs, simulated): """ Create and attach a controller to a subset of this robot's DOFs. If simulated is False, a controller is created using 'args' and is attached to the multicontroller. In simulation mode an IdealController is created instead. Regardless of the simulation mode, the multicontroller must be finalized before use. @param name user-readable name used to identify this controller @param args real controller arguments @param dof_indices controlled joint DOFs @param affine_dofs controleld affine DOFs @param simulated simulation mode @returns created controller """ if simulated: args = 'IdealController' delegate_controller = openravepy.RaveCreateController( self.GetEnv(), args) if delegate_controller is None: type_name = args.split()[0] message = 'Creating controller {0:s} of type {1:s} failed.'.format( name, type_name) raise openravepy.openrave_exception(message) self.multicontroller.AttachController(delegate_controller, dof_indices, affine_dofs) return delegate_controller
def __init__(self, env, manip): self.name = 'herb' self.robot = env.ReadRobotXMLFile( 'models/robots/herb2_padded.robot.xml') env.Add(self.robot) right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43] left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43] right_manip = self.robot.GetManipulator('right_wam') self.robot.SetActiveDOFs(right_manip.GetArmIndices()) self.robot.SetActiveDOFValues(right_relaxed) left_manip = self.robot.GetManipulator('left_wam') self.robot.SetActiveDOFs(left_manip.GetArmIndices()) self.robot.SetActiveDOFValues(left_relaxed) if manip == 'right': self.manip = right_manip self.robot.SetActiveManipulator('right_wam') else: self.manip = left_manip self.robot.SetActiveManipulator('left_wam') self.robot.SetActiveDOFs(self.manip.GetArmIndices()) self.robot.controller = openravepy.RaveCreateController( self.robot.GetEnv(), 'IdealController')
def GetRealStartPoses(self, youbot_names, objects): # Register with tfplugin idealcontrollers = {} if self.tfplugin is None: self.tfplugin = TfPlugin(self.env, 'map') for youbot_name in youbot_names: idealcontrollers[youbot_name] = self.youbots[ youbot_name].GetController( ) # plug in the simulation controller probotcontroller = orpy.RaveCreateController( self.env, 'youbotcontroller') # plug in the real controller self.youbots[youbot_name].SetController(probotcontroller) self.tfplugin.RegisterRobotHand(or_body=self.youbots[youbot_name], tf_id=youbot_name + '_arm') for oname in objects: self.tfplugin.RegisterBody(or_body=self.env.GetKinBody(oname), tf_id=oname) #raw_input('Hit enter if start poses are updated from vicon.') # Unregister with tfplugin time.sleep(3.0) for youbot_name in youbot_names: self.tfplugin.UnregisterBody(or_body=self.youbots[youbot_name]) # set back to idealcontroller self.youbots[youbot_name].SetController( idealcontrollers[youbot_name]) for oname in objects: self.tfplugin.UnregisterBody(or_body=self.env.GetKinBody(oname))
def load_ghost(env, robotname, prefix="ref_", color=[.8, .8, .4]): """ Create a ghost robot to overlay with an existing robot in the world to show an alternate state.""" ref_robot = env.ReadRobotURI(robotname) ref_robot.SetName(prefix + ref_robot.GetName()) ref_robot.Enable(False) env.Add(ref_robot) ref_robot.SetController(_rave.RaveCreateController(env, 'mimiccontroller')) set_robot_color(ref_robot, color, color, trans=.5) return ref_robot
def load_scene(env, scenename=None, stop=False, physics=True): """ Load a robot model into the given environment, configuring a trajectorycontroller and a reference robot to show desired movements vs. actual pose. The returned tuple contains: robot: handle to the created robot controller: either trajectorycontroller or idealcontroller depending on physics name-to-joint-index converter ref_robot: handle to visiualization "ghost" robot recorder: video recorder python class for quick video dumps """ # Set the robot controller and start the simulation recorder = viewerrecorder(env) #Default to "sim-timed video" i.e. plays back much faster recorder.videoparams[0:2] = [1024, 768] recorder.realtime = False with env: if stop: env.StopSimulation() if type(scenename) is list: for n in scenename: loaded = env.Load(n) elif type(scenename) is str: loaded = env.Load(scenename) time.sleep(1) #Explicitly disable physics if option is selected with env: if not physics: env.SetPhysicsEngine( rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine')) robot = env.GetRobots()[0] pose = ones(robot.GetDOF()) * .6 robot.SetDOFValues(pose) collisionChecker = rave.RaveCreateCollisionChecker(env, 'pqp') if collisionChecker == None: collisionChecker = rave.RaveCreateCollisionChecker(env, 'ode') print 'Note: Using ODE collision checker since PQP is not available' env.SetCollisionChecker(collisionChecker) if env.GetPhysicsEngine().GetXMLId( ) != 'GenericPhysicsEngine' and physics: controller = rave.RaveCreateController(env, 'servocontroller') robot.SetController(controller) controller.SendCommand('set gains 100 0 0') controller.SetDesired(pose / pose * pi / 4 * 1.1) time.sleep(.5) ind = makeNameToIndexConverter(robot) return (robot, controller, ind, recorder)
def __init__(self, env): self.name = 'simple' self.robot = env.ReadRobotXMLFile('models/robots/pr2-beta-sim.robot.xml') env.Add(self.robot) right_relaxed = [ 0., 0., 0., -2.3, 0., 0., 0.] left_relaxed = [ 0., 0., 0., -2.3, 0., 0., 0.] right_manip = self.robot.GetManipulator('rightarm') self.robot.SetActiveDOFs(right_manip.GetArmIndices()) self.robot.SetActiveDOFValues(right_relaxed) left_manip = self.robot.GetManipulator('leftarm') self.robot.SetActiveDOFs(left_manip.GetArmIndices()) self.robot.SetActiveDOFValues(left_relaxed) self.robot.controller = openravepy.RaveCreateController(self.robot.GetEnv(), 'IdealController')
def PlayTrajWithPhysics(robot, traj, autoinit=False, waitdone=True, resetafter=False): #TODO eliminate adding controller here, potential issues switching back #Lock the environment, halt simulation in preparation env = robot.GetEnv() with env: if robot.GetController().GetXMLId() != 'trajectorycontroller': ctrl = _rave.RaveCreateController(env, 'trajectorycontroller') robot.SetController(ctrl) else: ctrl = robot.GetController() ctrl.SetPath(traj) #Resets initial pose, so now we need to set any DOF not in trajectory if autoinit: setInitialPose(robot) ctrl.SetDesired(robot.GetDOFValues()) #Eventuall make this variable? might not matter if .0005 is good _time.sleep(1) ctrl.SendCommand('start') if waitdone: t0 = _time.time() while not (ctrl.IsDone()): print "Real time {}, sim time {}".format(_time.time() - t0, ctrl.GetTime()) #Only approximate time here _time.sleep(.1) handle = plot_projected_com(robot) if resetafter: with env: ctrl.Reset() return ctrl
right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43] left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43] right_manip = robot.GetManipulator('right_wam') robot.SetActiveDOFs(right_manip.GetArmIndices()) robot.SetActiveDOFValues(right_relaxed) left_manip = robot.GetManipulator('left_wam') robot.SetActiveDOFs(left_manip.GetArmIndices()) robot.SetActiveDOFValues(left_relaxed) if args.manip == 'right': robot.SetActiveManipulator('right_wam') else: robot.SetActiveManipulator('left_wam') robot.controller = openravepy.RaveCreateController(robot.GetEnv(), 'IdealController') robot.ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not robot.ikmodel.load(): robot.ikmodel.autogenerate() robot.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( robot) if not robot.irmodel.load(): print "irmodel didnt' load. " robot.irmodel.autogenerate() # add a table and move the robot into place table = env.ReadKinBodyXMLFile('models/objects/table.kinbody.xml') env.Add(table)
def __init__(self, sim=False, viewer=False, env_xml=None, youbot_names=[], registered_objects=[], use_tfplugin=True, youbot_model='kuka-youbot.robot.xml'): self.youbotpydir = os.popen('rospack find youbotpy').read() print self.youbotpydir #RaveSetDebugLevel(0) # suppresses printing of non fatal errors env = orpy.Environment() if viewer: env.SetViewer('qtcoin') if env_xml: env.Load(env_xml) youbots = {} youbot_hands = {} for youbot_name in youbot_names: specific_filename = self.youbotpydir[:-1] + '/../models/robots/' + youbot_model[ 0:-10] + '-' + youbot_name + youbot_model[-10:] if os.path.isfile(specific_filename): print('loading robot-specific filename: %d', specific_filename) youbot = env.ReadRobotURI(specific_filename) else: youbot = env.ReadRobotURI(self.youbotpydir[:-1] + '/../models/robots/' + youbot_model) #youbot = env.ReadRobotURI(self.youbotpydir[:-1] + '/../models/robots/kuka-youbot-hires.robot.xml') youbot.SetName(youbot_name) env.Add(youbot, True) youbots[youbot_name] = youbot self.basemanips = {} for name in youbot_names: self.basemanips[name] = orpy.interfaces.BaseManipulation( youbots[name], plannername='BiRRT') tfplugin = None if not sim: if use_tfplugin: tfplugin = TfPlugin(env, 'map') for youbot_name in youbot_names: probotcontroller = orpy.RaveCreateController( env, 'youbotcontroller') youbots[youbot_name].SetController(probotcontroller) #tfplugin.RegisterBody(or_body=youbots[youbot_name],tf_id=youbot_name) if use_tfplugin: openrave_frame_in_tf_frame = np.eye(4) openrave_frame_in_tf_frame[ 2, 3] = -0.12 # difference betweeen vicon origin and openrave kinbody origin. tfplugin.RegisterBody( or_body=youbots[youbot_name], tf_id=youbot_name, openrave_frame_in_tf_frame=openrave_frame_in_tf_frame) #tfplugin.RegisterBody(or_body=youbots[youbot_name],tf_id=youbot_name,openrave_frame_in_tf_frame=openrave_frame_in_tf_frame,planar_tracking=True) #tfplugin.RegisterRobotHand(or_body=youbots[youbot_name],tf_id=youbot_name+'_arm') # Also add youbot hands that are tracked separately. detached_hands = True if detached_hands: youbot_hand = env.ReadKinBodyURI( self.youbotpydir[:-1] + '/../models/objects/youbothand.kinbody.xml') youbot_hand.SetName(youbot_name + '_arm') env.Add(youbot_hand, True) youbot_hand.Enable(False) openrave_frame_in_tf_frame = np.eye(4) openrave_frame_in_tf_frame[ 2, 3] = 0.013 # difference betweeen vicon origin and openrave kinbody origin. tfplugin.RegisterBody( or_body=youbot_hand, tf_id=youbot_hand.GetName(), openrave_frame_in_tf_frame=openrave_frame_in_tf_frame) youbot_hands[youbot_name] = youbot_hand if use_tfplugin: if not (registered_objects is None): for oname in registered_objects: tfplugin.RegisterBody(or_body=env.GetKinBody(oname), tf_id=oname) if sim and viewer: # nudge robots a little, b/c clicking on trimeshes overlapping perfectly causes openrave to crash. for name in youbots: randpose = np.eye(4) randpose[:2, 3] = np.array([random.random(), random.random()]) youbots[name].SetTransform(randpose) self.env = env self.tfplugin = tfplugin self.sim = sim self.youbots = youbots self.youbot_hands = youbot_hands self.arm_joint_offsets = np.array([2.96, 1.70, -0.55, 0.80, 4.58])
import openhubo as oh import time from numpy import pi, sqrt (env, options) = oh.setup('qtcoin', True) env.SetDebugLevel(4) #Note that the load function now directly parses the option structure if options.physics is None: options.physics = True options.scenefile = None [robot, ctrl, ind, ref, recorder] = oh.load_scene(env, options) #Change the pose to lift the elbows and send pose = oh.Pose(robot, ctrl) pose['REP'] = -pi / 2 pose['LEP'] = -pi / 2 pose.send() physics = env.GetPhysicsEngine() physics.SetGravity([0, 0, 0]) ctrl2 = _rave.RaveCreateController(env, "idealcontroller") robot.SetController(ctrl2) env.StartSimulation(timestep=oh.TIMESTEP) initialPose = robot.GetDOFValues() time.sleep(5) finalPose = robot.GetDOFValues() err = sqrt(sum(pow(initialPose - finalPose, 2)))
def openrave_init(self): self.env = openravepy.Environment() self.env.SetViewer('qtcoin') self.env.GetViewer().SetName('HPN Viewer') #self.env.Load('models/%s.env.xml' %PACKAGE_NAME) # time.sleep(3) # wait for viewer to initialize. May be helpful to uncomment self.name = 'herb' self.robot = self.env.ReadRobotXMLFile('models/robots/herb2_padded.robot.xml') self.env.Add(self.robot) right_relaxed = [ 5.65, -1.76, -0.26, 1.96, -1.15 , 0.87, -1.43 ] left_relaxed = [ 0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43 ] right_manip = self.robot.GetManipulator('right_wam') self.robot.SetActiveDOFs(right_manip.GetArmIndices()) self.robot.SetActiveDOFValues(right_relaxed) left_manip = self.robot.GetManipulator('left_wam') self.robot.SetActiveDOFs(left_manip.GetArmIndices()) self.robot.SetActiveDOFValues(left_relaxed) self.manip = right_manip self.robot.SetActiveManipulator('right_wam') self.end_effector = self.manip.GetEndEffector() self.robot.SetActiveDOFs(self.manip.GetArmIndices()) self.robot.controller = openravepy.RaveCreateController(self.robot.GetEnv(), 'IdealController') # add a table self.table = self.robot.GetEnv().ReadKinBodyXMLFile('models/data/furniture/table.kinbody.xml') self.robot.GetEnv().Add(self.table) table_pose = np.array([[ 0, 0, -1, 0.6], [-1, 0, 0, 0], [ 0, 1, 0, 0], [ 0, 0, 0, 1]]) self.table.SetTransform(table_pose) # set the camera camera_pose = np.array([[ 0.3259757 , 0.31990565, -0.88960678, 2.84039211], [ 0.94516159, -0.0901412 , 0.31391738, -0.87847549], [ 0.02023372, -0.9431516 , -0.33174637, 1.61502194], [ 0. , 0. , 0. , 1. ]]) self.robot.GetEnv().GetViewer().SetCamera(camera_pose) #add kinbodies #glass self.target_kinbody1 = self.env.ReadKinBodyURI('models/data/objects/plastic_glass.kinbody.xml') self.robot.GetEnv().Add(self.target_kinbody1) glass_pose = np.array([[ 0, 0, 0, 0.7], [-1, 0, 1, -0.5], [ 0, 1, 0, 0.7165], [ 0, 0, 0, 1]]) self.target_kinbody1.SetTransform(glass_pose) #bowl self.target_kinbody2 = self.env.ReadKinBodyURI('models/data/objects/plastic_bowl.kinbody.xml') self.robot.GetEnv().Add(self.target_kinbody2) bowl_pose = np.array([[ 0, 0, 0, 0.5], [-1, 0, 1, -0.4], [ 0, 1, 0, 0.7165], [ 0, 0, 0, 1]]) self.target_kinbody2.SetTransform(bowl_pose) #plate self.target_kinbody3 = self.env.ReadKinBodyURI('models/data/objects/plastic_plate.kinbody.xml') self.robot.GetEnv().Add(self.target_kinbody3) plate_pose = np.array([[ 0, 0, 0, 0.7], [-1, 0, 1, -0.3], [ 0, 1, 0, 0.7165], [ 0, 0, 0, 1]]) self.target_kinbody3.SetTransform(plate_pose) #add tray self.target_tray = self.env.ReadKinBodyURI('models/data/objects/wicker_tray.kinbody.xml') self.robot.GetEnv().Add(self.target_tray) tray_pose = np.array([[ 0, 1, 0, 0.6], [1, 0, 0, 0.5], [ 0, 0, 1, 0.7165], [ 0, 0, 0, 1]]) self.target_tray.SetTransform(tray_pose)
'{}/openrave-{}.{}'.format(libpath, ormaj, ormin)) path_append(os.environ, 'OPENRAVE_DATA', '{}/ordata'.format(roslib.packages.get_pkg_dir('ada_description'))) # load an openrave environment openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info) atexit.register(openravepy.RaveDestroy) e = openravepy.Environment() atexit.register(e.Destroy) e.SetViewer('qtcoin') r = e.ReadRobotXMLFile('robots/mico-modified.robot.xml') e.Add(r) c = openravepy.RaveCreateController(e, 'roscontroller openrave / 1') r.SetController(c, range(8), 0) #r.SetActiveDOFs(range(6)) time.sleep(1) # make a sweet trajectory raw_input('Press [Enter] to send trajectory ...') t = openravepy.RaveCreateTrajectory(e, '') t.Init(r.GetActiveConfigurationSpecification()) print(t.GetConfigurationSpecification()) t.Insert(0, r.GetActiveDOFValues()) if r.GetActiveDOFValues()[5] < 0.5: t.Insert(1, [0.0, -1.57, 1.57, 0.0, 0.0, 1.0, 0.0, 0.0]) else:
#!/usr/bin/python # -*- coding: utf-8 -*- import openravepy import os env = openravepy.Environment() env.SetViewer('qtcoin') env.Load('tutorial/env1.xml') #robot= env.GetRobots()[0] robot= env.GetRobot('SimpleBot') #openravepy.RaveLoadPlugin(os.getcwd()+'/libtestcontrollers.so') openravepy.RaveLoadPlugin(os.getcwd()+'/test_ctrl/build/libtestcontrollers.so') controller= openravepy.RaveCreateController(env,'TestController') robot.SetController(controller,range(robot.GetDOF()),controltransform=1) raw_input("Press Enter to start...") import time env.UpdatePublishedBodies() time.sleep(0.1) nav = openravepy.examples.simplenavigation.SimpleNavigationPlanning(robot) nav.performNavigationPlanning() raw_input("Press Enter to exit...") env.Destroy()
def load_scene_from_options(env, options): """Load a scene for openhubo based on the options structure (see setup function for detailed options).""" if hasattr(options, 'physics') and options.physics is False: #Kludge since we won't be not using ODE for a while... physics = False elif hasattr(options, 'physics') and options.physics: physics = True elif options._physics == 'ode': physics = True else: physics = False vidrecorder = _recorder(env, filename=options.recordfile) vidrecorder.videoparams[0:2] = [1024, 768] vidrecorder.realtime = False with env: if options.stop or physics: #KLUDGE: need to do this for stability reasons, not sure why, probably can be fixed another way print "Stopping OpenRAVE simulation to load models and configure..." env.StopSimulation() if type(options.scenefile) is list: for n in options.scenefile: env.Load(n) elif type(options.scenefile) is str: env.Load(options.scenefile) #This method ensures that the URI is preserved in the robot if options.robotfile is not None: robot = env.ReadRobotURI(options.robotfile) env.Add(robot) else: robot = env.GetRobots()[0] _rave.raveLogWarn("Assuming robot is {} (id=0)...".format( robot.GetName())) robot.SetDOFValues(zeros(robot.GetDOF())) #Legacy command mode if options.physicsfile == True: options.physicsfile = 'physics.xml' with env: if physics and not check_physics(env): _rave.raveLogInfo('Loading physics parameters from "{}"'.format( options.physicsfile)) env.Load(options.physicsfile) elif not physics: env.SetPhysicsEngine( _rave.RaveCreatePhysicsEngine(env, 'GenericPhysicsEngine')) else: #TODO: find a more efficient way to avoid double creation? _rave.raveLogInfo( "Physics engine already configured, overwriting...") env.Load(options.physicsfile) if check_physics(env): _rave.raveLogInfo('Creating controller for physics simulation') controller = _rave.RaveCreateController(env, 'trajectorycontroller') robot.SetController(controller) controller.SendCommand('set gains 150 0 .9 ') controller.SendCommand('set radians ') else: #Just load ideal controller if physics engine is not present _rave.raveLogInfo( 'Physics engine not loaded, using idealcontroller...') controller = _rave.RaveCreateController(env, 'idealcontroller') robot.SetController(controller) if options.ghost and options.robotfile: ghost_robot = load_ghost(env, options.robotfile, prefix="ghost_") if check_physics(env): controller.SendCommand("set visrobot " + ghost_robot.GetName()) else: ghost_robot = None collisionChecker = _rave.RaveCreateCollisionChecker(env, 'pqp') if collisionChecker == None: collisionChecker = _rave.RaveCreateCollisionChecker(env, 'ode') _rave.raveLogWarn( 'Using ODE collision checker since PQP is not available...') env.SetCollisionChecker(collisionChecker) ind = make_name_to_index_converter(robot) if options.atheight is not None: align_robot(robot, options.atheight) if ghost_robot: align_robot(ghost_robot, options.atheight) #FIXME: better way to do this? global TIMESTEP TIMESTEP = get_timestep(env) return (robot, controller, ind, ghost_robot, vidrecorder)