00001
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 mapping_msgs.msg
00016 from mapping_msgs.msg import CollisionObject, AttachedCollisionObject
00017 from motion_planning_msgs.msg import CollisionOperation
00018 from geometric_shapes_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 motion_planning_msgs.msg import JointConstraint
00023 from planning_environment_msgs.srv import GetStateValidity,GetStateValidityRequest
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 = '/environment_server/get_state_validity'
00039 self.ik_info = '/pr2_right_arm_kinematics/get_ik_solver_info'
00040
00041 rospy.init_node('test_pr2_arm_kinematics_with_constraints')
00042
00043 self.att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject,latch=True)
00044 self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00045
00046 rospy.wait_for_service(self.coll_ik_name)
00047 self.ik_service = rospy.ServiceProxy(self.coll_ik_name, GetConstraintAwarePositionIK)
00048
00049 rospy.wait_for_service(self.ik_info)
00050 self.ik_info_service = rospy.ServiceProxy(self.ik_info, GetKinematicSolverInfo)
00051
00052 rospy.wait_for_service(self.env_server_name)
00053 self.state_validity = rospy.ServiceProxy(self.env_server_name, GetStateValidity)
00054
00055 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']
00056
00057 self.state_req = GetStateValidityRequest()
00058 self.state_req.robot_state.joint_state.name = self.joint_names
00059 self.state_req.robot_state.joint_state.position = [float(0.0) for _ in range(7)]
00060 self.state_req.check_collisions = True
00061
00062 ik_info_req = GetKinematicSolverInfoRequest()
00063 ik_info_res = self.ik_info_service.call(ik_info_req)
00064 self.min_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))]
00065 self.max_limits = [float() for _ in range(len(ik_info_res.kinematic_solver_info.limits))]
00066 for i in range(len(ik_info_res.kinematic_solver_info.limits)):
00067 self.min_limits[i] = ik_info_res.kinematic_solver_info.limits[i].min_position
00068 self.max_limits[i] = ik_info_res.kinematic_solver_info.limits[i].max_position
00069
00070 print len(self.min_limits), len(self.max_limits)
00071
00072 self.table = CollisionObject()
00073
00074 self.table.header.stamp = rospy.Time.now()
00075 self.table.header.frame_id = "base_link"
00076 self.table.id = "table";
00077 self.table.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00078 self.table.shapes = [Shape() for _ in range(1)]
00079 self.table.shapes[0].type = Shape.BOX
00080 self.table.shapes[0].dimensions = [float() for _ in range(3)]
00081 self.table.shapes[0].dimensions[0] = 1.0
00082 self.table.shapes[0].dimensions[1] = 1.0
00083 self.table.shapes[0].dimensions[2] = .05
00084 self.table.poses = [Pose() for _ in range(1)]
00085 self.table.poses[0].position.x = 1.0
00086 self.table.poses[0].position.y = 0
00087 self.table.poses[0].position.z = .5
00088 self.table.poses[0].orientation.x = 0
00089 self.table.poses[0].orientation.y = 0
00090 self.table.poses[0].orientation.z = 0
00091 self.table.poses[0].orientation.w = 1
00092
00093 self.att_box = AttachedCollisionObject()
00094 self.att_box.link_name = 'r_gripper_palm_link'
00095 self.att_box.object.header.stamp = rospy.Time.now()
00096 self.att_box.object.header.frame_id = "r_gripper_palm_link"
00097 self.att_box.object.id = "att_box.object";
00098 self.att_box.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00099 self.att_box.object.shapes = [Shape() for _ in range(1)]
00100 self.att_box.object.shapes[0].type = Shape.BOX
00101 self.att_box.object.shapes[0].dimensions = [float() for _ in range(3)]
00102 self.att_box.object.shapes[0].dimensions[0] = .04
00103 self.att_box.object.shapes[0].dimensions[1] = .04
00104 self.att_box.object.shapes[0].dimensions[2] = .2
00105 self.att_box.object.poses = [Pose() for _ in range(1)]
00106 self.att_box.object.poses[0].position.x = 0.12
00107 self.att_box.object.poses[0].position.y = 0.0
00108 self.att_box.object.poses[0].position.z = 0.0
00109 self.att_box.object.poses[0].orientation.x = 0
00110 self.att_box.object.poses[0].orientation.y = 0
00111 self.att_box.object.poses[0].orientation.z = 0
00112 self.att_box.object.poses[0].orientation.w = 1
00113 self.att_box.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
00114 'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link']
00115
00116 def tearDown(self):
00117 obj1 = CollisionObject()
00118 obj1.header.stamp = rospy.Time.now()
00119 obj1.header.frame_id = "base_link"
00120 obj1.id = "all";
00121 obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00122
00123
00124
00125 self.att_box.link_name = "all"
00126 self.att_box.object.id = "all"
00127
00128 self.att_box.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00129
00130
00131
00132 rospy.sleep(2.0)
00133
00134 def testIkWithCollisionObjects(self):
00135
00136
00137 self.att_pub.publish(self.att_box)
00138
00139 rospy.sleep(2.0)
00140
00141 kin_req = GetConstraintAwarePositionIKRequest()
00142 kin_req.ik_request.ik_link_name = 'r_wrist_roll_link'
00143 kin_req.timeout = rospy.Duration(2.0)
00144 kin_req.ik_request.pose_stamped.header.frame_id = 'base_link'
00145 kin_req.ik_request.pose_stamped.pose.position.x = .52
00146 kin_req.ik_request.pose_stamped.pose.position.y = -.2
00147 kin_req.ik_request.pose_stamped.pose.position.z = .8
00148 kin_req.ik_request.pose_stamped.pose.orientation.x = 0.0
00149 kin_req.ik_request.pose_stamped.pose.orientation.y = 0.7071
00150 kin_req.ik_request.pose_stamped.pose.orientation.z = 0.0
00151 kin_req.ik_request.pose_stamped.pose.orientation.w = 0.7071
00152
00153 kin_req.ik_request.ik_seed_state.joint_state.name = self.joint_names
00154 kin_req.ik_request.ik_seed_state.joint_state.position = [float(0.0) for _ in range(7)]
00155
00156 kin_req.ik_request.robot_state.joint_state.name = self.joint_names
00157 kin_req.ik_request.robot_state.joint_state.position = [float(0.0) for _ in range(7)]
00158
00159 for _ in range(25):
00160
00161 while(True):
00162 self.state_req.robot_state.joint_state.position = self.generateRandomValues()
00163 state_val_res = self.state_validity.call(self.state_req)
00164 if(state_val_res.error_code.val == state_val_res.error_code.SUCCESS):
00165 break
00166
00167 kin_req.ik_request.robot_state.joint_state.position = self.state_req.robot_state.joint_state.position
00168 kin_req.ik_request.ik_seed_state.joint_state.position = self.generateRandomValues()
00169
00170 kin_res = self.ik_service.call(kin_req)
00171
00172 self.failIf(kin_res.error_code.val != kin_res.error_code.SUCCESS)
00173 self.state_req.robot_state.joint_state.position = kin_res.solution.joint_state.position
00174 state_val_res = self.state_validity.call(self.state_req)
00175 self.failIf(state_val_res.error_code.val != state_val_res.error_code.SUCCESS)
00176
00177 if __name__ == '__main__':
00178
00179 import rostest
00180 rostest.unitrun('test_ik_with_collision_objects', 'test_ik_with_collision_objects', TestPr2ArmKinematicsWithCollisionObjects)