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
00038
00039
00040
00041
00042
00043
00044
00045 import numpy
00046 from numpy import reshape, array, zeros, diag, matrix, real
00047 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00048 import rospy
00049 from pr2_calibration_estimation.full_chain import FullChainRobotParams
00050 from sensor_msgs.msg import JointState
00051
00052 class ChainBundler:
00053 def __init__(self, valid_configs):
00054 self._valid_configs = valid_configs
00055
00056
00057 def build_blocks(self, M_robot):
00058 sensors = []
00059
00060 for cur_config in self._valid_configs:
00061 cur_chain_id = cur_config["chain_id"]
00062 if cur_chain_id == M_robot.chain_id and \
00063 cur_chain_id in [ x.chain_id for x in M_robot.M_chain ] :
00064 rospy.logdebug(" Found block")
00065 M_chain = [x for x in M_robot.M_chain if cur_chain_id == x.chain_id][0]
00066 cur_sensor = ChainSensor(cur_config, M_chain, M_robot.target_id)
00067 sensors.append(cur_sensor)
00068 else:
00069 rospy.logdebug(" Didn't find block")
00070 return sensors
00071
00072 class ChainSensor:
00073 def __init__(self, config_dict, M_chain, target_id):
00074
00075 self.sensor_type = "chain"
00076 self.sensor_id = config_dict["chain_id"]
00077
00078 self._config_dict = config_dict
00079 self._M_chain = M_chain
00080 self._target_id = target_id
00081
00082 self._full_chain = FullChainRobotParams(self._config_dict)
00083
00084 self.terms_per_sample = 3
00085
00086 def update_config(self, robot_params):
00087 self._full_chain.update_config(robot_params)
00088 self._checkerboard = robot_params.checkerboards[ self._target_id ]
00089
00090 def compute_residual(self, target_pts):
00091 h_mat = self.compute_expected(target_pts)
00092 z_mat = self.get_measurement()
00093 assert(h_mat.shape == z_mat.shape)
00094 assert(h_mat.shape[0] == 4)
00095 r_mat = h_mat[0:3,:] - z_mat[0:3,:]
00096 r = array(reshape(r_mat.T, [-1,1]))[:,0]
00097 return r
00098
00099 def compute_residual_scaled(self, target_pts):
00100 r = self.compute_residual(target_pts)
00101 gamma_sqrt = self.compute_marginal_gamma_sqrt(target_pts)
00102 r_scaled = gamma_sqrt * matrix(r).T
00103 return array(r_scaled.T)[0]
00104
00105 def compute_marginal_gamma_sqrt(self, target_pts):
00106 import scipy.linalg
00107
00108 cov = self.compute_cov(target_pts)
00109 gamma = matrix(zeros(cov.shape))
00110 num_pts = self.get_residual_length()/3
00111
00112 for k in range(num_pts):
00113
00114 first = 3*k
00115 last = 3*k+3
00116 sub_cov = matrix(cov[first:last, first:last])
00117 sub_gamma_sqrt_full = matrix(scipy.linalg.sqrtm(sub_cov.I))
00118 sub_gamma_sqrt = real(sub_gamma_sqrt_full)
00119 assert(scipy.linalg.norm(sub_gamma_sqrt_full - sub_gamma_sqrt) < 1e-6)
00120 gamma[first:last, first:last] = sub_gamma_sqrt
00121 return gamma
00122
00123 def compute_cov(self, target_pts):
00124 epsilon = 1e-8
00125
00126 num_joints = len(self._M_chain.chain_state.position)
00127 Jt = zeros([num_joints, self.get_residual_length()])
00128
00129 x = JointState()
00130 x.position = self._M_chain.chain_state.position[:]
00131
00132 f0 = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
00133 for i in range(num_joints):
00134 x.position = list(self._M_chain.chain_state.position[:])
00135 x.position[i] += epsilon
00136 fTest = reshape(array(self._calc_fk_target_pts(x)[0:3,:].T), [-1])
00137 Jt[i] = (fTest - f0)/epsilon
00138 cov_angles = [x*x for x in self._full_chain.calc_block._chain._cov_dict['joint_angles']]
00139
00140 cov = matrix(Jt).T * matrix(diag(cov_angles)) * matrix(Jt)
00141 return cov
00142
00143 def get_residual_length(self):
00144 pts = self._checkerboard.generate_points()
00145 N = pts.shape[1]
00146 return N*3
00147
00148 def get_measurement(self):
00149 '''
00150 Returns a 4xN matrix with the locations of the checkerboard points in homogenous coords,
00151 as per the forward kinematics of the chain
00152 '''
00153 return self._calc_fk_target_pts(self._M_chain.chain_state)
00154
00155 def _calc_fk_target_pts(self, chain_state):
00156
00157 target_pts_tip = self._checkerboard.generate_points()
00158
00159
00160 target_pose_root = self._full_chain.calc_block.fk(chain_state)
00161
00162
00163 target_pts_root = target_pose_root * target_pts_tip
00164
00165 return target_pts_root
00166
00167 def compute_expected(self, target_pts):
00168 return target_pts
00169
00170
00171 def build_sparsity_dict(self):
00172 sparsity = dict()
00173 sparsity['transforms'] = {}
00174 for cur_transform_name in ( self._config_dict['before_chain'] + self._config_dict['after_chain'] ):
00175 sparsity['transforms'][cur_transform_name] = [1, 1, 1, 1, 1, 1]
00176
00177 sparsity['dh_chains'] = {}
00178 chain_id = self._config_dict['chain_id']
00179 num_links = self._full_chain.calc_block._chain._M
00180 assert(num_links == len(self._M_chain.chain_state.position))
00181 sparsity['dh_chains'][chain_id] = {}
00182 sparsity['dh_chains'][chain_id]['dh'] = [ [1,1,1,1] ] * num_links
00183 sparsity['dh_chains'][chain_id]['gearing'] = [1] * num_links
00184
00185 sparsity['checkerboards'] = {}
00186 sparsity['checkerboards'][self._target_id] = { 'spacing_x': 1,
00187 'spacing_y': 1 }
00188
00189 return sparsity
00190
