35 import roslib; roslib.load_manifest(
'pr2_calibration_estimation')
44 from sensor_msgs.msg
import JointState
47 from pr2_calibration_estimation.srv
import FkTest
53 config = yaml.load(open(rospy.get_param(
'config_file')))
56 rospy.wait_for_service(
'fk', 3.0)
57 self.
_fk_ref = rospy.ServiceProxy(
'fk', FkTest)
60 command_str = rospy.get_param(param_name)
61 cmds = [ [float(y)
for y
in x.split()]
for x
in command_str.strip().split(
'\n')]
65 resp = self.
_fk_ref(root, tip, cmd)
66 T = matrix(zeros((4,4)), float)
67 T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
69 T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
74 def run_test(self, full_chain, root, tip, cmds):
76 print(
"On Command: %s" % cmd)
79 chain_state = JointState(position=cmd)
80 actual_T = full_chain.calc_block.fk(chain_state)
87 self.assertAlmostEqual(numpy.linalg.norm(expected_T-actual_T), 0.0, 6)
95 self.
run_test(full_chain,
'torso_lift_link',
'l_wrist_roll_link', cmds)
103 self.
run_test(full_chain,
'torso_lift_link',
'l_forearm_roll_link', cmds)
111 self.
run_test(full_chain,
'torso_lift_link',
'l_forearm_cam_frame', cmds)
119 self.
run_test(full_chain,
'torso_lift_link',
'l_forearm_cam_optical_frame', cmds)
121 if __name__ ==
'__main__':
123 rospy.init_node(
"fk_test")
124 rostest.unitrun(
'pr2_calibration_estimation',
'test_left_arm_FK', TestPR2LeftArmFk)
def loadCommands(self, param_name)
def run_test(self, full_chain, root, tip, cmds)
def test_right_forearm_cam_optical_fk(self)
def test_left_arm_fk(self)
def test_left_forearm_roll_fk(self)
def getExpected(self, root, tip, cmd)
def test_left_forearm_cam_fk(self)