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 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 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_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 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 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_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 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 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_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)