def load_side_table(self): """ Loads the side table :return: :rtype: """ pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc') side_table_ply_file = os.path.join( pdc_data_dir, self._mug_rack_config['background_mesh']) side_table_poly_data = ioUtils.readPolyData(side_table_ply_file) vis.showPolyData(side_table_poly_data, 'Side Table', parent=self._vis_container)
def load_object_model(self): """ Load the object model poly data :return: :rtype: """ model_filename = os.path.join(spartan_utils.get_data_dir(), "pdc/templates/mugs/mug.stl") print("model_filename", model_filename) self._poly_data = ioUtils.readPolyData(model_filename) self._object = vis.updatePolyData(self._poly_data, "Object", parent=self._vis_container) vis.addChildFrame(self._object) pos = [0.34301733, 0.00805546, 0.92509635] quat = [0.17589242, 0.98340013, -0.04305877, 0.01148846] t = transformUtils.transformFromPose(pos, quat) self._object.getChildFrame().copyFrame(t)
def load_mug_rack(self): """ Loads a mug rack and side table poly data :return: :rtype: """ pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc') rack_ply_file = os.path.join(pdc_data_dir, self._mug_rack_config["mesh"]) rack_poly_data = ioUtils.readPolyData(rack_ply_file) self._mug_rack = vis.showPolyData(rack_poly_data, 'Mug Rack', parent=self._vis_container) # rack_target_position target_pose_name = "left_side_table" target_pose = director_utils.transformFromPose( self._mug_rack_config['poses'][target_pose_name]) vis.addChildFrame(self._mug_rack) self._mug_rack.getChildFrame().copyFrame(target_pose)
def load_mug_platform(self): """ Load the mug platform :return: :rtype: """ pdc_data_dir = os.path.join(spartan_utils.get_data_dir(), 'pdc') platform_ply_file = os.path.join(pdc_data_dir, self._mug_platform_config['mesh']) platform_poly_data = ioUtils.readPolyData(platform_ply_file) self._mug_platform = vis.showPolyData(platform_poly_data, 'Platform', parent=self._vis_container) target_pose_name = "left_side_table" target_pose = director_utils.transformFromPose( self._mug_platform_config['poses'][target_pose_name]) vis.addChildFrame(self._mug_platform) self._mug_platform.getChildFrame().copyFrame(target_pose)
def capture_scene(self, use_close_up_poses=False): """ This "moves around and captures all of the data needed for fusion". I.e., it: 1. Moves the robot "home" 2. Starts bagging all data needed for fusion 3. Moves the robot around 4. Stops bagging data 5. Moves the robot back home This is not a service handler itself, but intended to be modularly called by service handlers. :return: bag_filepath, the full path to where (one) of the rosbag (fusion-*.bag) was saved :rtype: string """ # first move home home_pose_joint_positions = self.storedPoses["Elastic Fusion"][ self.config['home_pose_name']] print home_pose_joint_positions self.robotService.moveToJointPosition( home_pose_joint_positions, maxJointDegreesPerSecond=self.config['speed']['fast']) print "moved to home" # Start bagging for far out data collection # base_path = os.path.join(spartanUtils.getSpartanSourceDir(), 'data_volume', 'pdc', 'logs_proto') base_path = os.path.join(spartanUtils.get_data_dir(), 'pdc', 'logs_test') log_id_name = spartanUtils.get_current_YYYY_MM_DD_hh_mm_ss() log_subdir = "raw" bagfile_directory = os.path.join(base_path, log_id_name, log_subdir) bagfile_name = "fusion_" + log_id_name + ".bag" full_path_to_bagfile = os.path.join(bagfile_directory, bagfile_name) print "moving robot through regular scan poses" self.start_bagging(full_path_to_bag_file=full_path_to_bagfile) # pose_list = self.config['scan']['pose_list'] pose_list = self.config['scan']['left_table'] joint_positions = self.get_joint_positions_for_pose(pose_list[0]) self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed']['scan']) # rospy.sleep(3.0) self._move_robot_through_pose_list(pose_list, randomize_wrist=True, hit_original_poses=True) self._stop_bagging() # # move robot through close up scan poses if use_close_up_poses: log_subdir = "raw_close_up" bagfile_directory = os.path.join(base_path, log_id_name, log_subdir) bagfile_name = "fusion_" + log_id_name + ".bag" full_path_to_bagfile = os.path.join(bagfile_directory, bagfile_name) # print "moving robot through close up scan poses" pose_list = self.config['scan']['close_up'] # move to first pose before we start bagging joint_positions = self.get_joint_positions_for_pose(pose_list[0]) self.robotService.moveToJointPosition( joint_positions, maxJointDegreesPerSecond=self.config['speed']['scan']) # now start bagging and move the robot through the poses self.start_bagging(full_path_to_bag_file=full_path_to_bagfile) self._move_robot_through_pose_list(pose_list, randomize_wrist=True, hit_original_poses=True) # rospy.sleep(3.0) self._stop_bagging() rospy.sleep(1.0) # move back home self.robotService.moveToJointPosition( home_pose_joint_positions, maxJointDegreesPerSecond=self.config['speed']['fast']) return full_path_to_bagfile
app.viewOptions.setProperty('Orientation widget', True) app.viewOptions.setProperty('View angle', 30) app.sceneBrowserDock.setVisible(True) app.propertiesDock.setVisible(False) app.mainWindow.setWindowTitle('Depth Scanner') app.mainWindow.show() app.mainWindow.resize(920, 600) app.mainWindow.move(0, 0) view = app.view globalsDict['app'] = app globalsDict['view'] = view # load background scene if it exists background_ply_file = os.path.join(spartanUtils.get_data_dir(), 'pdc', 'logs_special', '2019-01-03-22-43-55', 'processed', 'fusion_mesh.ply') def visualize_background(): if not os.path.exists(background_ply_file): return parent = om.getOrCreateContainer("scene") poly_data = ioUtils.readPolyData(background_ply_file) vis.updatePolyData(poly_data, 'table', parent=parent) visualize_background() poser_vis = PoserVisualizer.make_default() object_manip = ObjectManipulation(poser_visualizer=poser_vis)
def setupRLGDirector(globalsDict=None): tfWrapper = TFWrapper() tfBuffer = tfWrapper.getBuffer() graspSupervisor = spartan.manipulation.grasp_supervisor.GraspSupervisor.makeDefault( tfBuffer=tfBuffer) graspSupervisor.robotSystem = globalsDict[ 'robotSystem'] # for visualization globalsDict['graspSupervisor'] = graspSupervisor backgroundSubtraction = spartan.manipulation.background_subtraction.BackgroundSubtractionDataCapture.makeDefault( tfBuffer=tfBuffer) globalsDict['backgroundSubtraction'] = backgroundSubtraction spartanSourceDir = spartanUtils.getSpartanSourceDir() handEyeCalibrationConfigFilename = os.path.join( spartanSourceDir, "src/catkin_projects/station_config/RLG_iiwa_1/hand_eye_calibration/carmine_1.yaml" ) cal = spartan.calibration.handeyecalibration.HandEyeCalibration( globalsDict['robotSystem'], configFilename=handEyeCalibrationConfigFilename) cal.loadConfigFromFile() globalsDict['cal'] = cal # set rate limit on RemoteTreeViewer # fix for https://github.com/RobotLocomotion/spartan/issues/244 globalsDict['treeViewer'].subscriber.setSpeedLimit(5) # ros_visualizer = DirectorROSVisualizer(tf_buffer=tfBuffer) topic = "/camera_carmine_1/depth/points" ros_visualizer.add_subscriber(topic, name="Carmine", visualize=True) globalsDict['ros_visualizer'] = ros_visualizer ros_visualizer.start() # load background scene if it exists background_ply_file = os.path.join(spartanUtils.get_data_dir(), 'pdc', 'logs_special', '2019-01-03-22-43-55', 'processed', 'fusion_mesh.ply') robotSystem = globalsDict['robotSystem'] robotStateModel = robotSystem.robotStateModel category_manip = CategoryManipulation(robotStateModel) category_manip.load_side_table() category_manip.load_mug_rack() # category_manip.setup_horizontal_mug_grasp() # category_manip.load_mug_platform() # category_manip.load_shoe_rack() graspSupervisor._category_manip = category_manip def visualize_background(): if not os.path.exists(background_ply_file): return parent = om.getOrCreateContainer("scene") poly_data = ioUtils.readPolyData(background_ply_file) vis.updatePolyData(poly_data, 'table', parent=parent) visualize_background() poser_vis = PoserVisualizer.make_default() object_manip = ObjectManipulation() globalsDict['object_manip'] = object_manip globalsDict['o'] = object_manip globalsDict['c'] = category_manip globalsDict['poser_vis'] = poser_vis globalsDict['g'] = graspSupervisor