00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import sys
00038 import xml.dom
00039
00040 def update_joint(str_in, name, xyz=None, rpy=None, ref_shift=None):
00041 '''
00042 Updates the joint's reference position and rpy terms for the input xml string
00043 '''
00044
00045 changelist = []
00046
00047 joint_span = find_split_elem_span(str_in, 'joint', name)
00048
00049 if joint_span is None:
00050 return None
00051
00052 if rpy is not None:
00053 origin_span = find_atomic_elem_span(str_in, 'origin', *joint_span)
00054 rpy_span = find_attr_span(str_in, 'rpy', *origin_span)
00055 if rpy_span is not None:
00056 changelist.append((rpy_span, "%.10f %.10f %.10f" % tuple(rpy)))
00057 xyz_span = find_attr_span(str_in, 'xyz', *origin_span)
00058 if xyz_span is not None:
00059 changelist.append((xyz_span, "%.10f %.10f %.10f" % tuple(xyz)))
00060
00061 if ref_shift is not None:
00062 calibration_span = find_atomic_elem_span(str_in, 'calibration', *joint_span)
00063 rising_span = find_attr_span(str_in, 'rising', *calibration_span)
00064 falling_span = find_attr_span(str_in, 'falling', *calibration_span)
00065 reference_position_span = find_attr_span(str_in, 'reference_position', *calibration_span)
00066 for cur_cal_span in [rising_span, falling_span, reference_position_span]:
00067 if cur_cal_span is not None:
00068 orig_val = float(str_in[cur_cal_span[0]:cur_cal_span[1]])
00069 changelist.append((cur_cal_span, "%.10f" % (orig_val + ref_shift)))
00070
00071 return changelist
00072
00073 def update_transmission(str_in, name, reduction_scale):
00074 '''
00075 Updates a transmission
00076 new_reduction = original_recuction * reduction_scale
00077 '''
00078 changelist = []
00079 trans_span = find_split_elem_span(str_in, 'transmission', name)
00080 if trans_span is None:
00081 return None
00082
00083 mech_red_span = find_split_elem_span(str_in, 'mechanicalReduction', None, *trans_span)
00084 if mech_red_span is None:
00085 return None
00086
00087 internal_span = find_split_internals(str_in, *mech_red_span)
00088 if internal_span is None:
00089 return None
00090
00091 reduction_orig = float(str_in[internal_span[0]:internal_span[1]])
00092 reduction_new = reduction_orig * reduction_scale
00093 changelist.append( (internal_span, "%.12f" % reduction_new) )
00094
00095 return changelist
00096
00097 def find_split_internals(str_in, start=0, end=None):
00098 '''
00099 Find everything enclosed by a split element tag
00100 '''
00101 if end is None:
00102 end = len(str_in)
00103
00104
00105 internals_start = str_in.find(">", start, end)+1
00106
00107
00108 left_arrow = str_in.find('<', start, end)
00109 if left_arrow < 0:
00110 print "Error: Couldn't any '<' in text"
00111 print str_in[start:end]
00112 sys.exit(-1)
00113
00114 while left_arrow >= 0:
00115 internals_end = left_arrow
00116 left_arrow = str_in.find('<', left_arrow+1, end)
00117
00118 return (internals_start, internals_end)
00119
00120 def find_attr_span(str_in, name, start=0, end=None):
00121 '''
00122 Find the span of an attribute in an element. The resulting start to (end+1) will
00123 display everything within the quotations.
00124 '''
00125
00126 if end is None:
00127 end = len(str_in)
00128
00129 attr_types = ["%s=\"" % name,
00130 "%s= \"" % name,
00131 "%s =\"" % name,
00132 "%s = \"" % name]
00133
00134 for cur_type in attr_types:
00135 attr_name_start = str_in.find(cur_type, start, end)
00136
00137 if attr_name_start >= 0:
00138 attr_start = attr_name_start + len(cur_type)
00139 attr_end = str_in.find('\"', attr_start, end)
00140 if attr_end < 0:
00141 print "Error: Unmatch quotations:"
00142 print str_in[start:end]
00143 return None
00144 return (attr_start, attr_end)
00145
00146 return None
00147
00148 def find_atomic_elem_span(str_in, node_name, start=0, end=None):
00149 '''
00150 Find the span of an element that is not 'split', like <elem atttr1="" attr2="" />, and notr <elem></elem>
00151 '''
00152
00153
00154 if end is None:
00155 end = len(str_in)
00156
00157 elem_start = str_in.find("<%s"%node_name, start, end)
00158 if elem_start < 0:
00159 print "Could not find [%s]" % node_name
00160 print str_in[elem_start:end]
00161 return None
00162
00163 elem_end = str_in.find("/>", elem_start, end)
00164 if elem_end < 0:
00165 print "Error: Could not find end of [%s] elem" % node_name
00166 print str_in[elem_start:end]
00167 return None
00168 return (elem_start, elem_end + len("/>"))
00169
00170
00171 def find_split_elem_span(str_in, elem_name, attr_name=None, start=0, end=None):
00172 '''
00173 Find the span of a xml node. This function assumes that it doesn't use an '/>'. Thus it should look like <elem> </elem>, and not <elem/>.
00174 '''
00175 if end is None:
00176 end = len(str_in)
00177
00178 elem_start = str_in.find("<%s" % elem_name, start, end)
00179 while elem_start >= 0:
00180 elem_slash_end = str_in.find("/>", elem_start, end)
00181 elem_end = str_in.find(">", elem_start, end)
00182 if elem_end == -1:
00183 print "ERROR: Found [<%s] without matching close [>]" % elem_name
00184 print str_in[elem_start:]
00185 sys.exit(-1)
00186
00187
00188 if elem_slash_end == -1 or elem_end < elem_slash_end:
00189
00190 block_delim = str_in.find("</%s>" % elem_name, elem_start, end)
00191 if block_delim < 0:
00192 print "ERROR: Didn't find </%s>" % elem_name
00193 print str_in[elem_start]
00194 block_delim += len("</%s>"%elem_name)
00195
00196
00197 if attr_name is None:
00198 break
00199
00200
00201 if "%s:"%elem_name not in str_in[elem_start:block_delim]:
00202
00203
00204 joint_dom = xml.dom.minidom.parseString(str_in[elem_start:block_delim])
00205
00206 if joint_dom.documentElement.getAttribute('name') == attr_name:
00207 print "Found [%s]" % attr_name
00208 break
00209 else:
00210
00211 pass
00212
00213 elem_start = str_in.find("<%s"%elem_name, elem_start+1, end)
00214
00215 if elem_start < 0:
00216 return None
00217
00218 return elem_start, block_delim
00219
