urdf_params.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2008-2011, Willow Garage, Inc.
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of Willow Garage, Inc. nor the names of its
19 # contributors may be used to endorse or promote products derived
20 # from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 
35 # Author: Michael Ferguson
36 
37 import roslib; roslib.load_manifest('calibration_estimation')
38 import rospy
39 
40 from urdf_parser_py.urdf import *
41 import yaml
42 import numpy
43 from numpy import matrix, vsplit, sin, cos, reshape, zeros, pi
44 
45 from calibration_estimation.joint_chain import JointChain
46 from calibration_estimation.tilting_laser import TiltingLaser
47 from calibration_estimation.camera import RectifiedCamera
48 from calibration_estimation.checkerboard import Checkerboard
49 from calibration_estimation.single_transform import SingleTransform
50 from calibration_estimation.single_transform import RPY_to_angle_axis, angle_axis_to_RPY
51 
52 # Construct a dictionary of all the primitives of the specified type
53 def init_primitive_dict(start_index, config_dict, PrimitiveType):
54  cur_index = start_index
55 
56  # Init the dictionary to store the constructed primitives
57  primitive_dict = dict()
58 
59  # Process each primitive in the list of configs
60  for key, elem in config_dict.items():
61  primitive_dict[key] = PrimitiveType(elem)
62  # Store lookup indices
63  primitive_dict[key].start = cur_index
64  primitive_dict[key].end = cur_index + primitive_dict[key].get_length()
65  cur_index = primitive_dict[key].end
66 
67  # Return the primitives, as well as the end index
68  return primitive_dict, cur_index
69 
70 # Given a dictionary of initialized primitives, inflate
71 # their configuration, given a parameter vector
72 def inflate_primitive_dict(param_vec, primitive_dict):
73  for key, elem in primitive_dict.items():
74  elem.inflate(param_vec[elem.start:elem.end,0])
75 
76 # Given a dictionary of initialized primitives, deflate
77 # their configuration, into the specified parameter vector
78 def deflate_primitive_dict(param_vec, primitive_dict):
79  for key, elem in primitive_dict.items():
80  param_vec[elem.start:elem.end,0] = elem.deflate()
81 
82 # Iterate over config dictionary and determine which parameters should be free,
83 # based on the the flags in the free dictionary. Once computed, update the part
84 # of the target vector that corresponds to this primitive
85 def update_primitive_free(target_list, config_dict, free_dict):
86  for key, elem in config_dict.items():
87  if key in free_dict.keys():
88  free = elem.calc_free(free_dict[key])
89  target_list[elem.start:elem.end] = free
90 
91 # Given a parameter vector, Compute the configuration of all the primitives of a given type
92 def primitive_params_to_config(param_vec, primitive_dict):
93  config_dict = dict()
94  for key, elem in primitive_dict.items():
95  config_dict[key] = elem.params_to_config(param_vec[elem.start:elem.end,0])
96  return config_dict
97 
98 
99 # Stores the current configuration of all the primitives in the system
101  def __init__(self, raw_urdf, config):
102  urdf = URDF().parse(raw_urdf)
103  self.configure(urdf, config)
104 
105  def configure(self, urdf, config_dict):
106  self.urdf = urdf
107  self.fakes = list() # list of fake joints which must be later removed.
108 
109  # set base_link to which all measurements are based
110  try:
111  self.base_link = config_dict['base_link']
112  except:
113  self.base_link = 'base_link'
114  transforms = config_dict['transforms']
115  checkerboards = config_dict["checkerboards"]
116  config_dict = config_dict['sensors']
117 
118  # length of parameter vector
119  cur_index = 0
120 
121  # clean up joints
122  for joint_name in urdf.joint_map.keys():
123  j = urdf.joint_map[joint_name]
124  if j.origin == None:
125  j.origin = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0])
126  if j.origin.rotation == None:
127  j.origin.rotation = [0.0, 0.0, 0.0]
128  if j.origin.position == None:
129  j.origin.position = [0.0, 0.0, 0.0]
130 
131  # build our transforms
132  self.transforms = dict()
133  for joint_name in urdf.joint_map.keys():
134  joint_name = str(joint_name)
135  j = urdf.joint_map[joint_name]
136  rot = j.origin.rotation
137  rot = RPY_to_angle_axis(rot)
138  p = j.origin.position + rot
139  self.transforms[joint_name] = SingleTransform(p, joint_name)
140  self.transforms[joint_name].start = cur_index
141  self.transforms[joint_name].end = cur_index + self.transforms[joint_name].get_length()
142  cur_index = self.transforms[joint_name].end
143  for name, transform in transforms.items():
144  transform = [eval(str(x)) for x in transform]
145  transform[3:6] = RPY_to_angle_axis(transform[3:6])
146  try:
147  self.transforms[name].inflate(reshape(matrix(transform, float), (-1,1)))
148  except:
149  rospy.logwarn("Transform not found in URDF %s", name)
150  self.transforms[name] = SingleTransform(transform, name)
151  self.transforms[name].start = cur_index
152  self.transforms[name].end = cur_index + self.transforms[name].get_length()
153  cur_index = self.transforms[name].end
154 
155  # build our chains
156  self.chains = dict()
157  chain_dict = config_dict['chains']
158  for chain_name in config_dict['chains']:
159  # create configuration
160  this_config = config_dict['chains'][chain_name]
161  this_config['joints'] = urdf.get_chain(this_config['root'], this_config['tip'], links=False)
162  this_config['active_joints'] = list()
163  this_config['axis'] = list()
164  this_config['transforms'] = dict()
165  this_config['gearing'] = config_dict['chains'][chain_name]['gearing'] # TODO: This always defaults to 1.0?
166  this_config['cov'] = config_dict['chains'][chain_name]['cov']
167  for joint_name in this_config["joints"]:
168  this_config['transforms'][joint_name] = self.transforms[joint_name]
169  if urdf.joint_map[joint_name].joint_type in ['revolute','continuous'] :
170  this_config["active_joints"].append(joint_name)
171  axis = urdf.joint_map[joint_name].axis
172  this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([4,5,6], axis)] ) )
173  # we can handle limited rotations here
174  rot = urdf.joint_map[joint_name].origin.rotation
175  if rot != None and (sum([abs(x) for x in rot]) - rot[abs(this_config["axis"][-1])-4]) > 0.001:
176  print 'Joint origin is rotated, calibration will fail: ', joint_name
177  elif urdf.joint_map[joint_name].joint_type == 'prismatic':
178  this_config["active_joints"].append(joint_name)
179  axis = urdf.joint_map[joint_name].axis
180  this_config["axis"].append( sum( [i[0]*int(i[1]) for i in zip([1,2,3], axis)] ) )
181  elif urdf.joint_map[joint_name].joint_type != 'fixed':
182  print 'Unknown joint type:', urdf.joint_map[joint_name].joint_type
183  # put a checkerboard in it's hand
184  self.urdf.add_link(Link(chain_name+"_cb_link"))
185  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])))
186  self.fakes += [chain_name+"_cb_link", chain_name+"_cb"]
187  self.chains[chain_name] = JointChain(this_config)
188  self.chains[chain_name].start = cur_index
189  self.chains[chain_name].end = cur_index + self.chains[chain_name].get_length()
190  cur_index = self.chains[chain_name].end
191 
192  # build our lasers:
193  self.tilting_lasers = dict()
194  if 'tilting_lasers' in config_dict.keys():
195  self.tilting_lasers, cur_index = init_primitive_dict(cur_index, config_dict["tilting_lasers"], TiltingLaser)
196  self.rectified_cams, cur_index = init_primitive_dict(cur_index, config_dict["rectified_cams"], RectifiedCamera)
197  self.checkerboards, cur_index = init_primitive_dict(cur_index, checkerboards, Checkerboard)
198 
199  self.length = cur_index
200 
201  # Initilize free list
202  def calc_free(self, free_dict):
203  free_list = [0] * self.length
204  update_primitive_free(free_list, self.transforms, free_dict["transforms"])
205  update_primitive_free(free_list, self.chains, free_dict["chains"])
206  update_primitive_free(free_list, self.tilting_lasers, free_dict["tilting_lasers"])
207  update_primitive_free(free_list, self.rectified_cams, free_dict["rectified_cams"])
208  update_primitive_free(free_list, self.checkerboards, free_dict["checkerboards"])
209  return free_list
210 
211  def params_to_config(self, param_vec):
212  config_dict = dict()
213  config_dict["base_link"] = self.base_link
214  config_dict["transforms"] = dict()
215  for key, elem in self.transforms.items():
216  config_dict["transforms"][key] = elem.params_to_config(param_vec[elem.start:elem.end,0])
217  config_dict["transforms"][key][3:6] = angle_axis_to_RPY(config_dict["transforms"][key][3:6])
218  config_dict["sensors"] = {}
219  config_dict["sensors"]["chains"] = primitive_params_to_config(param_vec, self.chains)
220  config_dict["sensors"]["tilting_lasers"] = primitive_params_to_config(param_vec, self.tilting_lasers)
221  config_dict["sensors"]["rectified_cams"] = primitive_params_to_config(param_vec, self.rectified_cams)
222  config_dict["checkerboards"] = primitive_params_to_config(param_vec, self.checkerboards)
223  return config_dict
224 
225  def inflate(self, param_vec):
226  inflate_primitive_dict(param_vec, self.transforms)
227  for key, elem in self.chains.items():
228  elem.inflate(param_vec[elem.start:elem.end,0])
229  for joint_name in elem._joints:
230  elem._transforms[joint_name] = self.transforms[joint_name]
231  inflate_primitive_dict(param_vec, self.tilting_lasers)
232  inflate_primitive_dict(param_vec, self.rectified_cams)
233  inflate_primitive_dict(param_vec, self.checkerboards)
234 
235  def deflate(self):
236  param_vec = numpy.matrix( numpy.zeros((self.length,1), float))
237  deflate_primitive_dict(param_vec, self.transforms)
238  deflate_primitive_dict(param_vec, self.chains)
239  deflate_primitive_dict(param_vec, self.tilting_lasers)
240  deflate_primitive_dict(param_vec, self.rectified_cams)
241  deflate_primitive_dict(param_vec, self.checkerboards)
242  return param_vec
243 
244  def get_clean_urdf(self):
245  ''' Remove checkerboard links/joints. '''
246  for joint in self.urdf.joint_map.keys():
247  if joint in self.fakes:
248  self.urdf.joints.remove(self.urdf.joint_map[joint])
249  del self.urdf.joint_map[joint]
250  for link in self.urdf.link_map.keys():
251  if link in self.fakes:
252  self.urdf.links.remove(self.urdf.link_map[link])
253  del self.urdf.link_map[link]
254  return self.urdf
255 
def inflate_primitive_dict(param_vec, primitive_dict)
Definition: urdf_params.py:72
def update_primitive_free(target_list, config_dict, free_dict)
Definition: urdf_params.py:85
def __init__(self, raw_urdf, config)
Definition: urdf_params.py:101
def deflate_primitive_dict(param_vec, primitive_dict)
Definition: urdf_params.py:78
def primitive_params_to_config(param_vec, primitive_dict)
Definition: urdf_params.py:92
def configure(self, urdf, config_dict)
Definition: urdf_params.py:105
def init_primitive_dict(start_index, config_dict, PrimitiveType)
Definition: urdf_params.py:53


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Thu Jun 6 2019 19:17:16