update_pr2_se_urdf.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, 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: Vijay Pradeep
00036 
00037 # TODO This file has not actually been checked for any PR2 SE compatability.
00038 
00039 import pr2_calibration_propagation.update_joint as update_joint
00040 import pr2_calibration_propagation.process_changelist as process_changelist
00041 import tf.transformations as transformations
00042 import math
00043 
00044 def update_urdf(initial_system, calibrated_system, xml_in):
00045     #find dh_param offsets for all requested dh chains
00046     dh_offsets = {"right_arm_chain":[],
00047                   "left_arm_chain":[],
00048                   "head_chain":[]}
00049 
00050     dh_joint_names = {"right_arm_chain" : ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'],
00051                       "left_arm_chain"  : ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'],
00052                       "head_chain"      : ['head_pan_joint', 'head_tilt_joint'] }
00053 
00054     # Check that the chains are in fact in the yaml system config
00055     chains_to_remove = [x for x in dh_offsets.keys() if x not in initial_system['dh_chains'].keys()];
00056     print "Need to ignore the following chains:", chains_to_remove
00057     for chain_to_remove in chains_to_remove:
00058       del dh_offsets[chain_to_remove]
00059 
00060     print "Computing All dh chain offsets"
00061     for chain_name in dh_offsets.keys():
00062         dh_offsets[chain_name] = find_dh_param_offsets(chain_name, initial_system, calibrated_system)
00063         print "%s offsets:" % chain_name, pplist(dh_offsets[chain_name])
00064 
00065     # Need to be able to lookup the joint offset for each joint
00066     joint_offsets_list = []
00067     for chain_name in dh_offsets.keys():
00068         joint_offsets_list.extend(zip(dh_joint_names[chain_name], dh_offsets[chain_name]))
00069     joint_offsets = dict(joint_offsets_list)
00070 
00071     #convert transforms to rpy
00072     transformdict = dict()
00073     for(name, rotvect) in calibrated_system['transforms'].iteritems():
00074         floatvect = [mixed_to_float(x) for x in rotvect]
00075         #print name, pplist(floatvect), angle_axis_to_RPY(floatvect[3:6])
00076         transformdict[name] = [floatvect[0:3], angle_axis_to_RPY(floatvect[3:6])]
00077 
00078     # Hack in transforms for tilting laser
00079 
00080     # PR2 Lite doesn't have a tilt laser
00081     #floatvect = [mixed_to_float(x) for x in calibrated_system['tilting_lasers']['tilt_laser']['before_joint'] ]
00082     #transformdict['laser_tilt_mount_joint'] =  [floatvect[0:3], angle_axis_to_RPY(floatvect[3:6])]
00083 
00084     #print "Floatvec: ", floatvec
00085     #print "tuple: ", transformdict['laser_tilt_mount_joint']
00086     #import code; code.interact(local=locals())
00087     #assert(False)
00088 
00089     #floatvect = [mixed_to_float(x) for x in calibrated_system['tilting_lasers']['tilt_laser']['after_joint'] ]
00090     #transformdict['laser_tilt_joint'] = [floatvect[0:3], angle_axis_to_RPY(floatvect[3:6])]
00091 
00092 
00093     # Combine the transforms and joint offsets into a single dict
00094     joints_dict = dict([(joint_name, [None, None, None]) for joint_name in transformdict.keys() + joint_offsets.keys()])
00095     for joint_name, val in transformdict.items():
00096         joints_dict[joint_name][0] = val[0]
00097         joints_dict[joint_name][1] = val[1]
00098 
00099     for joint_name, offset in joint_offsets.items():
00100         joints_dict[joint_name][2] = offset
00101 
00102     not_found = joints_dict.keys()
00103     changelist = []
00104 
00105     for joint_name, val in joints_dict.items():
00106         cur_cl = update_joint.update_joint(xml_in, joint_name, xyz=val[0], rpy=val[1], ref_shift=val[2])
00107         if cur_cl is not None:
00108             not_found.remove(joint_name)
00109             changelist.extend(cur_cl)
00110 
00111     # Hack to change laser gearing. Assumes that the laser reference position is 0
00112 
00113     # PR2 Lite Doesn't have a tilt laser
00114     #reduction_scale = 1.0 / calibrated_system['tilting_lasers']['tilt_laser']['gearing']
00115     #cur_cl = update_joint.update_transmission(xml_in, 'laser_tilt_mount_trans', reduction_scale)
00116     #changelist.extend(cur_cl)
00117 
00118     print "jointnames never found: ", not_found
00119 
00120     for span, result in changelist:
00121         print "\"%s\" -> \"%s\"" % (xml_in[span[0]:span[1]], result)
00122 
00123     xml_out = process_changelist.process_changelist(changelist, xml_in)
00124 
00125     return xml_out
00126 
00127 #pretty-print list to string
00128 def pplist(list):
00129     zeroed = []
00130     for x in list:
00131         if x is None:
00132             zeroed.append(0.)
00133         else:
00134             zeroed.append(x)
00135     return ' '.join(['%2.3f'%x for x in zeroed])
00136 
00137 
00138 #return 1 if value1 and value2 are within eps of each other, 0 otherwise
00139 def epsEq(value1, value2, eps = 1e-10):
00140     if math.fabs(value1-value2) <= eps:
00141         return 1
00142     return 0
00143 
00144 
00145 #convert a float/int/string containing 'pi' to just float
00146 def mixed_to_float(mixed):
00147     pi = math.pi
00148     if type(mixed) == str:
00149         try:
00150             val = eval(mixed)
00151         except:
00152             print >> sys.stderr, "bad value:", mixed, "substituting zero!!\n\n"
00153             val = 0.
00154     else:
00155         val = float(mixed)
00156     return val
00157 
00158 
00159 #calculate calibration offsets (whicharm = 'left' or 'right')
00160 def find_dh_param_offsets(chain_name, system_default, system_calibrated):
00161     offsets = []
00162     for (default_params, calib_params) in zip(system_default['dh_chains'][chain_name]['dh'], system_calibrated['dh_chains'][chain_name]['dh']):
00163         #print "default_params[0]:", default_params[0], "calib_params[0]:", calib_params[0]
00164         diff = mixed_to_float(calib_params[0]) - mixed_to_float(default_params[0])
00165         if epsEq(diff, 0):
00166             diff = None
00167         offsets.append(diff)
00168 
00169     return offsets
00170 
00171 
00172 
00173 #convert from rotation-axis-with-angle-as-magnitude representation to Euler RPY
00174 def angle_axis_to_RPY(vec):
00175     angle = math.sqrt(sum([vec[i]**2 for i in range(3)]))
00176     hsa = math.sin(angle/2.)
00177     if epsEq(angle, 0):
00178         return (0.,0.,0.)
00179     quat = [vec[0]/angle*hsa, vec[1]/angle*hsa, vec[2]/angle*hsa, math.cos(angle/2.)]
00180     rpy = quat_to_rpy(quat)
00181     return rpy
00182 
00183 def rpy_to_quat(rpy):
00184     return transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2], 'sxyz')
00185 
00186 def quat_to_rpy(q):
00187     rpy = transformations.euler_from_quaternion(q, 'sxyz')
00188     return rpy
00189 
00190 def parse_rpy(line):
00191     return [float(x) for x in line.split("rpy=\"")[1].split("\"")[0].split()]
00192 
00193 def parse_xyz(line):
00194     return [float(x) for x in line.split("xyz=\"")[1].split("\"")[0].split()]
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


pr2_se_calibration_launch
Author(s): Adam Leeper, Vijay Pradeep
autogenerated on Mon Aug 19 2013 10:50:53