def test_full_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: []
''')
configB = yaml.load('''
chain_id: chainB
before_chain: []
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure( yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
tilting_lasers:
laserB:
before_joint: [0, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
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
boardB:
corners_x: 1
corners_y: 1
spacing_x: 1
spacing_y: 1
''' ) )
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers:
laserB:
before_joint: [1, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [1, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
boardB:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardB")
laserB = TiltingLaserSensor({'laser_id':'laserB'}, LaserMeasurement( laser_id="laserB",
joint_points=[ JointState(position=[0,0,2]) ] ) )
multisensorA = MultiSensor(None)
multisensorA.sensors = [sensorA]
multisensorA.checkerboard = "boardA"
multisensorA.update_config(robot_params)
multisensorB = MultiSensor(None)
multisensorB.sensors = [sensorB, laserB]
multisensorB.checkerboard = "boardB"
multisensorB.update_config(robot_params)
poseA = numpy.array([1, 0, 0, 0, 0, 0])
poseB = numpy.array([2, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
opt_all_vec = concatenate([opt_param, poseA, poseB])
print "Calculating Sparse"
J = calc.calculate_jacobian(opt_all_vec)
numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")
#import code; code.interact(local=locals())
print "Calculating Dense"
from scipy.optimize.slsqp import approx_jacobian
J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")
self.assertAlmostEqual(numpy.linalg.norm(J-J_naive), 0.0, 6)
def test_full_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: []
''')
configB = yaml.load('''
chain_id: chainB
before_chain: []
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure(
yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
tilting_lasers:
laserB:
before_joint: [0, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
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
boardB:
corners_x: 1
corners_y: 1
spacing_x: 1
spacing_y: 1
'''))
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers:
laserB:
before_joint: [1, 0, 0, 0, 0, 0]
after_joint: [0, 0, 0, 0, 0, 0]
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [1, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
boardB:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(
configA,
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(
configB,
ChainMeasurement(chain_id="chainB",
chain_state=JointState(position=[0])), "boardB")
laserB = TiltingLaserSensor(
{'laser_id': 'laserB'},
LaserMeasurement(laser_id="laserB",
joint_points=[JointState(position=[0, 0, 2])]))
multisensorA = MultiSensor(None)
multisensorA.sensors = [sensorA]
multisensorA.checkerboard = "boardA"
multisensorA.update_config(robot_params)
multisensorB = MultiSensor(None)
multisensorB.sensors = [sensorB, laserB]
multisensorB.checkerboard = "boardB"
multisensorB.update_config(robot_params)
poseA = numpy.array([1, 0, 0, 0, 0, 0])
poseB = numpy.array([2, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
opt_all_vec = concatenate([opt_param, poseA, poseB])
print "Calculating Sparse"
J = calc.calculate_jacobian(opt_all_vec)
numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")
#import code; code.interact(local=locals())
print "Calculating Dense"
from scipy.optimize.slsqp import approx_jacobian
J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")
self.assertAlmostEqual(numpy.linalg.norm(J - J_naive), 0.0, 6)
def test_multisensor_pose_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
configB = yaml.load('''
chain_id: chainB
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure( yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
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
''' ) )
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardA")
multisensor = MultiSensor(None)
multisensor.sensors = [sensorA, sensorB]
multisensor.checkerboard = "boardA"
pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec, multisensor)
print J
def test_multisensor_pose_jacobian(self):
configA = yaml.load('''
chain_id: chainA
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
configB = yaml.load('''
chain_id: chainB
before_chain: [transformA]
dh_link_num: 1
after_chain: [transformB]
''')
robot_params = RobotParams()
robot_params.configure(
yaml.load('''
dh_chains:
chainA:
- [ 0, 0, 1, 0 ]
chainB:
- [ 0, 0, 2, 0 ]
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
'''))
free_dict = yaml.load('''
dh_chains:
chainA:
- [ 1, 0, 0, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [0, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 0
spacing_y: 0
''')
sensorA = ChainSensor(
configA,
ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0])), "boardA")
sensorB = ChainSensor(
configB,
ChainMeasurement(chain_id="chainB",
chain_state=JointState(position=[0])), "boardA")
multisensor = MultiSensor(None)
multisensor.sensors = [sensorA, sensorB]
multisensor.checkerboard = "boardA"
pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
calc = ErrorCalc(robot_params, free_dict, [])
expanded_param_vec = robot_params.deflate()
free_list = robot_params.calc_free(free_dict)
opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
opt_param = numpy.array(opt_param_mat)[0]
J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec,
multisensor)
print J