def load(self): config = yaml.load(''' chain_id: chainA before_chain: [transformA] dh_link_num: 1 after_chain: [transformB] ''') robot_params = RobotParams() robot_params.configure( yaml.load(''' dh_chains: chainA: dh: - [ 0, 0, 1, 0 ] gearing: [1] cov: joint_angles: [1] tilting_lasers: {} rectified_cams: {} transforms: transformA: [0, 0, 0, 0, 0, 0] transformB: [0, 0, 0, 0, 0, 0] checkerboards: boardA: corners_x: 2 corners_y: 2 spacing_x: 1 spacing_y: 1 ''')) return config, robot_params
def opt_runner(robot_params_dict, free_dict, blocks): # Load the robot params robot_params = RobotParams() robot_params.configure(robot_params_dict) for block in blocks: block.update_config(robot_params) # Load the free configuration free_list = robot_params.calc_free(free_dict) expanded_param_vec = robot_params.deflate() error_calc = ErrorCalc(robot_params, expanded_param_vec, free_list, blocks) # Construct the initial guess opt_guess_mat = expanded_param_vec[numpy.where(free_list), 0].copy() opt_guess = numpy.array(opt_guess_mat)[0] import scipy.optimize x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq(error_calc.calculate_error, opt_guess.copy(), full_output=1) # A hacky way to inflate x back into robot params full_param_vec = error_calc.calculate_full_param_vec(x) output_dict = error_calc._robot_params.params_to_config(full_param_vec) # Compute the rms error final_error = error_calc.calculate_error(x) rms_error = numpy.sqrt( numpy.mean(final_error**2) ) print "RMS Error: %f" % rms_error return output_dict
def load(self): config = yaml.load(''' chain_id: chainA before_chain: [transformA] dh_link_num: 1 after_chain: [transformB] ''') robot_params = RobotParams() robot_params.configure( yaml.load(''' dh_chains: chainA: dh: - [ 0, 0, 1, 0 ] gearing: [1] cov: joint_angles: [1] tilting_lasers: {} rectified_cams: {} transforms: transformA: [0, 0, 0, 0, 0, 0] transformB: [0, 0, 0, 0, 0, 0] checkerboards: boardA: corners_x: 2 corners_y: 2 spacing_x: 1 spacing_y: 1 ''' ) ) return config, robot_params
def test_params_to_config(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) config = robot_params.params_to_config(loadParamVec()) self.assertAlmostEqual(config["dh_chains"]["chain2"]['dh'][1][1], 10, 6) self.assertAlmostEqual(config["rectified_cams"]["camA"]["baseline_shift"], 4, 6) # self.assertAlmostEqual(config["tilting_lasers"]["laserA"]["before_joint"][1], 20, 6) self.assertAlmostEqual(config["checkerboards"]["boardA"]["spacing_y"], 40)
def test_free(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) free_list = robot_params.calc_free(loadFreeDict()) print free_list self.assertEqual(free_list[0], True) self.assertEqual(free_list[1], False) self.assertEqual(free_list[18], True) self.assertEqual(free_list[19], False) self.assertEqual(free_list[27], True)
def test_params_to_config(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) config = robot_params.params_to_config(loadParamVec()) self.assertAlmostEqual(config["dh_chains"]["chain2"]['dh'][1][1], 10, 6) self.assertAlmostEqual( config["rectified_cams"]["camA"]["baseline_shift"], 4, 6) # self.assertAlmostEqual(config["tilting_lasers"]["laserA"]["before_joint"][1], 20, 6) self.assertAlmostEqual(config["checkerboards"]["boardA"]["spacing_y"], 40)
def test_inflate(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) robot_params.inflate(loadParamVec()) self.assertEqual(robot_params.dh_chains["chain1"]._config[0,0], -10) self.assertEqual(robot_params.dh_chains["chain2"]._config[1,1], 10) # self.assertEqual(robot_params.tilting_lasers["laserA"]._before_joint.transform[1,3], 20) # self.assertEqual(robot_params.tilting_lasers["laserA"]._after_joint.transform[2,3], 30) self.assertEqual(robot_params.transforms["transformA"].transform[0,3], 40) self.assertEqual(robot_params.rectified_cams["camA"]._config['baseline_shift'], 4) self.assertEqual(robot_params.checkerboards["boardA"]._spacing_x, 30)
def opt_runner(robot_params_dict, pose_guess_arr, free_dict, multisensors, use_cov): """ Runs a single optimization step for the calibration optimization. robot_params_dict - Dictionary storing all of the system primitives' parameters (lasers, cameras, chains, transforms, etc) free_dict - Dictionary storing which parameters are free multisensor - list of list of measurements. Each multisensor corresponds to a single checkerboard pose pose_guesses - List of guesses as to where all the checkerboard are. This is used to initialze the optimization """ # Load the robot params robot_params = RobotParams() robot_params.configure(robot_params_dict) error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov) # Construct the initial guess opt_all = build_opt_vector(robot_params, free_dict, pose_guess_arr) x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq( error_calc.calculate_error, opt_all, Dfun=error_calc.calculate_jacobian, full_output=1) #x = opt_all #error_calc.calculate_error(x) J = error_calc.calculate_jacobian(x) # A hacky way to inflate x back into robot params opt_param_vec, pose_vec = error_calc.split_all(x) expanded_param_vec = error_calc.calculate_full_param_vec(opt_param_vec) opt_pose_arr = reshape(pose_vec, [-1, 6]) output_dict = error_calc._robot_params.params_to_config(expanded_param_vec) errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr) print "" print "Errors Breakdown:" for sensor_id, error_list in errors_dict.items(): error_cat = numpy.concatenate(error_list) rms_error = numpy.sqrt(numpy.mean(error_cat**2)) print " %s: %.6f" % (sensor_id, rms_error) # Compute the rms error #final_error = error_calc.calculate_error(x) #rms_error = numpy.sqrt( numpy.mean(final_error**2) ) #print "RMS Error: %f" % rms_error return output_dict, opt_pose_arr, J
def __init__(self, name): path ="/u/robot/git/care-o-bot/src/cob_calibration/cob_calibration_config/cob3-8/" with open(path+"autogenerated/system.yaml", "r") as f: system =yaml.safe_load(f) with open(path+"user_defined/sensors.yaml", "r") as f: sensors =yaml.safe_load(f) r = RobotParams() r.configure(system) self.full_chain = FullChainRobotParams(sensors["camera_chains"][0]["chain"], sensors) self.full_chain.update_config(r) self.calc_block = self.full_chain.calc_block self.state=None self.name = name
def opt_runner(robot_params_dict, pose_guess_arr, free_dict, multisensors, use_cov): """ Runs a single optimization step for the calibration optimization. robot_params_dict - Dictionary storing all of the system primitives' parameters (lasers, cameras, chains, transforms, etc) free_dict - Dictionary storing which parameters are free multisensor - list of list of measurements. Each multisensor corresponds to a single checkerboard pose pose_guesses - List of guesses as to where all the checkerboard are. This is used to initialze the optimization """ # Load the robot params robot_params = RobotParams() robot_params.configure(robot_params_dict) error_calc = ErrorCalc(robot_params, free_dict, multisensors, use_cov) # Construct the initial guess opt_all = build_opt_vector(robot_params, free_dict, pose_guess_arr) x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq(error_calc.calculate_error, opt_all, Dfun=error_calc.calculate_jacobian, full_output=1) #x = opt_all #error_calc.calculate_error(x) J = error_calc.calculate_jacobian(x) # A hacky way to inflate x back into robot params opt_param_vec, pose_vec = error_calc.split_all(x) expanded_param_vec = error_calc.calculate_full_param_vec(opt_param_vec) opt_pose_arr = reshape(pose_vec, [-1, 6]) output_dict = error_calc._robot_params.params_to_config(expanded_param_vec) errors_dict = compute_errors_breakdown(error_calc, multisensors, opt_pose_arr) print "" print "Errors Breakdown:" for sensor_id, error_list in errors_dict.items(): error_cat = numpy.concatenate(error_list) rms_error = numpy.sqrt( numpy.mean(error_cat**2) ) print " %s: %.6f" % (sensor_id, rms_error) # Compute the rms error #final_error = error_calc.calculate_error(x) #rms_error = numpy.sqrt( numpy.mean(final_error**2) ) #print "RMS Error: %f" % rms_error return output_dict, opt_pose_arr, J
def load(self): config = yaml.load(''' camera_id: camA chain: before_chain: [transformA] chain_id: chainA dh_link_num: 1 after_chain: [transformB] ''') robot_params = RobotParams() robot_params.configure( yaml.load(''' dh_chains: chainA: dh: - [ 0, 0, 1, 0 ] gearing: [1] cov: joint_angles: [1.0] chainB: dh: - [ 0, 0, 2, 0 ] gearing: [1] cov: joint_angles: [1.0] tilting_lasers: {} rectified_cams: camA: baseline_shift: 0.0 f_shift: 0.0 cx_shift: 0.0 cy_shift: 0.0 cov: {u: 1.0, v: 1.0} transforms: transformA: [0, 0, 0, 0, 0, 0] transformB: [0, 0, 0, 0, 0, 0] transformC: [0, 0, 0, 0, 0, 0] transformD: [0, 0, 0, 0, 0, 0] checkerboards: {} ''' ) ) P = [ 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0 ] return config, robot_params, P
def load(self): config = yaml.load(''' camera_id: camA chain: before_chain: [transformA] chain_id: chainA dh_link_num: 1 after_chain: [transformB] ''') robot_params = RobotParams() robot_params.configure( yaml.load(''' dh_chains: chainA: dh: - [ 0, 0, 1, 0 ] gearing: [1] cov: joint_angles: [1.0] chainB: dh: - [ 0, 0, 2, 0 ] gearing: [1] cov: joint_angles: [1.0] tilting_lasers: {} rectified_cams: camA: baseline_shift: 0.0 f_shift: 0.0 cx_shift: 0.0 cy_shift: 0.0 cov: {u: 1.0, v: 1.0} transforms: transformA: [0, 0, 0, 0, 0, 0] transformB: [0, 0, 0, 0, 0, 0] transformC: [0, 0, 0, 0, 0, 0] transformD: [0, 0, 0, 0, 0, 0] checkerboards: {} ''')) P = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0] return config, robot_params, P
def test_deflate(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) p = loadParamVec() robot_params.inflate(p) result = robot_params.deflate() self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
def opt_runner(robot_params_dict, free_dict, blocks): # Load the robot params robot_params = RobotParams() robot_params.configure(robot_params_dict) for block in blocks: block.update_config(robot_params) # Load the free configuration free_list = robot_params.calc_free(free_dict) expanded_param_vec = robot_params.deflate() error_calc = ErrorCalc(robot_params, expanded_param_vec, free_list, blocks) # Construct the initial guess opt_guess_mat = expanded_param_vec[numpy.where(free_list), 0].copy() opt_guess = numpy.array(opt_guess_mat)[0] import scipy.optimize x, cov_x, infodict, mesg, iter = scipy.optimize.leastsq( error_calc.calculate_error, opt_guess.copy(), full_output=1) # A hacky way to inflate x back into robot params full_param_vec = error_calc.calculate_full_param_vec(x) output_dict = error_calc._robot_params.params_to_config(full_param_vec) # Compute the rms error final_error = error_calc.calculate_error(x) rms_error = numpy.sqrt(numpy.mean(final_error**2)) print "RMS Error: %f" % rms_error return output_dict
def test_configure(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) # print "laser start: %u" % robot_params.tilting_lasers["laserA"].start # print "laser end: %u" % robot_params.tilting_lasers["laserA"].end print "transform start: %u" % robot_params.transforms[ "transformA"].start print "transform end: %u" % robot_params.transforms["transformA"].end print "cam start: %u" % robot_params.rectified_cams["camA"].start print "cam end: %u" % robot_params.rectified_cams["camA"].end # ****** DH Chains ****** self.assertEqual(robot_params.dh_chains["chain1"].start, 15) self.assertEqual(robot_params.dh_chains["chain1"].end, 25) self.assertEqual(robot_params.dh_chains["chain2"].start, 0) self.assertEqual(robot_params.dh_chains["chain2"].end, 15) # ****** Tilting Lasers ****** # self.assertEqual(robot_params.tilting_lasers["laserA"].start, 25) # self.assertEqual(robot_params.tilting_lasers["laserA"].end, 38) # ****** Transforms ****** self.assertEqual(robot_params.transforms["transformA"].start, 25) self.assertEqual(robot_params.transforms["transformA"].end, 31) # ****** Rectified Cams ****** self.assertEqual(robot_params.rectified_cams["camA"].start, 31) self.assertEqual(robot_params.rectified_cams["camA"].end, 35) # ****** Checkerboards ****** self.assertEqual(robot_params.checkerboards["boardA"].start, 35) self.assertEqual(robot_params.checkerboards["boardA"].end, 37)
def test_configure(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) # print "laser start: %u" % robot_params.tilting_lasers["laserA"].start # print "laser end: %u" % robot_params.tilting_lasers["laserA"].end print "transform start: %u" % robot_params.transforms["transformA"].start print "transform end: %u" % robot_params.transforms["transformA"].end print "cam start: %u" % robot_params.rectified_cams["camA"].start print "cam end: %u" % robot_params.rectified_cams["camA"].end # ****** DH Chains ****** self.assertEqual(robot_params.dh_chains["chain1"].start, 15) self.assertEqual(robot_params.dh_chains["chain1"].end, 25) self.assertEqual(robot_params.dh_chains["chain2"].start, 0) self.assertEqual(robot_params.dh_chains["chain2"].end, 15) # ****** Tilting Lasers ****** # self.assertEqual(robot_params.tilting_lasers["laserA"].start, 25) # self.assertEqual(robot_params.tilting_lasers["laserA"].end, 38) # ****** Transforms ****** self.assertEqual(robot_params.transforms["transformA"].start, 25) self.assertEqual(robot_params.transforms["transformA"].end, 31) # ****** Rectified Cams ****** self.assertEqual(robot_params.rectified_cams["camA"].start, 31) self.assertEqual(robot_params.rectified_cams["camA"].end, 35) # ****** Checkerboards ****** self.assertEqual(robot_params.checkerboards["boardA"].start, 35) self.assertEqual(robot_params.checkerboards["boardA"].end, 37)
def test_inflate(self): robot_params = RobotParams() robot_params.configure(loadConfigDict()) robot_params.inflate(loadParamVec()) self.assertEqual(robot_params.dh_chains["chain1"]._config[0, 0], -10) self.assertEqual(robot_params.dh_chains["chain2"]._config[1, 1], 10) # self.assertEqual(robot_params.tilting_lasers["laserA"]._before_joint.transform[1,3], 20) # self.assertEqual(robot_params.tilting_lasers["laserA"]._after_joint.transform[2,3], 30) self.assertEqual(robot_params.transforms["transformA"].transform[0, 3], 40) self.assertEqual( robot_params.rectified_cams["camA"]._config['baseline_shift'], 4) self.assertEqual(robot_params.checkerboards["boardA"]._spacing_x, 30)