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 test_length(self):
st = SingleTransform([0, 0, 0, 0, 0, 0])
self.assertEqual(st.get_length(), 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
