35 import roslib; roslib.load_manifest(
'pr2_calibration_estimation')
42 from pr2_calibration_estimation.full_chain
import FullChainRobotParams
43 from pr2_calibration_estimation.robot_params
import RobotParams
44 from sensor_msgs.msg
import JointState
47 from pr2_calibration_estimation.srv
import FkTest
58 f = rospy.get_param(
"system")
59 all_config = yaml.load(f)
62 self.robot_params.configure(all_config)
65 rospy.wait_for_service(
'fk', 3.0)
66 self.
_fk_ref = rospy.ServiceProxy(
'fk', FkTest)
74 command_str = rospy.get_param(param_name)
77 cmds = [ [float(y)
for y
in x.split()]
for x
in command_str.strip().split(
'\n')]
82 resp = self.
_fk_ref(root, tip, cmd)
84 T = matrix(zeros((4,4)), float)
85 T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
87 T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
92 def run_test(self, full_chain, root, tip, cmds):
94 print(
"On Command: %s" % cmd)
97 chain_state = JointState(position=cmd)
98 actual_T = full_chain.calc_block.fk(chain_state)
105 self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
111 before_chain: [head_pan_joint] 113 after_chain: [head_chain_tip_adj] 117 full_chain = FullChainRobotParams(yaml.load(config_str))
122 self.
run_test(full_chain,
'torso_lift_link',
'head_tilt_link', cmds)
127 before_chain: [head_pan_joint] 129 after_chain: [head_chain_tip_adj, head_plate_frame_joint] 131 full_chain = FullChainRobotParams(yaml.load(config_str))
134 self.
run_test(full_chain,
'torso_lift_link',
'head_plate_frame', cmds)
139 before_chain: [head_pan_joint] 141 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint] 143 full_chain = FullChainRobotParams(yaml.load(config_str))
146 self.
run_test(full_chain,
'torso_lift_link',
'double_stereo_link', cmds)
151 before_chain: [head_pan_joint] 153 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint] 155 full_chain = FullChainRobotParams(yaml.load(config_str))
158 self.
run_test(full_chain,
'torso_lift_link',
'wide_stereo_link', cmds)
163 before_chain: [head_pan_joint] 165 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, wide_stereo_frame_joint, wide_stereo_optical_frame_joint] 167 full_chain = FullChainRobotParams(yaml.load(config_str))
170 self.
run_test(full_chain,
'torso_lift_link',
'wide_stereo_optical_frame', cmds)
175 before_chain: [head_pan_joint] 177 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint] 179 full_chain = FullChainRobotParams(yaml.load(config_str))
182 self.
run_test(full_chain,
'torso_lift_link',
'narrow_stereo_link', cmds)
187 before_chain: [head_pan_joint] 189 after_chain: [head_chain_tip_adj, head_plate_frame_joint, double_stereo_frame_joint, narrow_stereo_frame_joint, narrow_stereo_optical_frame_joint] 191 full_chain = FullChainRobotParams(yaml.load(config_str))
194 self.
run_test(full_chain,
'torso_lift_link',
'narrow_stereo_optical_frame', cmds)
199 before_chain: [head_pan_joint] 201 after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint] 203 full_chain = FullChainRobotParams(yaml.load(config_str))
206 self.
run_test(full_chain,
'torso_lift_link',
'high_def_frame', cmds)
211 before_chain: [head_pan_joint] 213 after_chain: [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint] 215 full_chain = FullChainRobotParams(yaml.load(config_str))
218 self.
run_test(full_chain,
'torso_lift_link',
'high_def_optical_frame', cmds)
267 actual_T = self.robot_params.tilting_lasers[
"tilt_laser"].compute_pose([cmd])
268 expected_T = self.
getExpected(
"torso_lift_link",
"laser_tilt_link", [cmd])
275 self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
282 if __name__ ==
'__main__':
284 rospy.init_node(
"fk_test")
285 rostest.unitrun(
'pr2_calibration_estimation',
'test_PR2Fk', TestPR2Fk)
def test_narrow_stereo(self)
def test_narrow_stereo_optical(self)
def test_wide_stereo_optical(self)
def loadCommands(self, param_name)
def check_tilt_laser(self, cmd)
def test_head_tilt_link(self)
def test_high_def_optical(self)
def getExpected(self, root, tip, cmd)
def test_wide_stereo(self)
def test_tilt_laser(self)
def test_double_stereo(self)
def run_test(self, full_chain, root, tip, cmds)
def test_head_plate(self)