def loadSystem1(): calc_block1 = FullChainCalcBlock() calc_block2 = FullChainCalcBlock() before_chain_Ts = [SingleTransform([00, 0, 0, 0, 0, 0])] chain1 = DhChain({ 'dh': [[-2.8407, math.pi / 2, 0, 0.1915], [0, -math.pi / 2, 0, 0], [math.pi, math.pi / 2, 0, 0.4], [0, -math.pi / 2, 0, 0], [math.pi, math.pi / 2, 0, 0.39], [0, -math.pi / 2, 0, 0], [0, 0, 0, 0.057]], 'gearing': [1] * 7, 'cov': { 'joint_angles': [0.01] * 7 } }) dh_link_num1 = 7 after_chain_Ts = [SingleTransform([0, 0, 0, 0, 0, 0])] last_link = 'arm_7_link' first_link = 'arm_0_link' dh_chain = ['arm_%s_joint' % (i + 1) for i in range(7)] chain2 = DhChain({'cov': {'joint_angles': [0.01] * 7}}) calc_block1.update_config(before_chain_Ts, chain1, dh_link_num1, after_chain_Ts) calc_block2.update_config(before_chain_Ts, chain2, None, after_chain_Ts, first_link, last_link, dh_chain, 'arm') return (calc_block1, calc_block2)
def loadSystem1(): calc_block1 = FullChainCalcBlock() calc_block2 = FullChainCalcBlock() before_chain_Ts = [SingleTransform([10, 0, 0, 0, 0, 0])] chain1 = DhChain({ 'dh': [[0, math.pi / 2, 0, 0], [0, -math.pi / 2, 0, -0.2565], [0, 0, 0, 0]], 'gearing': [1, 1, 1], 'cov': { 'joint_angles': [0.01] * 3 } }) dh_link_num1 = 3 after_chain_Ts = [SingleTransform([0, 0, 20, 0, 0, 0])] last_link = 'torso_upper_neck_tilt_link' first_link = 'torso_0_link' dh_chain = [ 'torso_lower_neck_tilt_joint', 'torso_pan_joint', 'torso_upper_neck_tilt_joint' ] chain2 = DhChain({'cov': {'joint_angles': [0.01] * 3}}) calc_block1.update_config(before_chain_Ts, chain1, dh_link_num1, after_chain_Ts) calc_block2.update_config(before_chain_Ts, chain2, None, after_chain_Ts, first_link, last_link, dh_chain, 'torso') return (calc_block1, calc_block2)
def test_inflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) st.inflate(reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1))) expected = numpy.matrix( [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def test_free(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) free_list = st.calc_free([0, 0, 1, 1, 0, 0]) self.assertEqual(free_list[0], False) self.assertEqual(free_list[1], False) self.assertEqual(free_list[2], True) self.assertEqual(free_list[3], True) self.assertEqual(free_list[4], False) self.assertEqual(free_list[5], False)
def test_free(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) free_list = st.calc_free( [0, 0, 1, 1, 0, 0] ) self.assertEqual(free_list[0], False) self.assertEqual(free_list[1], False) self.assertEqual(free_list[2], True) self.assertEqual(free_list[3], True) self.assertEqual(free_list[4], False) self.assertEqual(free_list[5], False)
def test_deflate(self): st = SingleTransform([0, 0, 0, 0, 1, 0]) for i in range(5): print "next test_deflate" p = numpy.random.rand(6, 1) st.inflate(p) result = st.deflate() print result print p self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
def loadSystem1(): calc_block = FullChainCalcBlock() before_chain_Ts = [SingleTransform([10, 0, 0, 0, 0, 0])] chain = DhChain( {'dh':[ [0, 0, 1, 0] ], 'gearing':[1], 'cov':{'joint_angles':[1]}} ) dh_link_num = -1 after_chain_Ts = [SingleTransform([ 0, 0, 20, 0, 0, 0])] calc_block.update_config(before_chain_Ts, chain, dh_link_num, after_chain_Ts) return calc_block
def test_inflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) st.inflate( reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )) expected = numpy.matrix( [[ 1, 0, 0, 1], [ 0, 1, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1]], float ) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
def test_equality(self): st = SingleTransform() for i in range(5): print "------next-------" p = numpy.random.rand(6, 1) new = st.inflate(p, True) old = st.inflate_old(p) print new print old print "diff: " print old - new self.assertAlmostEqual(numpy.linalg.norm(old - new), 0.0, 6)
def calculate_error(self, opt_all_vec): print "x ", sys.stdout.flush() opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) full_param_vec = self.calculate_full_param_vec(opt_param_vec) # Update the primitives with the new set of parameters self._robot_params.inflate(full_param_vec) # Update all the blocks' configs for multisensor in self._multisensors: multisensor.update_config(self._robot_params) r_list = [] for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)): # Process cb pose cb_points = SingleTransform( cb_pose_vec).transform * self._robot_params.checkerboards[ multisensor.checkerboard].generate_points() if (self._use_cov): r_list.append(multisensor.compute_residual_scaled(cb_points)) else: r_list.append(multisensor.compute_residual(cb_points)) #import code; code.interact(local=locals()) r_vec = concatenate(r_list) rms_error = numpy.sqrt(numpy.mean(r_vec**2)) print "%.3f " % rms_error, sys.stdout.flush() return array(r_vec)
def test_hard(self): sample_nums = range(1, 6) params_filenames = [ "test/data/single_transform_data/params_%02u.txt" % n for n in sample_nums ] transform_filenames = [ "test/data/single_transform_data/transform_%02u.txt" % n for n in sample_nums ] for params_filename, transform_filename in zip(params_filenames, transform_filenames): f_params = open(params_filename) f_transforms = open(transform_filename) param_elems = [float(x) for x in f_params.read().split()] transform_elems = [float(x) for x in f_transforms.read().split()] st = SingleTransform(param_elems) expected_result = numpy.matrix(transform_elems) expected_result.shape = 4, 4 self.assertAlmostEqual( numpy.linalg.norm(st.transform - expected_result), 0.0, 4, "Failed on %s" % params_filename)
def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor): """ Generates the jacobian from a target pose to the multisensor's residual. Input: - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing) - pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis - multisensor: The actual multisensor definition. Output: - J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the target's pose is encoded using 6 parameters. If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma is the information matrix for this measurement. """ # Update the primitives with the new set of parameters full_param_vec = self.calculate_full_param_vec(opt_param_vec) self._robot_params.inflate(full_param_vec) multisensor.update_config(self._robot_params) cb_model = self._robot_params.checkerboards[multisensor.checkerboard] local_cb_points = cb_model.generate_points() # based on code from scipy.slsqp x0 = pose_param_vec epsilon = 1e-6 world_pts = SingleTransform(x0).transform * local_cb_points f0 = multisensor.compute_residual(world_pts) gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts) Jt = numpy.zeros([len(x0), len(f0)]) dx = numpy.zeros(len(x0)) for i in range(len(x0)): dx[i] = epsilon test_vec = x0 + dx fTest = multisensor.compute_residual( SingleTransform(test_vec).transform * local_cb_points) Jt[i] = (fTest - f0) / epsilon #import code; code.interact(local=locals()) dx[i] = 0.0 J = Jt.transpose() if (self._use_cov): J_scaled = gamma_sqrt * J else: J_scaled = J return J_scaled
def test_easy_rot_1(self): st = SingleTransform([0, 0, 0, 0, 0, pi / 2]) expected = numpy.matrix( [[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def test_easy_trans_1(self): st = SingleTransform([1, 2, 3, 0, 0, 0]) expected = numpy.matrix( [[1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def generate_transformation_dict(transformation_list, listener): transformation_dict = {} for i in range(len(transformation_list)): if transformation_list[i] != '**' and transformation_list[i] != '': try: if transformation_list[i + 1] in ['', '**']: continue except IndexError: pass else: transform = get_single_transform(transformation_list[i], transformation_list[i + 1], listener) print transform t = SingleTransform() t.inflate_rpy(reshape(transform,(6,1))) transform = reshape(t.deflate(),(1,6)).tolist()[0] print "new format:", transform transformation_dict[transformation_list[i + 1]] = transform return transformation_dict
def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr): errors_dict = {} # Compute the error for each sensor type for ms, k in zip(multisensors, range(len(multisensors))): cb = error_calc._robot_params.checkerboards[ms.checkerboard] cb_pose_vec = opt_pose_arr[k] target_pts = SingleTransform( cb_pose_vec).transform * cb.generate_points() for sensor in ms.sensors: r_sensor = sensor.compute_residual(target_pts) * numpy.sqrt( sensor.terms_per_sample ) # terms per sample is a hack to find rms distance, instead of a pure rms, based on each term if sensor.sensor_id not in errors_dict.keys(): errors_dict[sensor.sensor_id] = [] errors_dict[sensor.sensor_id].append(r_sensor) return errors_dict
def test_inflate_old_vs_new_4(self): p = [0, 0, 1, numpy.pi / 5, 0, 0] st = SingleTransform(p) self.assertAlmostEqual( numpy.linalg.norm(st.transform - st.inflate_old(p)), 0.0, 4)
def test_inflate_old_vs_new_4(self): p=[0,0,1,numpy.pi/5,0,0] st=SingleTransform(p) self.assertAlmostEqual(numpy.linalg.norm(st.transform-st.inflate_old(p)), 0.0,4)
def test_deflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1)) st.inflate(p) result = st.deflate() self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
def test_params_to_config(self): st = SingleTransform() p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1)) config = st.params_to_config(p) self.assertAlmostEqual(config[0], 1, 6) self.assertAlmostEqual(config[1], 0, 6)
def test_length(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) self.assertEqual(st.get_length(), 6)
def test_deflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ) st.inflate(p) result = st.deflate() self.assertAlmostEqual(numpy.linalg.norm(p-result), 0.0, 6)
def test_params_to_config(self): st = SingleTransform() p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ) config = st.params_to_config(p) self.assertAlmostEqual( config[0], 1, 6) self.assertAlmostEqual( config[1], 0, 6)
def calculate_jacobian(self, opt_all_vec): """ Full Jacobian: The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows: [ J_multisensor_0 ] J = [ J_multisensor_1 ] [ : ] [ J_multisensor_M ] where M is the number of multisensors, which is generally the number of calibration samples collected. Multisensor Jacobian: The Jacobian for a given multisensor is created by concatenating jacobians for each sensor [ J_sensor_m_0 ] J_multisensor_m = [ J_sensor_m_1 ] [ : ] [ J_sensor_m_S ] where S is the number of sensors in this multisensor. Sensor Jacobian: A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ] The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual. J_params_m_s shows how modifying the free system parameters affects the residual. Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that was viewed by the sensors in this multisensor. """ sys.stdout.write("J-") sys.stdout.flush() #import scipy.optimize.slsqp.approx_jacobian as approx_jacobian #J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6) opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) # Allocate the full jacobian matrix ms_r_len = [ms.get_residual_length() for ms in self._multisensors] J = zeros([sum(ms_r_len), len(opt_all_vec)]) # Calculate at which row each multisensor starts and ends ms_end_row = list(cumsum(ms_r_len)) ms_start_row = [0] + ms_end_row[:-1] # Calculate at which column each multisensor starts and ends ms_end_col = list( cumsum([6] * len(self._multisensors)) + len(opt_param_vec)) ms_start_col = [x - 6 for x in ms_end_col] for i, ms in zip(range(len(self._multisensors)), self._multisensors): # Populate the parameter section for this multisensor J_ms_params = J[ms_start_row[i]:ms_end_row[i], 0:len(opt_param_vec)] s_r_len = [s.get_residual_length() for s in ms.sensors] s_end_row = list(cumsum(s_r_len)) s_start_row = [0] + s_end_row[:-1] #import code; code.interact(local=locals()) target_pose_T = SingleTransform(full_pose_arr[i, :]).transform # Fill in parameter section one sensor at a time for k, s in zip(range(len(ms.sensors)), ms.sensors): J_s_params = J_ms_params[s_start_row[k]:s_end_row[k], :] J_s_params[:, :] = self.single_sensor_params_jacobian( opt_param_vec, target_pose_T, ms.checkerboard, s) # Populate the pose section for this multisensor J_ms_pose = J[ms_start_row[i]:ms_end_row[i], ms_start_col[i]:ms_end_col[i]] assert (J_ms_pose.shape[1] == 6) J_ms_pose[:, :] = self.multisensor_pose_jacobian( opt_param_vec, full_pose_arr[i, :], ms) #import code; code.interact(local=locals()) print "-J", sys.stdout.flush() return J