$search
00001 #! /usr/bin/env python 00002 00003 PKG = 'pr2_arm_ik_tests' 00004 00005 import roslib; roslib.load_manifest(PKG) 00006 import rospy 00007 import sys 00008 import unittest 00009 import actionlib 00010 import actionlib_msgs 00011 import math 00012 import random 00013 00014 import sensor_msgs.msg 00015 import arm_navigation_msgs.msg 00016 from arm_navigation_msgs.msg import CollisionObject, AttachedCollisionObject 00017 from arm_navigation_msgs.msg import CollisionOperation 00018 from arm_navigation_msgs.msg import Shape 00019 from geometry_msgs.msg import Pose, PointStamped 00020 from kinematics_msgs.srv import GetConstraintAwarePositionIK, GetConstraintAwarePositionIKRequest, GetKinematicSolverInfo, GetKinematicSolverInfoRequest 00021 from tf import TransformListener 00022 from arm_navigation_msgs.msg import JointConstraint 00023 from arm_navigation_msgs.srv import GetStateValidity,GetStateValidityRequest, SetPlanningSceneDiff, SetPlanningSceneDiffRequest 00024 00025 class TestPr2ArmKinematicsWithCollisionObjects(unittest.TestCase): 00026 00027 def generateRandomValues(self): 00028 ret = [float() for _ in range(len(self.min_limits))] 00029 for i in range(len(self.min_limits)): 00030 ret[i] = random.uniform(self.min_limits[i], self.max_limits[i]) 00031 return ret 00032 00033 def setUp(self): 00034 00035 random.seed() 00036 00037 self.coll_ik_name = '/pr2_right_arm_kinematics/get_constraint_aware_ik' 00038 self.env_server_name = '/planning_scene_validity_server/get_state_validity' 00039 self.ik_info = '/pr2_right_arm_kinematics/get_ik_solver_info' 00040 self.set_planning_scene_diff_name = '/environment_server/set_planning_scene_diff' 00041 00042 rospy.init_node('test_pr2_arm_kinematics_with_constraints') 00043 00044 self.att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject,latch=True) 00045 self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True) 00046 00047 rospy.wait_for_service(self.coll_ik_name) 00048 self.ik_service = rospy.ServiceProxy(self.coll_ik_name, GetConstraintAwarePositionIK) 00049 00050 rospy.wait_for_service(self.ik_info) 00051 self.ik_info_service = rospy.ServiceProxy(self.ik_info, GetKinematicSolverInfo) 00052 00053 rospy.wait_for_service(self.set_planning_scene_diff_name) 00054 self.set_planning_scene_diff = rospy.ServiceProxy(self.set_planning_scene_diff_name, SetPlanningSceneDiff) 00055 00056 rospy.wait_for_service(self.env_server_name) 00057 self.state_validity = rospy.ServiceProxy(self.env_server_name, GetStateValidity) 00058 00059 self.joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] 00060 00061 self.state_req = GetStateValidityRequest() 00062 self.state_req.group_name = 'right_arm' 00063 self.state_req.robot_state.joint_state.name = self.joint_names 00064 self.state_req.robot_state.joint_state.position = [float(0.0) for _ in range(7)] 00065 self.state_req.check_collisions = True 00066 00067 ik_info_req = GetKinematicSolverInfoRequest() 00068 ik_info_res = self.ik_info_service.call(ik_info_req) 00069 self.min_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))] 00070 self.max_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))] 00071 for i in range(len(ik_info_res.kinematic_solver_info.limits)): 00072 self.min_limits[i] = ik_info_res.kinematic_solver_info.limits[i].min_position 00073 self.max_limits[i] = ik_info_res.kinematic_solver_info.limits[i].max_position 00074 00075 print len(self.min_limits), len(self.max_limits) 00076 00077 self.table = CollisionObject() 00078 00079 self.table.header.stamp = rospy.Time.now() 00080 self.table.header.frame_id = "base_link" 00081 self.table.id = "table"; 00082 self.table.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD 00083 self.table.shapes = [Shape() for _ in range(1)] 00084 self.table.shapes[0].type = Shape.BOX 00085 self.table.shapes[0].dimensions = [float() for _ in range(3)] 00086 self.table.shapes[0].dimensions[0] = 1.0 00087 self.table.shapes[0].dimensions[1] = 1.0 00088 self.table.shapes[0].dimensions[2] = .05 00089 self.table.poses = [Pose() for _ in range(1)] 00090 self.table.poses[0].position.x = 1.0 00091 self.table.poses[0].position.y = 0 00092 self.table.poses[0].position.z = .5 00093 self.table.poses[0].orientation.x = 0 00094 self.table.poses[0].orientation.y = 0 00095 self.table.poses[0].orientation.z = 0 00096 self.table.poses[0].orientation.w = 1 00097 00098 self.att_box = AttachedCollisionObject() 00099 self.att_box.link_name = 'r_gripper_palm_link' 00100 self.att_box.object.header.stamp = rospy.Time.now() 00101 self.att_box.object.header.frame_id = "r_gripper_palm_link" 00102 self.att_box.object.id = "att_box.object"; 00103 self.att_box.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD 00104 self.att_box.object.shapes = [Shape() for _ in range(1)] 00105 self.att_box.object.shapes[0].type = Shape.BOX 00106 self.att_box.object.shapes[0].dimensions = [float() for _ in range(3)] 00107 self.att_box.object.shapes[0].dimensions[0] = .04 00108 self.att_box.object.shapes[0].dimensions[1] = .04 00109 self.att_box.object.shapes[0].dimensions[2] = .2 00110 self.att_box.object.poses = [Pose() for _ in range(1)] 00111 self.att_box.object.poses[0].position.x = 0.12 00112 self.att_box.object.poses[0].position.y = 0.0 00113 self.att_box.object.poses[0].position.z = 0.0 00114 self.att_box.object.poses[0].orientation.x = 0 00115 self.att_box.object.poses[0].orientation.y = 0 00116 self.att_box.object.poses[0].orientation.z = 0 00117 self.att_box.object.poses[0].orientation.w = 1 00118 self.att_box.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 00119 'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link'] 00120 00121 def testIkWithCollisionObjects(self): 00122 00123 # add table 00124 set_planning_scene_diff_request = SetPlanningSceneDiffRequest() 00125 set_planning_scene_diff_request.planning_scene_diff.attached_collision_objects.append(self.att_box) 00126 00127 self.set_planning_scene_diff.call(set_planning_scene_diff_request) 00128 00129 kin_req = GetConstraintAwarePositionIKRequest() 00130 kin_req.ik_request.ik_link_name = 'r_wrist_roll_link' 00131 kin_req.timeout = rospy.Duration(2.0) 00132 kin_req.ik_request.pose_stamped.header.frame_id = 'base_link' 00133 kin_req.ik_request.pose_stamped.pose.position.x = .52 00134 kin_req.ik_request.pose_stamped.pose.position.y = -.2 00135 kin_req.ik_request.pose_stamped.pose.position.z = .8 00136 kin_req.ik_request.pose_stamped.pose.orientation.x = 0.0 00137 kin_req.ik_request.pose_stamped.pose.orientation.y = 0.7071 00138 kin_req.ik_request.pose_stamped.pose.orientation.z = 0.0 00139 kin_req.ik_request.pose_stamped.pose.orientation.w = 0.7071 00140 00141 kin_req.ik_request.ik_seed_state.joint_state.name = self.joint_names 00142 kin_req.ik_request.ik_seed_state.joint_state.position = [float(0.0) for _ in range(7)] 00143 00144 kin_req.ik_request.robot_state.joint_state.name = self.joint_names 00145 kin_req.ik_request.robot_state.joint_state.position = [float(0.0) for _ in range(7)] 00146 00147 for _ in range(25): 00148 00149 while(True): 00150 self.state_req.robot_state.joint_state.position = self.generateRandomValues() 00151 state_val_res = self.state_validity.call(self.state_req) 00152 if(state_val_res.error_code.val == state_val_res.error_code.SUCCESS): 00153 break 00154 00155 kin_req.ik_request.robot_state.joint_state.position = self.state_req.robot_state.joint_state.position 00156 kin_req.ik_request.ik_seed_state.joint_state.position = self.generateRandomValues() 00157 00158 kin_res = self.ik_service.call(kin_req) 00159 00160 self.failIf(kin_res.error_code.val != kin_res.error_code.SUCCESS) 00161 self.state_req.robot_state.joint_state.position = kin_res.solution.joint_state.position 00162 state_val_res = self.state_validity.call(self.state_req) 00163 self.failIf(state_val_res.error_code.val != state_val_res.error_code.SUCCESS) 00164 00165 if __name__ == '__main__': 00166 00167 import rostest 00168 rostest.unitrun('test_ik_with_collision_objects', 'test_ik_with_collision_objects', TestPr2ArmKinematicsWithCollisionObjects)