def setUp(self):
#config_filename = "config/system.yaml"
#f = open(config_filename)
f = rospy.get_param("system")
all_config = yaml.load(f)
self.robot_params = RobotParams()
self.robot_params.configure(all_config)
rospy.wait_for_service('fk', 3.0)
self._fk_ref = rospy.ServiceProxy('fk', FkTest)
class LoadData(unittest.TestCase):
def setUp(self):
#config_filename = "config/system.yaml"
#f = open(config_filename)
f = rospy.get_param("system")
all_config = yaml.load(f)
self.robot_params = RobotParams()
self.robot_params.configure(all_config)
rospy.wait_for_service('fk', 3.0)
self._fk_ref = rospy.ServiceProxy('fk', FkTest)
#f.close()
def loadCommands(self, param_name):
#f = open(filename)
#cmds = [ [ float(y) for y in x.split()] for x in f.readlines()]
#f.close()
command_str = rospy.get_param(param_name)
cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]
return cmds
def getExpected(self, root, tip, cmd):
resp = self._fk_ref(root, tip, cmd)
#print resp
T = matrix(zeros((4,4)), float)
T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
T[3,3] = 1.0;
T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
return T
def test_single_sensor(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:
- [ 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, 1, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [1, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
''')
chainM = ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0]))
sensor = ChainSensor(config, chainM, "boardA" )
sensor.update_config(robot_params)
target_pts = matrix([[1, 2, 1, 2],
[0, 0, 1, 1],
[0, 0, 0, 0],
[1, 1, 1, 1]])
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.single_sensor_params_jacobian(opt_param, target_pts, sensor)
print J
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_single_sensor(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:
- [ 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, 1, 0 ]
chainB:
- [ 0, 0, 0, 0 ]
tilting_lasers: {}
rectified_cams: {}
transforms:
transformA: [1, 0, 0, 0, 0, 0]
transformB: [0, 0, 0, 0, 0, 0]
checkerboards:
boardA:
spacing_x: 1
spacing_y: 1
''')
chainM = ChainMeasurement(chain_id="chainA",
chain_state=JointState(position=[0]))
sensor = ChainSensor(config, chainM, "boardA")
sensor.update_config(robot_params)
target_pts = matrix([[1, 2, 1, 2], [0, 0, 1, 1], [0, 0, 0, 0],
[1, 1, 1, 1]])
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.single_sensor_params_jacobian(opt_param, target_pts, sensor)
print J
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
