Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 PKG = 'sr_grasp'
00022 NAME = 'test_grasp_contact'
00023
00024 import os, unittest;
00025 import rospy, rostest
00026 from actionlib import SimpleActionClient, GoalStatus
00027 from sr_robot_msgs.msg import GraspAction, GraspGoal
00028 from moveit_msgs.msg import Grasp
00029 from trajectory_msgs.msg import JointTrajectoryPoint
00030 from sr_hand.shadowhand_ros import ShadowHand_ROS
00031
00032 class TestGraspContact(unittest.TestCase):
00033 longMessage = True
00034
00035 joint_names = [
00036 'LFJ0', 'LFJ3', 'LFJ4', 'LFJ5',
00037 'RFJ0', 'RFJ3', 'RFJ4',
00038 'MFJ0', 'MFJ3', 'MFJ4',
00039 'FFJ0', 'FFJ3', 'FFJ4',
00040 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5',
00041 'WRJ1', 'WRJ2']
00042
00043 def mk_grasp(self, joints):
00044 grasp = Grasp()
00045
00046 grasp.pre_grasp_posture.joint_names = self.joint_names
00047 jtp = JointTrajectoryPoint()
00048 for jname in self.joint_names:
00049 jtp.positions.append(0.0)
00050 grasp.pre_grasp_posture.points.append(jtp)
00051
00052 grasp.grasp_posture.joint_names = self.joint_names
00053 jtp = JointTrajectoryPoint()
00054 for jname in self.joint_names:
00055 if jname in joints:
00056 jtp.positions.append(joints[jname])
00057 else:
00058 jtp.positions.append(0.0)
00059 grasp.grasp_posture.points.append(jtp)
00060 return grasp
00061
00062
00063
00064
00065 def test_grasp_contact(self):
00066 """Test sending a grasp with object in the way"""
00067 rospy.sleep(2)
00068 hand = ShadowHand_ROS()
00069
00070
00071 for j in hand.allJoints:
00072 hand.sendupdate(j.name, 0.0)
00073 rospy.sleep(2)
00074
00075 goal = GraspGoal()
00076 goal.grasp = self.mk_grasp({
00077 'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4,
00078 'LFJ0': 2.0, 'RFJ0': 2.0, 'MFJ0': 2.0, 'FFJ0': 2.0,
00079 })
00080 goal.pre_grasp = False
00081
00082 client = SimpleActionClient('grasp', GraspAction)
00083 client.wait_for_server()
00084 client.send_goal(goal)
00085 client.wait_for_result(rospy.Duration.from_sec(20.0))
00086 self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
00087 "Action did not return in SUCCEEDED state.")
00088
00089 rospy.sleep(2)
00090
00091
00092 for j in hand.allJoints:
00093 hand.sendupdate(j.name, 0.0)
00094 rospy.sleep(2)
00095
00096
00097 if __name__ == '__main__':
00098 rospy.init_node(NAME)
00099 rostest.rosrun(PKG, NAME, TestGraspContact)
00100