hironx_ros_bridge: test_hironx_ros_bridge

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 from test_hironx_ros_bridge import *
00005 
00006 class TestHiroROSBridgePose(TestHiroROSBridge):
00007 
00008     def test_LArmIK(self):
00009         #   /WAIST /LARM_JOINT5_Link
00010         # - Translation: [0.325, 0.182, 0.074]
00011         # - Rotation: in Quaternion [-0.000, -0.707, 0.000, 0.707]
00012         #             in RPY [-1.694, -1.571, 1.693]
00013         for torso_angle in ([0, -10, 10]):
00014             torso_goal = self.goal_Torso()
00015             torso_goal = self.setup_Positions(torso_goal, [[torso_angle]])
00016             self.torso.send_goal_and_wait(torso_goal)
00017             for p in [[ 0.325, 0.182, 0.074], # initial
00018                       [ 0.3, -0.2, 0.074], [ 0.3, -0.1, 0.074], 
00019                       #[ 0.3,  0.0, 0.074], [ 0.3,  0.1, 0.074], 
00020                       [ 0.3,  0.2, 0.074], [ 0.3,  0.3, 0.074],
00021                       [ 0.4, -0.1, 0.074], [ 0.4, -0.0, 0.074],
00022                       #[ 0.4,  0.1, 0.074], [ 0.4,  0.2, 0.074], 
00023                       [ 0.4,  0.3, 0.074], [ 0.4,  0.4, 0.074],
00024                       ] :
00025 
00026                 print "solve ik at p = ", p
00027                 ret = self.set_target_pose('LARM', p, [-1.694,-1.571, 1.693], 1.0)
00028                 self.assertTrue(ret.operation_return, "ik failed")
00029                 ret = self.wait_interpolation_of_group('LARM')
00030                 self.assertTrue(ret.operation_return, "wait interpolation failed")
00031 
00032                 rospy.sleep(1)
00033                 now = rospy.Time.now()
00034                 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00035                 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00036                 numpy.testing.assert_array_almost_equal(trans, p, decimal=1)
00037                 print "current position   p = ", trans
00038                 print "                 rot = ", rot
00039                 print "                     = ", quaternion_matrix(rot)[0:3,0:3]
00040                 # this fails?
00041                 #numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00042                 #                                        numpy.array([[ 0,  0, -1],
00043                 #                                                     [ 0,  1,  0],
00044                 #                                                     [ 1,  0,  0]]),
00045                 #                                        decimal=3)
00046 
00047 
00048     def test_LArm(self):
00049         goal = self.goal_LArm()
00050         goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00051                                            [  25,-139,-157,  45,   0,   0],
00052                                            [ 0.618, -0.157,-100.0,-15.212, 9.501, -3.188]])
00053         self.larm.send_goal_and_wait(goal)
00054 
00055         rospy.sleep(1)
00056         now = rospy.Time.now()
00057         self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00058         (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00059         numpy.testing.assert_array_almost_equal(trans, [0.325493, 0.18236, 0.0744674], decimal=3)
00060         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00061                                                 numpy.array([[ 0, 0,-1],
00062                                                              [ 0, 1, 0],
00063                                                              [ 1, 0, 0]]), decimal=2)
00064 
00065     def test_RArm(self):
00066         goal = self.goal_RArm()
00067         goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00068                                            [  25,-139,-157,  45,   0,   0],
00069                                            [-0.618, -0.157,-100.0,15.212, 9.501, 3.188]])
00070         self.rarm.send_goal_and_wait(goal)
00071 
00072         rospy.sleep(1)
00073         now = rospy.Time.now()
00074         self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00075         (trans, rot) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00076         numpy.testing.assert_array_almost_equal(trans, [0.325493,-0.18236, 0.0744674], decimal=3)
00077         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00078                                                 numpy.array([[ 0, 0,-1],
00079                                                              [ 0, 1, 0],
00080                                                              [ 1, 0, 0]]), decimal=2)
00081 
00082     def test_Torso(self):
00083         goal = self.goal_Torso()
00084         goal = self.setup_Positions(goal, [[   0.0],
00085                                            [-162.949],
00086                                            [ 162.949]])
00087         self.torso.send_goal_and_wait(goal)
00088 
00089         rospy.sleep(1)
00090         now = rospy.Time.now()
00091         self.listener.waitForTransform("WAIST", "CHEST_JOINT0_Link", now, rospy.Duration(1.0))
00092         (trans, rot) = self.listener.lookupTransform("WAIST", "CHEST_JOINT0_Link", now)
00093         numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.0], decimal=3)
00094         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00095                                                 numpy.array([[-0.956044, -0.293223, 0.0],
00096                                                              [ 0.293223, -0.956044, 0.0],
00097                                                              [ 0.0,       0.0,      1.0]]), decimal=3)
00098 
00099     def test_Head(self):
00100         goal = self.goal_Head()
00101         goal = self.setup_Positions(goal, [[  0.0,  0.0],
00102                                            [ 70.0, 70.0],
00103                                            [-70.0,-20.0]])
00104         self.head.send_goal_and_wait(goal)
00105 
00106         rospy.sleep(1)
00107         now = rospy.Time.now()
00108         self.listener.waitForTransform("WAIST", "HEAD_JOINT1_Link", now, rospy.Duration(1.0))
00109         (trans, rot) = self.listener.lookupTransform("WAIST", "HEAD_JOINT1_Link", now)
00110         numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.5695], decimal=3)
00111         numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00112                                                 numpy.array([[0.321394, 0.939693, -0.116978],
00113                                                              [-0.883022, 0.34202, 0.321394],
00114                                                              [0.34202, 0.0, 0.939693]]), decimal=3)
00115 
00116 if __name__ == '__main__':
00117     import rostest
00118     rostest.rosrun(PKG, 'test_hronx_ros_bridge_pose', TestHiroROSBridgePose)
00119 
00120 
00121