def MoveArmTo(robot,goal,planner): try: orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose) with env: robot.SetActiveDOFs(range(5)) traj = planner.MoveActiveJoints(goal=goal,maxiter=5000,steplength=0.001,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.00025) robot.GetController().SetPath(traj) while not robot.GetController().IsDone(): time.sleep(0.01) finally: orpy.RaveSetDebugLevel(orpy.DebugLevel.Info)
def MoveBaseTo(robot,xyyaw,planner,skip_waypoints=False): print "move" print robot print xyyaw try: orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose) robot.GetEnv().GetKinBody('floor').Enable(False) current = robot.GetTransform() currentxyyaw = np.array([current[0,3],current[1,3],np.arctan2(current[1,0],current[0,0])]) if np.linalg.norm(currentxyyaw-xyyaw) < 0.0001: print robot.GetName(),' already at goal. Not moving.' return with env: robot.SetActiveDOFs([],orpy.DOFAffine.X|orpy.DOFAffine.Y|orpy.DOFAffine.RotationAxis,[0,0,1]) traj = planner.MoveActiveJoints(goal=xyyaw,maxiter=5000,steplength=0.001,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.00025) print 'trajectory' print traj if skip_waypoints: traj.Remove(1,traj.GetNumWaypoints()-1) robot.GetController().SetPath(traj) while not robot.GetController().IsDone(): time.sleep(0.01) except Exception, e: print str(e) c = raw_input('IPython shell?(y/n)') if c == 'y': IPython.embed() raise e
def __init__(self, args): # Initialize values self.busy = False self.topic_name = args.topic self.file_name = args.file_name self.octomap_resolution = str(args.resolution) self.octomap_range = str(args.range) self.octomap_frame = args.frame # env_name = '' # TODO: # robot_name = '' # TODO: # obstacle = '' # TODO: self.durations = [] # Initialize OpenRAVE environment self.env = orpy.Environment() self.env.SetViewer('qtcoin') orpy.RaveSetDebugLevel(orpy.DebugLevel.Error) # self.env.Load(env_name) # Initialize ROS subscribers and publisher self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array', MarkerArray, callback=self.process_octomap, queue_size=1) # Initialize octomap plugin orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin self.sensor_server = orpy.RaveCreateSensorSystem(self.env, "or_octomap") self.sensor_server.SendCommand("Enable") # Enable octomap thread self.sensor_server.SendCommand("ResetTopic " + self.topic_name) # Reset the registered pcl topic self.sensor_server.SendCommand("ResetResolution " + self.octomap_resolution) # Reset the octomap resolution self.sensor_server.SendCommand("ResetRange " + self.octomap_range) # Reset the octomap max range self.sensor_server.SendCommand("ResetFrameID " + self.octomap_frame) # Reset the octomap frame self.time_start = time.time()
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 setUpClass(cls): np.set_printoptions(precision=6, suppress=True) env = orpy.Environment() if not env.Load('data/lab1.env.xml'): raise Exception('Could not load scene: data/lab1.env.xml') robot = env.GetRobot('BarrettWAM') np.random.seed(123) q = ru.kinematics.random_joint_values(robot) robot.SetActiveDOFValues(q) orpy.RaveSetDebugLevel(orpy.DebugLevel.Fatal) print('') # dummy line cls.env = env cls.robot = robot
def __init__(self, args): # Initialize values self.busy = False self.topic_name = args.topic rospack = rospkg.RosPack() path = rospack.get_path('or_octomap_plugin') + '/tests/' self.file_name = path + args.file_name self.octomap_resolution = str(args.resolution) self.octomap_range = args.range self.octomap_frame = args.frame self.timeout = args.timeout self.pause = False # self.env_name = '' # TODO: # self.robot_name = '' # TODO: # self.obstacle_name = '' # TODO: self.durations = [] # Initialize OpenRAVE environment self.env = orpy.Environment() self.env.SetViewer('qtcoin') orpy.RaveSetDebugLevel(orpy.DebugLevel.Error) # self.env.Load(self.env_name) # Initialize ROS subscribers and publisher self.sub_octomap = rospy.Subscriber('/occupied_cells_vis_array', MarkerArray, callback=self.process_octomap, queue_size=1) # Initialize octomap plugin orpy.RaveLoadPlugin("or_octomap") # Load octomap plugin self.sensor_server = orpy.RaveCreateSensorSystem( self.env, "or_octomap") self.sensor_server.SendCommand("Enable") # Enable octomap thread self.sensor_server.SendCommand( "ResetTopic " + self.topic_name) # Reset the registered pcl topic self.sensor_server.SendCommand( "ResetResolution " + self.octomap_resolution) # Reset the octomap resolution self.sensor_server.SendCommand( "ResetFrameID " + self.octomap_frame) # Reset the octomap frame # Initialize dynamic reconfigure server self.server_d = dynamic_reconfigure.server.Server( OctomapServerConfig, self.cb_reconfig_server, '') # Reset octomap server parameters self.sensor_server.SendCommand('TogglePause') rospy.sleep(0.1) rospy.set_param('or_octomap/sensor_model_max_range', self.octomap_range) self.sensor_server.SendCommand('Reset') self.time_start = time.time()
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)
def chomp_plan(robot, group_name, active_joint_names, active_affine, target_dof_values, init_trajs): datadir = 'chomp_data' n_points = args.n_steps assert active_affine == 0 or active_affine == 11 use_base = active_affine == 11 saver = openravepy.RobotStateSaver(robot) target_base_pose = None if use_base: with robot: robot.SetActiveDOFValues(target_dof_values) target_base_pose = openravepy.poseFromMatrix(robot.GetTransform()) robot.SetActiveDOFs( robot.GetActiveDOFIndices(), 0) # turn of affine dofs; chomp takes that separately target_dof_values = target_dof_values[:-3] # strip off the affine part openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn) m_chomp = get_chomp_module(robot.GetEnv()) env_hash = hash_env(robot.GetEnv()) if active_affine != 0: env_hash += "_aa" + str(active_affine) # load distance field j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()] for link in robot.GetLinks(): for j1idx in j1idxs: if robot.DoesAffect(j1idx, link.GetIndex()): link.Enable(False) try: aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume m_chomp.computedistancefield(kinbody=robot, aabb_padding=aabb_padding, cache_filename='%s/chomp-sdf-%s.dat' % (datadir, env_hash)) except Exception, e: print 'Exception in computedistancefield:', repr(e)
def MoveBaseTo(robot, goal, planner, skip_waypoints=False): try: orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose) robot.GetEnv().GetKinBody('floor').Enable(False) xyyaw = np.array( [goal[0, 3], goal[1, 3], np.arctan2(goal[1, 0], goal[0, 0])]) current = robot.GetTransform() currentxyyaw = np.array([ current[0, 3], current[1, 3], np.arctan2(current[1, 0], current[0, 0]) ]) if np.linalg.norm(currentxyyaw - xyyaw) < 0.0001: print robot.GetName(), ' already at goal. Not moving.' return with env: robot.SetActiveDOFs([], orpy.DOFAffine.X | orpy.DOFAffine.Y | orpy.DOFAffine.RotationAxis, [0, 0, 1]) traj = planner.MoveActiveJoints(goal=xyyaw, maxiter=5000, steplength=0.15, maxtries=2, execute=False, outputtrajobj=True, jitter=-0.01) if skip_waypoints: traj.Remove(1, traj.GetNumWaypoints() - 1) robot.GetController().SetPath(traj) while not robot.GetController().IsDone(): time.sleep(0.01) except Exception, e: print str(e) if sim: print 'Snapping to goal.' robot.SetTransform(goal) else: MoveBack(robot, planner)
# not be available if the user manually set the OPENRAVE_DATA environmental # variable, e.g. through openrave_catkin. try: share_path = subprocess.check_output(['openrave-config', '--share-dir']).strip() os.environ['OPENRAVE_DATA'] = os.path.join(share_path, 'data') except subprocess.CalledProcessError as e: print( 'error: Failed using "openrave-config" to find the default' ' OPENRAVE_DATA path. Loading assets may fail.', file=sys.stderr) # Initialize OpenRAVE. openravepy.RaveInitialize(True) openravepy.misc.InitOpenRAVELogging() openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Fatal) class PlannerTestsMeta(type): PLANNER_NAMES = [ "BKPIECE1", "EST", "KPIECE1", "LazyRRT", "LBKPIECE1", "PRM", "LazyPRM", "PRMstar", "RRT", "RRTConnect", "SBL",
bar.update(count) bar.finish() return if __name__ == '__main__': args = parse_args() np.set_printoptions(precision=6, suppress=True) cu.logger.initialize_logging(format_level=logging.INFO) # Load the OpenRAVE environment world_xml = 'worlds/airbus_challenge.env.xml' env = orpy.Environment() if not env.Load(world_xml): logger.error('Failed to load: {0}'.format(world_xml)) raise IOError orpy.RaveSetDebugLevel(orpy.DebugLevel.Fatal) # Setup robot and manip robot = env.GetRobot('robot') manip = robot.SetActiveManipulator('drill') robot.SetActiveDOFs(manip.GetArmIndices()) qhome = np.deg2rad([0, -20, 130, 0, 70, 0.]) with env: robot.SetActiveDOFValues(qhome) # Load IKFast and links stats iktype = orpy.IkParameterizationType.Transform6D if not ru.kinematics.load_ikfast(robot, iktype): logger.error('Failed to load IKFast {0}'.format(iktype.name)) exit() success = ru.kinematics.load_link_stats(robot, xyzdelta=0.01) # The robot velocity limits are quite high in the model robot.SetDOFVelocityLimits([1., 0.7, 0.7, 1., 0.7, 1.57])
while not robot.GetController().IsDone(): time.sleep(0.01) except Exception, e: print str(e) if sim: print 'Snapping to goal.' robot.SetTransform(goal) else: MoveBack(robot, planner) #c = raw_input('IPython shell?(y/n)') #if c == 'y': # IPython.embed() #raise e finally: robot.GetEnv().GetKinBody('floor').Enable(True) orpy.RaveSetDebugLevel(orpy.DebugLevel.Info) def getArmNorm(traj, robot): traj_start = traj.GetWaypoint(0) traj_end = traj.GetWaypoint(traj.GetNumWaypoints() - 1) #get last waypoint first_config = traj.GetConfigurationSpecification().ExtractJointValues( traj_start, robot, robot.GetActiveDOFIndices()) last_config = traj.GetConfigurationSpecification().ExtractJointValues( traj_end, robot, robot.GetActiveDOFIndices()) return np.linalg.norm(first_config - last_config, np.inf) #supnorm def MoveArmTo(robot, goal, planner):
rob.GetLink('link5').Enable(enable) def EnableGrabbedObjects(env,robot_name,enable): with env: rob = env.GetRobot(robot_name) for o in rob.GetGrabbed(): o.Enable(enable) <<<<<<< .mine def MoveBaseTo(robot,goal,planner,skip_waypoints=False): ======= def MoveBack(robot,planner): >>>>>>> .r1790 try: <<<<<<< .mine orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose) ======= EnableGripperLinks(robot.GetEnv(),robot.GetName(),False) goal = robot.GetTransform() goal[:2,3] -= 0.20*robot.GetManipulators()[0].GetEndEffectorTransform()[:2,2] xyyaw = np.array([goal[0,3],goal[1,3],np.arctan2(goal[1,0],goal[0,0])]) with env: robot.SetActiveDOFs([],orpy.DOFAffine.X|orpy.DOFAffine.Y|orpy.DOFAffine.RotationAxis,[0,0,1]) traj = planner.MoveActiveJoints(goal=xyyaw,maxiter=5000,steplength=0.15,maxtries=2,execute=False,outputtrajobj=True, jitter=-0.01) robot.GetController().SetPath(traj) while not robot.GetController().IsDone(): time.sleep(0.01) except Exception, e: print 'Exception when moving back.' print str(e) c = raw_input('IPython shell?(y/n)')
def MoveBaseTo(robot,goal,planner,skip_waypoints=False): try: EnableGripperLinks(robot.GetEnv(),robot.GetName(),False) # HACK TODO XXX orpy.RaveSetDebugLevel(orpy.DebugLevel.Verbose)
ap.add_argument('--test', action='store_true') ap.add_argument('--params', default='params/quadrotor.yaml') return ap.parse_args() @rave.with_destroy def run(): args = parse_args() params = parser.Yaml(file_name=args.params) env = rave.Environment() env.SetViewer('qtcoin') env.Reset() # load a scene from ProjectRoom environment XML file env.Load(params.scene) env.UpdatePublishedBodies() time.sleep(0.1) # 1) get the 1st robot that is inside the loaded scene # 2) assign it to the variable named 'robot' robot = env.GetRobots()[0] raw_input("Press enter to exit...") if __name__ == "__main__": rave.RaveSetDebugLevel(rave.DebugLevel.Verbose) run()
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 elif args.robot == 'simple': robot = SimpleRobot(env) planning_env = SimpleEnvironment(robot) else:
for i in range(len(marker.points)): print 'Transform { translation %f %f %f \n ' % ( marker.points[i].x, marker.points[i].y, marker.points[i].z) f.write('Transform { translation %f %f %f \n ' % (marker.points[i].x, marker.points[i].y, marker.points[i].z)) f.write( 'children [ Shape { appearance Appearance { material Material { diffuseColor %f %f %f } } ' % (marker.colors[i].r, marker.colors[i].g, marker.colors[i].b) + 'geometry Box { size %f %f %f } } ]\n}\n' % (marker.scale.x, marker.scale.y, marker.scale.z)) openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Error) if __name__ == "__main__": file_name = 'crinspect_octomap' topic_name = '/camera/depth/points' rospy.init_node('test_octomap', anonymous=False) env = openravepy.Environment() # env.Load('robots/denso_with_ftsensor.robot.xml') env.Load('worlds/lab_demo.env.xml') robot = env.GetRobot('robot') robot.SetActiveDOFValues([0, -0.34906585, 2.26892803, 0, 1.22173048, 0]) # env.SetDefaultViewer() env.SetViewer('qtcoin') # Initialize octomap