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