def test_left_arm_fk(self):
print ""
full_chain = FullChainRobotParams('left_arm_chain',
'l_wrist_roll_link')
full_chain.update_config(self.robot_params)
cmds = self.loadCommands('l_arm_commands')
self.run_test(full_chain, 'torso_lift_link', 'l_wrist_roll_link', cmds)
def test_kinect_head_optical(self):
full_chain = FullChainRobotParams('head_chain',
'head_camera_rgb_optical_frame')
full_chain.update_config(self.robot_params)
cmds = self.loadCommands('head_commands')
self.run_test(full_chain, 'torso_link',
'head_camera_rgb_optical_frame', cmds)
def test_right_forearm_cam_optical_fk(self):
print ""
full_chain = FullChainRobotParams('left_arm_chain',
'l_forearm_cam_optical_frame')
full_chain.update_config(self.robot_params)
cmds = self.loadCommands('l_arm_commands')
self.run_test(full_chain, 'torso_lift_link',
'l_forearm_cam_optical_frame', cmds)
def test_fk_partial(self):
print ""
params = loadSystem1()
chain = FullChainRobotParams('chain1', 'j1_link')
chain.update_config(params)
chain_state = JointState(position=[0, 0])
result = chain.calc_block.fk(chain_state)
expected = numpy.matrix(
[[1, 0, 0, 11], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float)
print result
self.assertAlmostEqual(numpy.linalg.norm(result - expected), 0.0, 6)
def __init__(self, config_dict, M_chain, target_id):
self.sensor_type = "chain"
self.sensor_id = config_dict["sensor_id"]
self._config_dict = config_dict
self._M_chain = M_chain
self._target_id = target_id
self._full_chain = FullChainRobotParams(self.sensor_id,
self.sensor_id + "_cb_link")
self.terms_per_sample = 3
def __init__(self, config_dict, M_cam, M_chain):
"""
Generates a single sensor block for a single configuration
Inputs:
- config_dict: The configuration for this specific sensor. This looks exactly like
a single element from the valid_configs list passed into CameraChainBundler.__init__
- M_cam: The camera measurement of type calibration_msgs/CameraMeasurement
- M_chain: The chain measurement of type calibration_msgs/ChainMeasurement
"""
self.sensor_type = "camera"
self.sensor_id = config_dict["sensor_id"]
self._config_dict = config_dict
self._M_cam = M_cam
self._M_chain = M_chain
self._full_chain = FullChainRobotParams(config_dict["chain_id"],
config_dict["frame_id"])
self.terms_per_sample = 2
if "baseline_rgbd" in config_dict.keys():
self.terms_per_sample = 3
def test_head_tilt_link(self):
full_chain = FullChainRobotParams('head_chain', 'head_tilt_link')
full_chain.update_config(self.robot_params)
cmds = self.loadCommands('head_commands')
self.run_test(full_chain, 'torso_link', 'head_tilt_link', cmds)
