multi_step_cov_estimator.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 # author: Vijay Pradeep
00035 
00036 import roslib; roslib.load_manifest('calibration_estimation')
00037 
00038 import sys
00039 import rospy
00040 import time
00041 import numpy
00042 import rosbag
00043 import yaml
00044 import os.path
00045 import numpy
00046 import math
00047 
00048 import stat
00049 import os
00050 
00051 from numpy import matrix
00052 
00053 from calibration_estimation.cal_bag_helpers import *
00054 from calibration_estimation.urdf_params import UrdfParams
00055 from calibration_estimation.sensors.multi_sensor import MultiSensor
00056 from calibration_estimation.opt_runner import opt_runner
00057 
00058 from calibration_estimation.single_transform import angle_axis_to_RPY, RPY_to_angle_axis
00059 
00060 def usage():
00061     rospy.logerr("Not enough arguments")
00062     print "Usage:"
00063     print " ./proto1.py [bagfile] [output_dir]"
00064     sys.exit(0)
00065 
00066 
00067 def build_sensor_defs(sensors):
00068     '''
00069     Given a list of sensor definition dictionaries, merge them into a single dictionary
00070     '''
00071     all_sensors_dict = dict()
00072 
00073     #for cur_sensors_file in sensors_dump:
00074     for cur_sensor_type, cur_sensor_list in sensors.items():
00075         if cur_sensor_type in ['checkerboards']:
00076             continue
00077         for cur_sensor_name, cur_sensor in cur_sensor_list.items():
00078             cur_sensor['sensor_id'] = cur_sensor_name  # legacy support...
00079             # We want sensor_ids to be unique. Thus, we should warn the user if there are duplicate block IDs being loaded
00080             if cur_sensor['sensor_id'] in all_sensors_dict.keys():  # TODO: can we get rid of this?
00081                 rospy.logwarn("Loading a duplicate [%s]. Overwriting previous" % cur_sensor['sensor_id'])
00082             all_sensors_dict[cur_sensor['sensor_id']] = cur_sensor
00083             all_sensors_dict[cur_sensor['sensor_id']]['sensor_type'] = cur_sensor_type
00084 
00085     print "The following error sensors have been loaded:\n"
00086     # We want to sort by the types of blocks
00087     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00088 
00089     for cur_sensor_type in all_sensor_types:
00090         print "  %s sensors" % cur_sensor_type
00091         cur_sensor_ids = [cur_sensor_id for cur_sensor_id,cur_sensor in all_sensors_dict.items() if cur_sensor['sensor_type'] == cur_sensor_type]
00092         cur_sensor_ids.sort()
00093         for cur_sensor_id in cur_sensor_ids:
00094             print "   - %s" % cur_sensor_id
00095         print ""
00096 
00097     return all_sensors_dict
00098 
00099 def load_calibration_steps(steps_dict):
00100     # We want to execute the calibration in alphabetical order, based on the key names
00101     step_keys = steps_dict.keys()
00102     step_keys.sort()
00103     step_list = []
00104     for step_name in step_keys:
00105         # Add the step name to the dict (since we'll lose this info once we listify)
00106         steps_dict[step_name]["name"] = step_name
00107         step_list.append(steps_dict[step_name])
00108     print "Loaded the calibration steps to execute in the following order:"
00109     for cur_step in step_list:
00110         print " - %s" % cur_step['name']
00111     return step_list
00112 
00113 # Verifies that the given filename has the correct permissions.
00114 # Either it shouldn't exist, or it should be writable
00115 def check_file_permissions(filename):
00116     # If the file doesn't exist, then we're fine
00117     if not os.path.isfile(filename):
00118         return True
00119 
00120     statinfo = os.stat(filename);
00121 
00122     # See if the current user owns the file. If so, look at user's write priveleges
00123     if (os.geteuid() == statinfo.st_uid):
00124         return (os.stat(filename).st_mode & stat.S_IWUSR) > 0
00125 
00126     # See if the current user's group owns the file. If so, look at groups's write priveleges
00127     if (os.getgid() == statinfo.st_gid):
00128         return (os.stat(filename).st_mode & stat.S_IWGRP) > 0
00129 
00130     # Not the owner, nor part of the group, so check the 'other' permissions
00131     return (os.stat(filename).st_mode & stat.S_IWOTH) > 0
00132 
00133 
00134 def load_requested_sensors(all_sensors_dict, requested_sensors):
00135     '''
00136     Build a sensor dictionary with the subset of sensors that we request
00137     '''
00138     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00139     cur_sensors = dict([(x,[]) for x in all_sensor_types])
00140     for requested_sensor_id in requested_sensors:
00141         # We need to now find requested_sensor_id in our library of sensors
00142         if requested_sensor_id in all_sensors_dict.keys():
00143             cur_sensor_type = all_sensors_dict[requested_sensor_id]['sensor_type']
00144             cur_sensors[cur_sensor_type].append(all_sensors_dict[requested_sensor_id])
00145         else:
00146             rospy.logerr("Could not find [%s] in block library. Skipping this block", requested_sensor_id)
00147     return cur_sensors
00148 
00149 
00150 def diff(v1, v2, eps = 1e-10):
00151     ''' Determine the difference in two vectors. '''
00152     if sum( [ math.fabs(x-y) for x,y in zip(v1, v2) ] ) <= eps:
00153         return 0
00154     return 1
00155 
00156 # URDF updating -- this should probably go in a different file
00157 def update_transmission(urdf, joint, gearing):
00158     for transmission in urdf.transmissions.values():
00159         if transmission.joint == joint:
00160             transmission.reduction = transmission.reduction * gearing
00161             return
00162     print "No transmission found for:", joint
00163 
00164 def update_urdf(urdf, calibrated_params):
00165     ''' Given urdf and calibrated robot_params, updates the URDF. '''
00166     joints = list()
00167     axis = list()
00168     # update each transmission
00169     for chain in calibrated_params.chains.values():
00170         joints += chain._active
00171         axis += numpy.array(chain._axis)[0,:].tolist()
00172         for joint, gearing in zip(chain._active, chain._gearing):
00173             if gearing != 1.0:
00174                 update_transmission(urdf, joint, gearing)
00175     for laser in calibrated_params.tilting_lasers.values():
00176         joints.append(laser._config['joint'])
00177         axis.append(5) # TODO: remove this assumption
00178         if laser._gearing != 1.0:
00179             update_transmission(urdf, laser._config['joint'], laser._gearing)
00180 
00181     # update each transform (or joint calibration)
00182     for joint_name in calibrated_params.transforms.keys():
00183         try:
00184             updated = calibrated_params.transforms[joint_name]._config.T.tolist()[0]
00185             if diff(updated[0:3],  urdf.joints[joint_name].origin.position):
00186                 print 'Updating xyz for', joint_name, 'old:', urdf.joints[joint_name].origin.position, 'new:', updated[0:3]
00187                 urdf.joints[joint_name].origin.position = updated[0:3]
00188             r1 = RPY_to_angle_axis(urdf.joints[joint_name].origin.rotation)
00189             if diff(r1, updated[3:6]):
00190                 # TODO: remove assumption that joints are revolute
00191                 if joint_name in joints and urdf.joints[joint_name].calibration != None:
00192                     cal = urdf.joints[joint_name].calibration 
00193                     a = axis[joints.index(joint_name)]
00194                     a = int(a) - 1
00195                     print 'Updating calibration for', joint_name, 'by', updated[a]
00196                     if cal.rising != None:
00197                         urdf.joints[joint_name].calibration.rising += updated[a]   
00198                     if cal.falling != None:
00199                         urdf.joints[joint_name].calibration.falling += updated[a]
00200                 else:
00201                     rot = angle_axis_to_RPY(updated[3:6])
00202                     print 'Updating rpy for', joint_name, 'old:', urdf.joints[joint_name].origin.rotation, 'new:', rot
00203                     urdf.joints[joint_name].origin.rotation = rot                
00204         except KeyError:
00205             print "Joint removed:", joint_name
00206 
00207     return urdf
00208 
00209 if __name__ == '__main__':
00210     import time
00211     xml_time = time.strftime('%Y_%m_%d_%H_%M', time.localtime())
00212     calibrated_xml = 'robot_calibrated_'+xml_time+'.xml'
00213 
00214     rospy.init_node("multi_step_cov_estimator", anonymous=True)
00215     print "Starting The Multi Step [Covariance] Estimator Node\n"
00216 
00217     if (len(rospy.myargv()) < 2):
00218         usage()
00219     elif (len(rospy.myargv()) < 3):
00220         bag_filename = rospy.myargv()[1]
00221         output_dir = "."
00222     else:
00223         bag_filename = rospy.myargv()[1]
00224         output_dir = rospy.myargv()[2]
00225 
00226     print "Using Bagfile: %s\n" % bag_filename
00227     if not os.path.isfile(bag_filename):
00228         rospy.logerr("Bagfile does not exist. Exiting...")
00229         sys.exit(1)
00230 
00231     config_param_name = "calibration_config"
00232     if not rospy.has_param(config_param_name):
00233         rospy.logerr("Could not find parameter [%s]. Please populate this namespace with the estimation configuration.", config_param_name)
00234         sys.exit(1)
00235     config = rospy.get_param(config_param_name)
00236 
00237     # Process all the sensor definitions that are on the parameter server
00238     sensors_name = "sensors"
00239     if sensors_name not in config.keys():
00240         rospy.logerr("Could not find namespace [%s/%s]. Please populate this namespace with sensors.", (config_param_name, sensors_name))
00241         sys.exit(1)
00242     #sensors_dump = [yaml.load(x) for x in config[sensors_name].values()]
00243     all_sensors_dict = build_sensor_defs(config[sensors_name])
00244     all_sensor_types = list(set([x['sensor_type'] for x in all_sensors_dict.values()]))
00245 
00246     # Load all the calibration steps.
00247     step_list = load_calibration_steps(config["cal_steps"])
00248 
00249     # Count how many checkerboard poses we need to track
00250     sample_skip_list = rospy.get_param('calibration_skip_list', [])
00251     msg_count = get_robot_measurement_count(bag_filename, sample_skip_list)
00252 
00253     if 'initial_poses' in config.keys():
00254         previous_pose_guesses = numpy.array(yaml.load(config['initial_poses']))
00255     else:
00256         previous_pose_guesses = numpy.zeros([msg_count,6])
00257 
00258     # Check if we can write to all of our output files
00259     output_filenames = [calibrated_xml]
00260     for suffix in [".yaml", "_poses.yaml", "_cov.txt"]:
00261         output_filenames += [output_dir + "/" + cur_step["output_filename"] + suffix for cur_step in step_list]
00262 
00263     valid_list = [check_file_permissions(curfile) for curfile in output_filenames];
00264     permissions_valid = all(valid_list)
00265     if not permissions_valid:
00266         print "Invalid file permissions. You need to be able to write to the following files:"
00267         print "\n".join([" - " + cur_file for cur_file,cur_valid in zip(output_filenames, valid_list) if not cur_valid])
00268         sys.exit(-1)
00269 
00270     # Generate robot parameters
00271     robot_description = get_robot_description(bag_filename)
00272     robot_params = UrdfParams(robot_description, config)
00273 
00274     # Load all the sensors from the bagfile and config file
00275     for cur_step in step_list:
00276         print ""
00277         print "*"*30
00278         print "Beginning [%s]" % cur_step["name"]
00279 
00280         # Need to load only the sensors that we're interested in
00281         cur_sensors = load_requested_sensors(all_sensors_dict, cur_step['sensors'])
00282 
00283         # Load all the sensors from bag
00284         multisensors = get_multisensors(bag_filename, cur_sensors, sample_skip_list)
00285 
00286         # Display sensor count statistics
00287         print "Executing step with the following Sensors:"
00288         # Iterate over sensor definitions for this step
00289         for cur_sensor_type, cur_sensor_list in cur_sensors.items():
00290             print "  %s Sensors:" % cur_sensor_type
00291             cur_sensor_ids = [cur_sensor['sensor_id'] for cur_sensor in cur_sensor_list]
00292             cur_sensor_ids.sort()
00293             for cur_sensor_id in cur_sensor_ids:
00294                 counts = [ len([s for s in ms.sensors if s.sensor_id == cur_sensor_id]) for ms in multisensors]
00295                 count = sum(counts)
00296                 print "   - %s (%u)" % (cur_sensor_id, count)
00297             print ""
00298 
00299         print "Sensor breakdown (By Sample):"
00300         for k,ms in zip(range(len(multisensors)), multisensors):
00301             print " % 2u) %s" % (k, ", ".join([s.sensor_id for s in ms.sensors]))
00302 
00303         print "Pose Guesses:\n", previous_pose_guesses
00304 
00305         if len(multisensors) == 0:
00306             rospy.logwarn("No error blocks were generated for this optimization step. Skipping this step.  This will result in a miscalibrated sensor")
00307             output_dict = robot_params.params_to_config(robot_params.deflate())
00308             output_poses = previous_pose_guesses
00309         else:
00310             free_dict = yaml.load(cur_step["free_params"])
00311             use_cov = cur_step['use_cov']
00312             if use_cov:
00313                 print "Executing step with covariance calculations"
00314             else:
00315                 print "Executing step without covariance calculations"
00316             output_dict, output_poses, J = opt_runner(robot_params, previous_pose_guesses, free_dict, multisensors, use_cov)
00317 
00318         # Dump results to file
00319         out_f = open(output_dir + "/" + cur_step["output_filename"] + ".yaml", 'w')
00320         yaml.dump(output_dict, out_f)
00321         out_f.close()
00322 
00323         out_f = open(output_dir + "/" + cur_step["output_filename"] + "_poses.yaml", 'w')
00324         yaml.dump([list([float(x) for x in pose]) for pose in list(output_poses)], out_f)
00325         out_f.close()
00326 
00327         cov_x = matrix(J).T * matrix(J)
00328         numpy.savetxt(output_dir + "/" + cur_step["output_filename"] + "_cov.txt", cov_x, fmt="% 9.3f")
00329 
00330         previous_pose_guesses = output_poses
00331 
00332     # write out to URDF
00333     rospy.loginfo('Writing updates to %s', calibrated_xml)
00334     outfile = open(calibrated_xml, 'w')
00335     urdf = update_urdf(robot_params.get_clean_urdf(), robot_params)
00336     outfile.write( urdf.to_xml() )
00337     outfile.close()
00338 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Aug 15 2013 10:15:40