def format_data_for_tsdf(image_folder): """ Processes the data into the format needed for tsdf-fusion algorithm """ # image_folder = os.path.join(data_folder, 'images') camera_info_yaml = os.path.join(image_folder, "camera_info.yaml") camera_info = spartan_utils.getDictFromYamlFilename(camera_info_yaml) K_matrix = camera_info['camera_matrix']['data'] # print K_matrix n = K_matrix[0] def sci(n): return "{:.8e}".format(n) camera_intrinsics_out = os.path.join(image_folder, "camera-intrinsics.txt") with open(camera_intrinsics_out, 'w') as the_file: the_file.write(" " + sci(K_matrix[0]) + " " + sci(K_matrix[1]) + " " + sci(K_matrix[2]) + " \n") the_file.write(" " + sci(K_matrix[3]) + " " + sci(K_matrix[4]) + " " + sci(K_matrix[5]) + " \n") the_file.write(" " + sci(K_matrix[6]) + " " + sci(K_matrix[7]) + " " + sci(K_matrix[8]) + " \n") ### HANDLE POSES pose_data_yaml = os.path.join(image_folder, "pose_data.yaml") with open(pose_data_yaml, 'r') as stream: try: pose_data_dict = yaml.load(stream) except yaml.YAMLError as exc: print(exc) print pose_data_dict[0] for i in pose_data_dict: # print i # print pose_data_dict[i] pose4 = spartan_utils.homogenous_transform_from_dict( pose_data_dict[i]['camera_to_world']) depth_image_filename = pose_data_dict[i]['depth_image_filename'] prefix = depth_image_filename.split("depth")[0] print prefix pose_file_name = prefix + "pose.txt" pose_file_full_path = os.path.join(image_folder, pose_file_name) with open(pose_file_full_path, 'w') as the_file: the_file.write(" " + sci(pose4[0, 0]) + " " + sci(pose4[0, 1]) + " " + sci(pose4[0, 2]) + " " + sci(pose4[0, 3]) + " \n") the_file.write(" " + sci(pose4[1, 0]) + " " + sci(pose4[1, 1]) + " " + sci(pose4[1, 2]) + " " + sci(pose4[1, 3]) + " \n") the_file.write(" " + sci(pose4[2, 0]) + " " + sci(pose4[2, 1]) + " " + sci(pose4[2, 2]) + " " + sci(pose4[2, 3]) + " \n") the_file.write(" " + sci(pose4[3, 0]) + " " + sci(pose4[3, 1]) + " " + sci(pose4[3, 2]) + " " + sci(pose4[3, 3]) + " \n")
def __init__(self): self.bagging = False self.rosbag_proc = None storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config','RLG_iiwa_1','stored_poses.yaml') self.storedPoses = spartanUtils.getDictFromYamlFilename(storedPosesFile) self.robotService = rosUtils.RobotService(self.storedPoses['header']['joint_names']) self.setupConfig()
def downsample_by_pose_difference_threshold(images_dir_full_path, threshold): pose_yaml = os.path.join(images_dir_full_path, "pose_data.yaml") pose_dict = spartanUtils.getDictFromYamlFilename(pose_yaml) images_dir_temp_path = os.path.join(os.path.dirname(images_dir_full_path), 'images_temp') if not os.path.isdir(images_dir_temp_path): os.makedirs(images_dir_temp_path) previous_pose = FusionServer.get_numpy_position_from_pose(pose_dict[0]) print "Using downsampling by pose difference threshold... " print "Previously: ", len(pose_dict), " images" num_kept_images = 0 num_deleted_images = 0 for i in range(0,len(pose_dict)): single_frame_data = pose_dict[i] this_pose = FusionServer.get_numpy_position_from_pose(pose_dict[i]) if i == 0: keep_image = True num_kept_images += 1 elif np.linalg.norm(previous_pose - this_pose) > threshold: previous_pose = this_pose num_kept_images += 1 else: # delete pose from forward kinematics del pose_dict[i] continue # if we have gotten here, then move the images over to the new directory rgb_filename = os.path.join(images_dir_full_path, single_frame_data['rgb_image_filename']) rgb_filename_temp = os.path.join(images_dir_temp_path, single_frame_data['rgb_image_filename']) shutil.move(rgb_filename, rgb_filename_temp) depth_filename = os.path.join(images_dir_full_path, single_frame_data['depth_image_filename']) depth_filename_temp = os.path.join(images_dir_temp_path, single_frame_data['depth_image_filename']) shutil.move(depth_filename, depth_filename_temp) # # delete pose from posegraph # del posegraph_list[i-num_deleted_images] # num_deleted_images += 1 # write downsamples pose_data.yaml (forward kinematics) spartanUtils.saveToYaml(pose_dict, os.path.join(images_dir_temp_path,'pose_data.yaml')) # remove old images shutil.move(os.path.join(images_dir_full_path, 'camera_info.yaml'), os.path.join(images_dir_temp_path, 'camera_info.yaml')) shutil.rmtree(images_dir_full_path) print "renaming %s to %s " %(images_dir_temp_path, images_dir_full_path) # rename temp images to images os.rename(images_dir_temp_path, images_dir_full_path) print "After: ", num_kept_images, " images"
def __init__(self, robotStateModel, mug_rack_config=None, mug_platform_config=None, shoe_rack_config=None): self._robotStateModel = robotStateModel self.clear_visualation() self.setup_config() self._gripper_link_frame_name = "wsg_50_base_link" self._object = None robotStateModel.connectModelChanged(self.on_robot_model_changed) if mug_rack_config is None: mug_rack_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/mug_rack.yaml' ) self._mug_rack_config = spartan_utils.getDictFromYamlFilename( mug_rack_config_file) else: self._mug_rack_config = mug_rack_config if mug_platform_config is None: mug_platform_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/mug_platform.yaml' ) self._mug_platform_config = spartan_utils.getDictFromYamlFilename( mug_platform_config_file) else: self._mug_platform_config = mug_platform_config if shoe_rack_config is None: shoe_rack_config_file = os.path.join( spartan_utils.getSpartanSourceDir(), 'src/catkin_projects/station_config/RLG_iiwa_1/manipulation/shoe_rack.yaml' ) self._shoe_rack_config = spartan_utils.getDictFromYamlFilename( shoe_rack_config_file) else: self._shoe_rack_config = shoe_rack_config
def __init__(self, camera_serial_number="carmine_1"): self.camera_serial_number = camera_serial_number self.bagging = False self.rosbag_proc = None self.tfBuffer = None storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config','RLG_iiwa_1','stored_poses.yaml') self.storedPoses = spartanUtils.getDictFromYamlFilename(storedPosesFile) self.robotService = rosUtils.RobotService(self.storedPoses['header']['joint_names']) self.setupConfig() self.setupSubscribers() self.setupPublishers() self.setupTF() self.setupCache()
def __init__(self): self.cameraName = rospy.get_param('~camera_name') self.cameraInfoFilename = rospy.get_param('~camera_info_filename') rospy.loginfo("camera_info_filename = %s", self.cameraInfoFilename) cameraInfoYamlDict = spartanUtils.getDictFromYamlFilename( self.cameraInfoFilename) self.cameraTypes = ['rgb', 'depth'] self.parseCameraInfo(cameraInfoYamlDict) self.broadcaster = tf2_ros.TransformBroadcaster() self.staticTransformBroadcaster = tf2_ros.StaticTransformBroadcaster() rospy.loginfo("finished initializing the node")
def makeDefault(**kwargs): storedPosesFile = os.path.join(spartanUtils.getSpartanSourceDir(), 'src', 'catkin_projects', 'station_config', 'RLG_iiwa_1', 'background_subtraction', 'stored_poses.yaml') cameraInfoFilename = os.path.join( spartanUtils.getSpartanSourceDir(), 'src/catkin_projects/camera_config/data/1112170110/master/camera_ros_data.yaml' ) poseData = spartanUtils.getDictFromYamlFilename(storedPosesFile) cameraInfoDict = spartanUtils.getDictFromYamlFilename( cameraInfoFilename) return BackgroundSubtractionDataCapture( poseData['header']['joint_names'], poseData, cameraInfoDict, poseFilename=storedPosesFile, cameraInfoFilename=cameraInfoFilename, **kwargs)
def extract_intrinsics(path_to_tar_file, path_to_camera_info_yaml_file, camera_name): cmd = 'mkdir -p /tmp/calibrationdata' os.system(cmd) cmd = 'tar xvf ' + path_to_tar_file + ' -C ' + '/tmp/calibrationdata' + ' > /dev/null 2>&1' os.system(cmd) camera_info_dict = spartanUtils.getDictFromYamlFilename( '/tmp/calibrationdata/ost.yaml') print camera_info_dict camera_info_dict['camera_name'] = camera_name spartanUtils.saveToYaml(camera_info_dict, path_to_camera_info_yaml_file)
def fromConfigFilename(robotSystem, configFilename, channelName): config = spartanUtils.getDictFromYamlFilename(configFilename) transformDict = config['depth']['extrinsics'][ 'transform_to_reference_link'] cameraToLinkTransform = spartanUtils.transformFromPose(transformDict) rgbTransformDict = config['rgb']['extrinsics'][ 'transform_to_reference_link'] rgbCameraToLinkTransform = spartanUtils.transformFromPose( rgbTransformDict) return CameraTransform( robotSystem, referenceLinkName=config['depth']['extrinsics'] ['reference_link_name'], cameraToLinkTransform=cameraToLinkTransform, channelName=channelName, rgbCameraToLinkTransform=rgbCameraToLinkTransform)
def __init__(self, storedPosesFile=None, cameraSerialNumber=1112170110): self.storedPoses = spartanUtils.getDictFromYamlFilename( storedPosesFile) self.cameraSerialNumber = cameraSerialNumber self.cameraName = 'camera_' + str(cameraSerialNumber) self.pointCloudTopic = '/' + str(self.cameraName) + '/depth/points' self.graspFrameName = 'base' self.depthOpticalFrameName = self.cameraName + "_depth_optical_frame" self.robotService = rosUtils.RobotService( self.storedPoses['header']['joint_names']) self.usingDirector = True self.setupConfig() if USING_DIRECTOR: self.taskRunner = TaskRunner() self.taskRunner.callOnThread(self.setupSubscribers) self.taskRunner.callOnThread(self.setupTF) else: self.setupSubscribers() self.setupTF()
def reloadParams(self): self.graspingParams = spartanUtils.getDictFromYamlFilename(self.graspingParamsFile)
def extract_and_fuse_single_scene( log_full_path, downsample=True, linear_distance_threshold=LINEAR_DISTANCE_THRESHOLD, angle_distance_threshold=ANGLE_DISTANCE_THRESHOLD): """ Extracts all the images from the rosbag. :param log_full_path: :param downsample: whether or not to downsample :param linear_distance_threshold: threshold used in downsampling :param angle_distance_threshold: threshold used in downsampling :return: """ global fs if fs is None: fs = FusionServer() print "extracting and fusing scene:", log_full_path log = os.path.split(log_full_path)[-1] mem() processed_dir = os.path.join(log_full_path, 'processed') images_dir = os.path.join(processed_dir, 'images') raw_dir = os.path.join(log_full_path, 'raw') raw_close_up_dir = os.path.join(log_full_path, 'raw_close_up') bag_filepath = os.path.join(log_full_path, 'raw', 'fusion_' + log + '.bag') bag_close_up_filepath = os.path.join(raw_close_up_dir, 'fusion_' + log + '.bag') pose_data_filename = os.path.join(images_dir, 'pose_data.yaml') metadata_filename = os.path.join(images_dir, 'metadata.yaml') if not already_extracted_rosbag(log_full_path): print "extracting", bag_filepath # run this memory intensive step in a process, to allow # it to release memory back to the operating system # when finished proc = mp.Process(target=extract_data_from_rosbag, args=(bag_filepath, images_dir)) proc.start() proc.join() print "finished extracting", bag_filepath # counter_new_extracted += 1 else: print "already extracted", bag_filepath # we need to free some memory gc.collect() mem() if not already_ran_tsdf_fusion(log_full_path): print "preparing for tsdf_fusion for", log_full_path tsdf_fusion.format_data_for_tsdf(images_dir) gc.collect() print "running tsdf fusion" fusion_params = FUSION_CONFIG[FUSION_LOCATION] tsdf_fusion.run_tsdf_fusion_cuda( images_dir, bbox_min=fusion_params['bbox_min'], bbox_max=fusion_params['bbox_max'], voxel_size=fusion_params['voxel_size']) gc.collect() print "converting tsdf to ply" tsdf_bin_filename = os.path.join(processed_dir, 'tsdf.bin') tsdf_mesh_filename = os.path.join(processed_dir, 'fusion_mesh.ply') tsdf_fusion.convert_tsdf_to_ply(tsdf_bin_filename, tsdf_mesh_filename) # counter_new_fused += 1 else: print "already ran tsdf_fusion for", log_full_path if not already_downsampled(log_full_path) and downsample: print "downsampling image folder" # fs = FusionServer() fs.downsample_by_pose_difference_threshold(images_dir, linear_distance_threshold, angle_distance_threshold) # counter_new_downsampled += 1 # create metadata.yaml file if it doesn't yet exist if not os.path.exists(metadata_filename): pose_data = spartanUtils.getDictFromYamlFilename( pose_data_filename) original_image_indices = pose_data.keys() metadata = dict() metadata['original_image_indices'] = original_image_indices metadata['close_up_image_indices'] = [] spartanUtils.saveToYaml(metadata, metadata_filename) else: print "already downsampled for", log_full_path # extract the up close up log if it exists if os.path.exists( bag_close_up_filepath ) and not already_extracted_close_up_log(metadata_filename): print "\n\n-----------extracting close up images----------\n\n" print "extracting", bag_close_up_filepath close_up_temp_dir = os.path.join(processed_dir, 'images_close_up') # run this memory intensive step in a process, to allow # it to release memory back to the operating system # when finished proc = mp.Process(target=extract_data_from_rosbag, args=(bag_close_up_filepath, close_up_temp_dir)) proc.start() proc.join() print "downsampling image folder %s" % (close_up_temp_dir) if downsample: fs.downsample_by_pose_difference_threshold( close_up_temp_dir, linear_distance_threshold, angle_distance_threshold) pose_data_close_up_filename = os.path.join(close_up_temp_dir, 'pose_data.yaml') pose_data_close_up = spartanUtils.getDictFromYamlFilename( pose_data_close_up_filename) pose_data = spartanUtils.getDictFromYamlFilename(pose_data_filename) original_image_indices = pose_data.keys() largest_original_image_idx = max(original_image_indices) close_up_image_indices = [] print "largest_original_image_idx", largest_original_image_idx for img_idx, data in pose_data_close_up.iteritems(): # just to give some buffer and avoid edge cases in some checks img_idx_new = img_idx + largest_original_image_idx + 4 rgb_filename = "%06i_%s.png" % (img_idx_new, "rgb") depth_filename = "%06i_%s.png" % (img_idx_new, "depth") rgb_filename_prev = data['rgb_image_filename'] depth_filename_prev = data['depth_image_filename'] data['rgb_image_filename'] = rgb_filename data['depth_image_filename'] = depth_filename pose_data[img_idx_new] = data close_up_image_indices.append(img_idx_new) # move images around shutil.move(os.path.join(close_up_temp_dir, rgb_filename_prev), os.path.join(images_dir, rgb_filename)) shutil.move(os.path.join(close_up_temp_dir, depth_filename_prev), os.path.join(images_dir, depth_filename)) shutil.rmtree(close_up_temp_dir) original_image_indices.sort() close_up_image_indices.sort() spartanUtils.saveToYaml(pose_data, pose_data_filename) metadata = dict() metadata['normal_image_indices'] = original_image_indices metadata['close_up_image_indices'] = close_up_image_indices spartanUtils.saveToYaml(metadata, metadata_filename) else: if os.path.exists(bag_close_up_filepath): print "already extracted close up log, skipping" else: print "raw_close_up doesn't exist, skipping"
def already_extracted_close_up_log(metadata_filename): if not os.path.exists(metadata_filename): return False metadata_temp = spartanUtils.getDictFromYamlFilename(metadata_filename) return (len(metadata_temp['close_up_image_indices']) > 0)
import shutil import argparse from fusion_server.fusion import FusionServer import fusion_server.tsdf_fusion as tsdf_fusion import spartan.utils.utils as spartanUtils fs = None LINEAR_DISTANCE_THRESHOLD = 0.03 # 3cm ANGLE_DISTANCE_THRESHOLD = 10 # 10 degrees FUSION_CONFIG_FILE = os.path.join( spartanUtils.getSpartanSourceDir(), "src/catkin_projects/station_config/RLG_iiwa_1/fusion/fusion_params.yaml") FUSION_CONFIG = spartanUtils.getDictFromYamlFilename(FUSION_CONFIG_FILE) FUSION_LOCATION = "front" def mem(): print( 'Memory usage : % 2.2f MB' % round(resource.getrusage(resource.RUSAGE_SELF).ru_maxrss / 1024.0, 1)) def already_extracted_rosbag(log_full_path): images_dir = os.path.join(log_full_path, 'processed', 'images') file_to_check = os.path.join(images_dir, 'camera_info.yaml') return os.path.exists(file_to_check)
def processCalibrationData(posegraph_file, robot_data_filename, save_data_filename): cameraPoses = CameraPoses(posegraph_file) calibrationData = spartanUtils.getDictFromYamlFilename(robot_data_filename) calibrationData = addCameraPosesToDataFile(calibrationData, cameraPoses) spartanUtils.saveToYaml(calibrationData, save_data_filename)
def loadConfigFromFile(self, filename=None): if filename is None: filename = self.configFilename self.config = spartanUtils.getDictFromYamlFilename(filename) self.calibrationPosesConfig = self.config['poses']
def downsample_by_pose_difference_threshold(images_dir_full_path, linear_distance_threshold, rotation_angle_threshold): """ Downsamples poses and keeps only those that are sufficiently apart :param images_dir_full_path: :type images_dir_full_path: :param linear_distance_threshold: threshold on the translation, in meters :type linear_distance_threshold: :param rotation_angle_threshold: threshold on the angle between the rotations, in degrees :type rotation_angle_threshold: :return: :rtype: """ pose_yaml = os.path.join(images_dir_full_path, "pose_data.yaml") pose_dict = spartanUtils.getDictFromYamlFilename(pose_yaml) images_dir_temp_path = os.path.join( os.path.dirname(images_dir_full_path), 'images_temp') if not os.path.isdir(images_dir_temp_path): os.makedirs(images_dir_temp_path) previous_pose_pos = FusionServer.get_numpy_position_from_pose( pose_dict[0]) previous_pose_quat = FusionServer.get_quaternion_from_pose( pose_dict[0]) print "Using downsampling by pose difference threshold... " print "Previously: ", len(pose_dict), " images" num_kept_images = 0 num_deleted_images = 0 for i in range(0, len(pose_dict)): single_frame_data = pose_dict[i] this_pose_pos = FusionServer.get_numpy_position_from_pose( pose_dict[i]) this_pose_quat = FusionServer.get_quaternion_from_pose( pose_dict[i]) linear_distance = np.linalg.norm(this_pose_pos - previous_pose_pos) rotation_distance = spartanUtils.compute_angle_between_quaternions( this_pose_quat, previous_pose_quat) if i == 0: keep_image = True num_kept_images += 1 elif (linear_distance > linear_distance_threshold) or ( np.rad2deg(rotation_distance) > rotation_angle_threshold): previous_pose_pos = this_pose_pos previous_pose_quat = this_pose_quat num_kept_images += 1 else: # delete pose from forward kinematics del pose_dict[i] continue # if we have gotten here, then move the images over to the new directory rgb_filename = os.path.join( images_dir_full_path, single_frame_data['rgb_image_filename']) rgb_filename_temp = os.path.join( images_dir_temp_path, single_frame_data['rgb_image_filename']) shutil.move(rgb_filename, rgb_filename_temp) depth_filename = os.path.join( images_dir_full_path, single_frame_data['depth_image_filename']) depth_filename_temp = os.path.join( images_dir_temp_path, single_frame_data['depth_image_filename']) shutil.move(depth_filename, depth_filename_temp) # # delete pose from posegraph # del posegraph_list[i-num_deleted_images] # num_deleted_images += 1 # write downsamples pose_data.yaml (forward kinematics) spartanUtils.saveToYaml( pose_dict, os.path.join(images_dir_temp_path, 'pose_data.yaml')) # remove old images shutil.move(os.path.join(images_dir_full_path, 'camera_info.yaml'), os.path.join(images_dir_temp_path, 'camera_info.yaml')) shutil.rmtree(images_dir_full_path) print "renaming %s to %s " % (images_dir_temp_path, images_dir_full_path) # rename temp images to images os.rename(images_dir_temp_path, images_dir_full_path) print "After: ", num_kept_images, " images"
#! /usr/bin/env python import yaml # spartan from robot_control.robotmovementservice import RobotMovementService import spartan.utils.utils as spartan_utils # ROS import rospy if __name__ == "__main__": rospy.init_node("RobotMovementService") config_filename = rospy.get_param("~config_filename") rospy.loginfo("config_filename: %s", config_filename) config = spartan_utils.getDictFromYamlFilename(config_filename) robotMovementService = RobotMovementService(config) robotMovementService.advertiseServices() rospy.loginfo("RobotMovementService ready!") while not rospy.is_shutdown(): rospy.spin()
def do_main(): rospy.init_node('simple_teleop', anonymous=True) # setup listener for tf2s (used for ee and controller poses) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) robotSubscriber = JointStateSubscriber("/joint_states") # wait until robot state found robotSubscriber = ros_utils.JointStateSubscriber("/joint_states") print("Waiting for full kuka state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1) print("got full state") # init gripper handDriver = SchunkDriver() # init mouse manager mouse_manager = TeleopMouseManager() roll_goal = 0.0 yaw_goal = 0.0 pitch_goal = 0.0 # Start by moving to an above-table pregrasp pose that we know # EE control will work well from (i.e. far from singularity) stored_poses_dict = spartan_utils.getDictFromYamlFilename("../station_config/RLG_iiwa_1/stored_poses.yaml") above_table_pre_grasp = stored_poses_dict["Grasping"]["above_table_pre_grasp"] robotService = ros_utils.RobotService.makeKukaRobotService() success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=5) print("Moved to position") gripper_goal_pos = 0.0 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print("sent close goal to gripper") time.sleep(2) gripper_goal_pos = 0.1 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.1, timeout=0.01) print("sent open goal to gripper") frame_name = "iiwa_link_ee" # end effector frame name for i in range(10): if i == 9: print "Couldn't find robot pose" sys.exit(0) try: ee_pose_above_table = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) ee_tf_above_table = tf_matrix_from_pose(ee_pose_above_table) break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up robot pose...") time.sleep(0.1) # Then kick off task space streaming sp = rospy.ServiceProxy('plan_runner/init_task_space_streaming', robot_msgs.srv.StartStreamingPlan) init = robot_msgs.srv.StartStreamingPlanRequest() init.force_guard.append(make_force_guard_msg(20.)) res = sp(init) print("Started task space streaming") pub = rospy.Publisher('plan_runner/task_space_streaming_setpoint', robot_msgs.msg.CartesianGoalPoint, queue_size=1); def cleanup(): rospy.wait_for_service("plan_runner/stop_plan") sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger) init = std_srvs.srv.TriggerRequest() print sp(init) print("Done cleaning up and stopping streaming plan") rospy.on_shutdown(cleanup) br = tf.TransformBroadcaster() rate = rospy.Rate(100) # max rate at which control should happen ee_tf_last_commanded = np.zeros((4,4)) def get_initial_pose(): while not rospy.is_shutdown(): # get current tf from ros world to ee try: ee_pose_current = ros_utils.poseFromROSTransformMsg( tfBuffer.lookup_transform("base", frame_name, rospy.Time()).transform) ee_tf_last_commanded = tf_matrix_from_pose(ee_pose_current) return ee_tf_last_commanded except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print("Troubling looking up robot pose...") rate.sleep() continue print ee_tf_last_commanded ee_tf_last_commanded = get_initial_pose() print ee_tf_last_commanded sys.path.append("../imitation_tools/scripts") from capture_imitation_data_client import start_bagging_imitation_data_client, stop_bagging_imitation_data_client start_bagging_imitation_data_client() pose_save_counter = 0 saved_pose_dict = dict() saved_pose_counter = 0 try: # control loop while not rospy.is_shutdown(): # # get teleop mouse events = mouse_manager.get_events() if events["r"]: print above_table_pre_grasp print "that was above_table_pre_grasp" success = robotService.moveToJointPosition(above_table_pre_grasp, timeout=3) roll_goal = 0.0 yaw_goal = 0.0 pitch_goal = 0.0 ee_tf_last_commanded = get_initial_pose() continue pose_save_counter += 1 if events["o"] and pose_save_counter >= 100: # this make it not happen to much joint_name_list = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7'] joint_position_vector = robotSubscriber.get_position_vector_from_joint_names(joint_name_list) print joint_position_vector print "joint positions saved" new_pose_name = "pose_"+str(saved_pose_counter).zfill(3) saved_pose_counter += 1 saved_pose_dict[new_pose_name] = joint_position_vector.tolist() pose_save_counter = 0 if events["escape"]: stop_bagging_imitation_data_client() if len(saved_pose_dict) > 0: spartan_utils.saveToYaml(saved_pose_dict, "saved_poses.yaml") sys.exit(0) scale_down = 0.0001 delta_x = events["delta_x"]*scale_down delta_y = events["delta_y"]*-scale_down delta_forward = 0.0 forward_scale = 0.001 if events["w"]: delta_forward -= forward_scale if events["s"]: delta_forward += forward_scale # extract and normalize quat from tf if events["rotate_left"]: roll_goal += 0.01 if events["rotate_right"]: roll_goal -= 0.01 roll_goal = np.clip(roll_goal, a_min = -0.9, a_max = 0.9) if events["side_button_back"]: yaw_goal += 0.01 print("side button back") if events["side_button_forward"]: yaw_goal -= 0.01 print("side side_button_forward") yaw_goal = np.clip(yaw_goal, a_min = -1.314, a_max = 1.314) if events["d"]: pitch_goal += 0.01 if events["a"]: pitch_goal -= 0.01 pitch_goal = np.clip(pitch_goal, a_min = -1.314, a_max = 1.314) R = transformations.euler_matrix(pitch_goal, roll_goal, yaw_goal, 'syxz') # third is "yaw", when in above table pre-grasp # second is "roll", '' # first must be "pitch" above_table_quat_ee = transformations.quaternion_from_matrix(R.dot(ee_tf_above_table)) above_table_quat_ee = np.array(above_table_quat_ee) / np.linalg.norm(above_table_quat_ee) # calculate controller position delta and add to start position to get target ee position target_translation = np.asarray([delta_forward, delta_x, delta_y]) empty_matrx = np.zeros_like(ee_tf_last_commanded) empty_matrx[:3, 3] = target_translation ee_tf_last_commanded += empty_matrx target_trans_ee = ee_tf_last_commanded[:3, 3] # publish target pose as cartesian goal point new_msg = robot_msgs.msg.CartesianGoalPoint() new_msg.xyz_point.header.stamp = rospy.Time.now() new_msg.xyz_point.header.frame_id = "world" new_msg.xyz_point.point.x = target_trans_ee[0] new_msg.xyz_point.point.y = target_trans_ee[1] new_msg.xyz_point.point.z = target_trans_ee[2] new_msg.xyz_d_point.x = 0.0 new_msg.xyz_d_point.y = 0.0 new_msg.xyz_d_point.z = 0.0 new_msg.quaternion.w = above_table_quat_ee[0] new_msg.quaternion.x = above_table_quat_ee[1] new_msg.quaternion.y = above_table_quat_ee[2] new_msg.quaternion.z = above_table_quat_ee[3] new_msg.roll = roll_goal new_msg.pitch = pitch_goal new_msg.yaw = yaw_goal new_msg.gain = make_cartesian_gains_msg(5., 10.) new_msg.ee_frame_id = frame_name pub.publish(new_msg) #gripper if events["mouse_wheel_up"]: gripper_goal_pos += 0.006 if events["mouse_wheel_down"]: gripper_goal_pos -= 0.006 if gripper_goal_pos < 0: gripper_goal_pos = 0.0 if gripper_goal_pos > 0.1: gripper_goal_pos = 0.1 handDriver.sendGripperCommand(gripper_goal_pos, speed=0.2, stream=True) rate.sleep() except Exception as e: print "Suffered exception ", e
def runROSCalibrationFixedMount(self, headerData, topic_name, poses_file): headerData['target']['transform_to_hand_frame'] = dict() headerData['target']['transform_to_hand_frame']['translation'] = dict() headerData['target']['transform_to_hand_frame']['translation'][ 'x'] = 0.0 headerData['target']['transform_to_hand_frame']['translation'][ 'y'] = 0.0 headerData['target']['transform_to_hand_frame']['translation'][ 'z'] = 0.0931 headerData['target']['transform_to_hand_frame']['quaternion'] = dict() headerData['target']['transform_to_hand_frame']['quaternion'][ 'x'] = 0.49894425 headerData['target']['transform_to_hand_frame']['quaternion'][ 'y'] = 0.50080633 headerData['target']['transform_to_hand_frame']['quaternion'][ 'z'] = 0.51116578 headerData['target']['transform_to_hand_frame']['quaternion'][ 'w'] = -0.48883249 self.calibrationData = dict() calibrationRunData = self.calibrationData calibrationRunData['header'] = headerData self.passiveSubscriber = spartanROSUtils.SimpleSubscriber( topic_name, sensor_msgs.msg.Image) self.passiveSubscriber.start() unique_name = time.strftime( "%Y%m%d-%H%M%S") + "_" + self.calibrationType self.calibrationFolderName = os.path.join( spartanUtils.getSpartanSourceDir(), 'calibration_data', unique_name) os.system("mkdir -p " + self.calibrationFolderName) os.chdir(self.calibrationFolderName) rospy.loginfo("starting calibration run") calibrationData = [] poses = spartanUtils.getDictFromYamlFilename(poses_file) num_poses = len(poses) for index, pose_name in enumerate(sorted(poses.keys())): # move robot to that joint position info_msg = "\n moving to pose " + str(index) + " of " + str( num_poses) + " named " + pose_name rospy.loginfo(info_msg) self.robotService.moveToJointPosition(poses[pose_name]) rospy.loginfo("capturing images and robot data") data = self.captureCurrentRobotAndImageData( captureRGB=self.captureRGB, captureIR=self.captureIR, prefix=str(index)) calibrationData.append(data) rospy.loginfo("finished calibration routine, saving data to file") calibrationRunData['data_list'] = calibrationData spartanUtils.saveToYaml( calibrationRunData, os.path.join(self.calibrationFolderName, 'robot_data.yaml')) if self.passiveSubscriber: self.passiveSubscriber.stop() self.moveHome() return calibrationRunData
def loadConfigFromFiles(self): self.poseData = spartanUtils.getDictFromYamlFilename(self.poseFilename) self.cameraInfoDict = spartanUtils.getDictFromYamlFilename( self.cameraInfoFilename)