def test_tilting_laser_1(self):
print ""
config, robot_params = loadSystem()
joint_points = [
JointState(position=[0, 0, 0]),
JointState(position=[0, pi / 2, 1]),
JointState(position=[pi / 2, 0, 1])
]
sensor = TiltingLaserSensor(
config["tilting_lasers"]["laserA"],
LaserMeasurement(laser_id="laserA", joint_points=joint_points))
sensor.update_config(robot_params)
target_pts = matrix([[0, 0, 0], [0, 1, 0], [0, 0, -1], [1, 1, 1]])
h = sensor.compute_expected(target_pts)
z = sensor.get_measurement()
r = sensor.compute_residual(target_pts)
self.assertAlmostEqual(numpy.linalg.norm(h - target_pts), 0.0, 6)
self.assertAlmostEqual(numpy.linalg.norm(z - target_pts), 0.0, 6)
self.assertAlmostEqual(numpy.linalg.norm(r), 0.0, 6)
# Test Sparsity
sparsity = sensor.build_sparsity_dict()
self.assertEqual(sparsity['transforms']['j0'], [1, 1, 1, 1, 1, 1])
self.assertEqual(sparsity['transforms']['j1'], [1, 1, 1, 1, 1, 1])
self.assertEqual(sparsity['transforms']['j2'], [1, 1, 1, 1, 1, 1])
self.assertEqual(sparsity['tilting_lasers']['laserA']['gearing'], 1)
