urdf_params.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2008-2011, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of Willow Garage, Inc. nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 # Author: Michael Ferguson
00036 
00037 import roslib; roslib.load_manifest('calibration_estimation')
00038 import rospy
00039 
00040 from urdf_parser_py.urdf import *
00041 import yaml
00042 import numpy
00043 from numpy import matrix, vsplit, sin, cos, reshape, zeros, pi
00044 
00045 from calibration_estimation.joint_chain import JointChain
00046 from calibration_estimation.tilting_laser import TiltingLaser
00047 from calibration_estimation.camera import RectifiedCamera
00048 from calibration_estimation.checkerboard import Checkerboard
00049 from calibration_estimation.single_transform import SingleTransform
00050 from calibration_estimation.single_transform import RPY_to_angle_axis, angle_axis_to_RPY
00051 
00052 # Construct a dictionary of all the primitives of the specified type
00053 def init_primitive_dict(start_index, config_dict, PrimitiveType):
00054     cur_index = start_index
00055 
00056     # Init the dictionary to store the constructed primitives
00057     primitive_dict = dict()
00058 
00059     # Process each primitive in the list of configs
00060     for key, elem in config_dict.items():
00061         primitive_dict[key] = PrimitiveType(elem)
00062         # Store lookup indices
00063         primitive_dict[key].start = cur_index
00064         primitive_dict[key].end = cur_index + primitive_dict[key].get_length()
00065         cur_index = primitive_dict[key].end
00066 
00067     # Return the primitives, as well as the end index
00068     return primitive_dict, cur_index
00069 
00070 # Given a dictionary of initialized primitives, inflate
00071 # their configuration, given a parameter vector
00072 def inflate_primitive_dict(param_vec, primitive_dict):
00073     for key, elem in primitive_dict.items():
00074         elem.inflate(param_vec[elem.start:elem.end,0])
00075 
00076 # Given a dictionary of initialized primitives, deflate
00077 # their configuration, into the specified parameter vector
00078 def deflate_primitive_dict(param_vec, primitive_dict):
00079     for key, elem in primitive_dict.items():
00080         param_vec[elem.start:elem.end,0] = elem.deflate()
00081 
00082 # Iterate over config dictionary and determine which parameters should be free,
00083 # based on the the flags in the free dictionary. Once computed, update the part
00084 # of the target vector that corresponds to this primitive
00085 def update_primitive_free(target_list, config_dict, free_dict):
00086     for key, elem in config_dict.items():
00087         if key in free_dict.keys():
00088             free = elem.calc_free(free_dict[key])
00089             target_list[elem.start:elem.end] = free
00090 
00091 # Given a parameter vector, Compute the configuration of all the primitives of a given type
00092 def primitive_params_to_config(param_vec, primitive_dict):
00093     config_dict = dict()
00094     for key, elem in primitive_dict.items():
00095         config_dict[key] = elem.params_to_config(param_vec[elem.start:elem.end,0])
00096     return config_dict
00097 
00098 
00099 # Stores the current configuration of all the primitives in the system
00100 class UrdfParams:
00101     def __init__(self, raw_urdf, config):
00102         urdf = URDF().parse(raw_urdf)
00103         self.configure(urdf, config)
00104 
00105     def configure(self, urdf, config_dict):
00106         self.urdf = urdf
00107         self.fakes = list() # list of fake joints which must be later removed.
00108 
00109         # set base_link to which all measurements are based
00110         try:
00111             self.base_link = config_dict['base_link']
00112         except:
00113             self.base_link = 'base_link'
00114         transforms = config_dict['transforms']
00115         checkerboards = config_dict["checkerboards"]
00116         config_dict = config_dict['sensors']
00117 
00118         # length of parameter vector
00119         cur_index = 0
00120 
00121         # clean up joints
00122         for joint_name in urdf.joint_map.keys():
00123             j = urdf.joint_map[joint_name]
00124             if j.origin == None:
00125                 j.origin = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
00126             if j.origin.rotation == None:
00127                 j.origin.rotation = [0.0, 0.0, 0.0]
00128             if j.origin.position == None:
00129                 j.origin.position = [0.0, 0.0, 0.0]
00130 
00131         # build our transforms
00132         self.transforms = dict()
00133         for joint_name in urdf.joint_map.keys():
00134             joint_name = str(joint_name)
00135             j = urdf.joint_map[joint_name]
00136             rot = j.origin.rotation
00137             rot = RPY_to_angle_axis(rot)
00138             p = j.origin.position + rot
00139             self.transforms[joint_name] = SingleTransform(p, joint_name)
00140             self.transforms[joint_name].start = cur_index
00141             self.transforms[joint_name].end = cur_index + self.transforms[joint_name].get_length()
00142             cur_index = self.transforms[joint_name].end
00143         for name, transform in transforms.items():
00144             transform = [eval(str(x)) for x in transform]
00145             transform[3:6] = RPY_to_angle_axis(transform[3:6])
00146             try:
00147                 self.transforms[name].inflate(reshape(matrix(transform, float), (-1,1)))
00148             except:
00149                 rospy.logwarn("Transform not found in URDF %s", name)
00150                 self.transforms[name] = SingleTransform(transform, name)
00151                 self.transforms[name].start = cur_index
00152                 self.transforms[name].end = cur_index + self.transforms[name].get_length()
00153                 cur_index = self.transforms[name].end
00154 
00155         # build our chains
00156         self.chains = dict()
00157         chain_dict = config_dict['chains']
00158         for chain_name in config_dict['chains']:            
00159             # create configuration
00160             this_config = config_dict['chains'][chain_name]
00161             this_config['joints'] = urdf.get_chain(this_config['root'], this_config['tip'], links=False)
00162             this_config['active_joints'] = list()
00163             this_config['axis'] = list()
00164             this_config['transforms'] = dict()
00165             this_config['gearing'] = config_dict['chains'][chain_name]['gearing']   # TODO: This always defaults to 1.0?
00166             this_config['cov'] = config_dict['chains'][chain_name]['cov']
00167             for joint_name in this_config["joints"]:
00168                 this_config['transforms'][joint_name] = self.transforms[joint_name]
00169                 if urdf.joint_map[joint_name].joint_type in ['revolute','continuous'] :
00170                     this_config["active_joints"].append(joint_name)
00171                     axis = urdf.joint_map[joint_name].axis
00172                     this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([4,5,6], axis)] ) )
00173                     # we can handle limited rotations here
00174                     rot = urdf.joint_map[joint_name].origin.rotation
00175                     if rot != None and (sum([abs(x) for x in rot]) - rot[abs(this_config["axis"][-1])-4]) > 0.001:   
00176                         print 'Joint origin is rotated, calibration will fail: ', joint_name
00177                 elif urdf.joint_map[joint_name].joint_type == 'prismatic':
00178                     this_config["active_joints"].append(joint_name)
00179                     axis = urdf.joint_map[joint_name].axis
00180                     this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([1,2,3], axis)] ) )
00181                 elif urdf.joint_map[joint_name].joint_type != 'fixed':
00182                     print 'Unknown joint type:', urdf.joint_map[joint_name].joint_type
00183             # put a checkerboard in it's hand
00184             self.urdf.add_link(Link(chain_name+"_cb_link"))
00185             self.urdf.add_joint(Joint(chain_name+"_cb",this_config['tip'],chain_name+"_cb_link","fixed",origin=Pose([0.0,0.0,0.0],[0.0,0.0,0.0])))
00186             self.fakes += [chain_name+"_cb_link", chain_name+"_cb"]
00187             self.chains[chain_name] = JointChain(this_config)
00188             self.chains[chain_name].start = cur_index
00189             self.chains[chain_name].end = cur_index + self.chains[chain_name].get_length()
00190             cur_index = self.chains[chain_name].end
00191 
00192         # build our lasers:
00193         self.tilting_lasers = dict()
00194         if 'tilting_lasers' in config_dict.keys():
00195             self.tilting_lasers, cur_index = init_primitive_dict(cur_index, config_dict["tilting_lasers"], TiltingLaser)
00196         self.rectified_cams,     cur_index = init_primitive_dict(cur_index, config_dict["rectified_cams"], RectifiedCamera)
00197         self.checkerboards,      cur_index = init_primitive_dict(cur_index, checkerboards,   Checkerboard)
00198 
00199         self.length = cur_index
00200 
00201     # Initilize free list
00202     def calc_free(self, free_dict):
00203         free_list = [0] * self.length
00204         update_primitive_free(free_list, self.transforms, free_dict["transforms"])
00205         update_primitive_free(free_list, self.chains, free_dict["chains"])
00206         update_primitive_free(free_list, self.tilting_lasers, free_dict["tilting_lasers"])
00207         update_primitive_free(free_list, self.rectified_cams, free_dict["rectified_cams"])
00208         update_primitive_free(free_list, self.checkerboards, free_dict["checkerboards"])
00209         return free_list
00210 
00211     def params_to_config(self, param_vec):
00212         config_dict = dict()
00213         config_dict["base_link"] = self.base_link
00214         config_dict["transforms"] = dict()
00215         for key, elem in self.transforms.items():
00216             config_dict["transforms"][key] = elem.params_to_config(param_vec[elem.start:elem.end,0])
00217             config_dict["transforms"][key][3:6] = angle_axis_to_RPY(config_dict["transforms"][key][3:6])
00218         config_dict["sensors"] = {}
00219         config_dict["sensors"]["chains"]         = primitive_params_to_config(param_vec, self.chains)
00220         config_dict["sensors"]["tilting_lasers"] = primitive_params_to_config(param_vec, self.tilting_lasers)
00221         config_dict["sensors"]["rectified_cams"] = primitive_params_to_config(param_vec, self.rectified_cams)
00222         config_dict["checkerboards"]  = primitive_params_to_config(param_vec, self.checkerboards)
00223         return config_dict
00224 
00225     def inflate(self, param_vec):
00226         inflate_primitive_dict(param_vec, self.transforms)
00227         for key, elem in self.chains.items():
00228             elem.inflate(param_vec[elem.start:elem.end,0])
00229             for joint_name in elem._joints:
00230                 elem._transforms[joint_name] = self.transforms[joint_name]
00231         inflate_primitive_dict(param_vec, self.tilting_lasers)
00232         inflate_primitive_dict(param_vec, self.rectified_cams)
00233         inflate_primitive_dict(param_vec, self.checkerboards)
00234 
00235     def deflate(self):
00236         param_vec = numpy.matrix( numpy.zeros((self.length,1), float))
00237         deflate_primitive_dict(param_vec, self.transforms)
00238         deflate_primitive_dict(param_vec, self.chains)
00239         deflate_primitive_dict(param_vec, self.tilting_lasers)
00240         deflate_primitive_dict(param_vec, self.rectified_cams)
00241         deflate_primitive_dict(param_vec, self.checkerboards)
00242         return param_vec
00243 
00244     def get_clean_urdf(self):
00245         ''' Remove checkerboard links/joints. '''
00246         for joint in self.urdf.joint_map.keys():
00247             if joint in self.fakes:
00248                 self.urdf.joints.remove(self.urdf.joint_map[joint])
00249                 del self.urdf.joint_map[joint]
00250         for link in self.urdf.link_map.keys():
00251             if link in self.fakes:
00252                 self.urdf.links.remove(self.urdf.link_map[link])
00253                 del self.urdf.link_map[link]
00254         return self.urdf
00255 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Wed Aug 26 2015 10:56:21