00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import numpy
00038 import rospy
00039 from tf.transformations import quaternion_matrix, euler_from_matrix
00040 import time
00041
00042 from hironx_ros_bridge.testutil.test_rosbridge import TestHiroROSBridge
00043
00044 PKG = 'hironx_ros_bridge'
00045
00046
00047 class TestHiroROSBridgePose(TestHiroROSBridge):
00048
00049 def test_LArmIK(self):
00050
00051
00052
00053
00054 for torso_angle in ([0, -10, 10]):
00055 torso_goal = self.goal_Torso()
00056 torso_goal = self.setup_Positions(torso_goal, [[torso_angle]])
00057 self.torso.send_goal_and_wait(torso_goal)
00058 for p in [[ 0.325, 0.182, 0.074],
00059 [ 0.3, -0.2, 0.074], [ 0.3, -0.1, 0.074],
00060
00061 [ 0.3, 0.2, 0.074], [ 0.3, 0.3, 0.074],
00062 [ 0.4, -0.1, 0.074], [ 0.4, -0.0, 0.074],
00063
00064 [ 0.4, 0.3, 0.074], [ 0.4, 0.4, 0.074],
00065 ] :
00066
00067 print "solve ik at p = ", p
00068 ret = self.set_target_pose('LARM', p, [-1.694,-1.571, 1.693], 1.0)
00069 self.assertTrue(ret.operation_return, "ik failed")
00070 ret = self.wait_interpolation_of_group('LARM')
00071 self.assertTrue(ret.operation_return, "wait interpolation failed")
00072
00073 rospy.sleep(1)
00074 now = rospy.Time.now()
00075 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00076 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00077 numpy.testing.assert_array_almost_equal(trans, p, decimal=1)
00078 print "current position p = ", trans
00079 print " rot = ", rot
00080 print " = ", quaternion_matrix(rot)[0:3,0:3]
00081
00082
00083
00084
00085
00086
00087
00088
00089 def test_LArm(self):
00090 goal = self.goal_LArm()
00091 goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00092 [ 25,-139,-157, 45, 0, 0],
00093 [ 0.618, -0.157,-100.0,-15.212, 9.501, -3.188]])
00094 self.larm.send_goal_and_wait(goal)
00095
00096 rospy.sleep(1)
00097 now = rospy.Time.now()
00098 self.listener.waitForTransform("WAIST", "LARM_JOINT5_Link", now, rospy.Duration(1.0))
00099 (trans, rot) = self.listener.lookupTransform("WAIST", "LARM_JOINT5_Link", now)
00100 numpy.testing.assert_array_almost_equal(trans, [0.325493, 0.18236, 0.0744674], decimal=3)
00101 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00102 numpy.array([[ 0, 0,-1],
00103 [ 0, 1, 0],
00104 [ 1, 0, 0]]), decimal=2)
00105
00106 def test_RArm(self):
00107 goal = self.goal_RArm()
00108 goal = self.setup_Positions(goal, [[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
00109 [ 25,-139,-157, 45, 0, 0],
00110 [-0.618, -0.157,-100.0,15.212, 9.501, 3.188]])
00111 self.rarm.send_goal_and_wait(goal)
00112
00113 rospy.sleep(1)
00114 now = rospy.Time.now()
00115 self.listener.waitForTransform("WAIST", "RARM_JOINT5_Link", now, rospy.Duration(1.0))
00116 (trans, rot) = self.listener.lookupTransform("WAIST", "RARM_JOINT5_Link", now)
00117 numpy.testing.assert_array_almost_equal(trans, [0.325493,-0.18236, 0.0744674], decimal=3)
00118 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00119 numpy.array([[ 0, 0,-1],
00120 [ 0, 1, 0],
00121 [ 1, 0, 0]]), decimal=2)
00122
00123 def test_Torso(self):
00124 goal = self.goal_Torso()
00125 goal = self.setup_Positions(goal, [[ 0.0],
00126 [-162.949],
00127 [ 162.949]])
00128 self.torso.send_goal_and_wait(goal)
00129
00130 rospy.sleep(1)
00131 now = rospy.Time.now()
00132 self.listener.waitForTransform("WAIST", "CHEST_JOINT0_Link", now, rospy.Duration(1.0))
00133 (trans, rot) = self.listener.lookupTransform("WAIST", "CHEST_JOINT0_Link", now)
00134 numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.0], decimal=3)
00135 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00136 numpy.array([[-0.956044, -0.293223, 0.0],
00137 [ 0.293223, -0.956044, 0.0],
00138 [ 0.0, 0.0, 1.0]]), decimal=3)
00139
00140 def test_Head(self):
00141 goal = self.goal_Head()
00142 goal = self.setup_Positions(goal, [[ 0.0, 0.0],
00143 [ 70.0, 70.0],
00144 [-70.0,-20.0]])
00145 self.head.send_goal_and_wait(goal)
00146
00147 rospy.sleep(1)
00148 now = rospy.Time.now()
00149 self.listener.waitForTransform("WAIST", "HEAD_JOINT1_Link", now, rospy.Duration(1.0))
00150 (trans, rot) = self.listener.lookupTransform("WAIST", "HEAD_JOINT1_Link", now)
00151 numpy.testing.assert_array_almost_equal(trans, [0.0, 0.0, 0.5695], decimal=3)
00152 numpy.testing.assert_array_almost_equal(quaternion_matrix(rot)[0:3,0:3],
00153 numpy.array([[0.321394, 0.939693, -0.116978],
00154 [-0.883022, 0.34202, 0.321394],
00155 [0.34202, 0.0, 0.939693]]), decimal=3)
00156
00157 if __name__ == '__main__':
00158 import rostest
00159 rostest.rosrun(PKG, 'test_hronx_ros_bridge_pose', TestHiroROSBridgePose)
00160
00161
00162