testMotionExecutionBuffer.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 PKG = 'move_arm'
00004 
00005 import roslib; roslib.load_manifest(PKG)
00006 import rospy
00007 import arm_navigation_msgs.srv
00008 import sys
00009 import unittest
00010 import actionlib
00011 import actionlib_msgs
00012 import math
00013 
00014 import sensor_msgs.msg
00015 import mapping_msgs.msg
00016 from mapping_msgs.msg import CollisionObject
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 move_arm_msgs.msg import MoveArmGoal, MoveArmAction
00021 from tf import TransformListener
00022 from arm_navigation_msgs.msg import JointConstraint
00023 from arm_navigation_msgs import arm_navigation_msgs_utils
00024 
00025 padd_name = "ompl_planning/robot_padd"
00026 extra_buffer = .1
00027 
00028 class TestMotionExecutionBuffer(unittest.TestCase):
00029 
00030     def setUp(self):
00031 
00032         rospy.init_node('test_motion_execution_buffer')
00033         
00034         self.tf = TransformListener()        
00035 
00036         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00037         
00038         self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
00039 
00040         self.move_arm_action_client.wait_for_server()
00041 
00042         obj1 = CollisionObject()
00043     
00044         obj1.header.stamp = rospy.Time.now()
00045         obj1.header.frame_id = "base_link"
00046         obj1.id = "obj1";
00047         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00048         obj1.shapes = [Shape() for _ in range(1)]
00049         obj1.shapes[0].type = Shape.CYLINDER
00050         obj1.shapes[0].dimensions = [float() for _ in range(2)]
00051         obj1.shapes[0].dimensions[0] = .1
00052         obj1.shapes[0].dimensions[1] = 1.5
00053         obj1.poses = [Pose() for _ in range(1)]
00054         obj1.poses[0].position.x = .6
00055         obj1.poses[0].position.y = -.6
00056         obj1.poses[0].position.z = .75
00057         obj1.poses[0].orientation.x = 0
00058         obj1.poses[0].orientation.y = 0
00059         obj1.poses[0].orientation.z = 0
00060         obj1.poses[0].orientation.w = 1
00061         
00062         self.obj_pub.publish(obj1)
00063 
00064         rospy.sleep(2.0)
00065         
00066     def tearDown(self):
00067         obj1 = CollisionObject()
00068         obj1.header.stamp = rospy.Time.now()
00069         obj1.header.frame_id = "base_link"
00070         obj1.id = "all";
00071         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00072 
00073         self.obj_pub.publish(obj1)
00074         
00075         rospy.sleep(2.0)
00076 
00077     def testMotionExecutionBuffer(self):
00078         
00079         global padd_name
00080         global extra_buffer
00081         
00082         #too much trouble to read for now
00083         allow_padd = .05#rospy.get_param(padd_name)
00084         
00085 
00086         joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
00087         goal = MoveArmGoal()
00088 
00089         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00090 
00091         goal.motion_plan_request.group_name = "right_arm"
00092         goal.motion_plan_request.num_planning_attempts = 1
00093         goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00094         goal.motion_plan_request.planner_id = ""
00095         goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00096 
00097         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00098         for i in range(len(joint_names)):
00099             goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
00100             goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
00101             goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
00102             goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
00103 
00104         for z in range(2):
00105 
00106             min_dist = 1000
00107             
00108             if(z%2 == 0):
00109                 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
00110                 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00111                 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00112             else:
00113                 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
00114                 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00115                 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00116 
00117             for x in range(3):
00118 
00119                 self.move_arm_action_client.send_goal(goal)
00120 
00121                 r = rospy.Rate(10)
00122 
00123                 while True:
00124                     cur_state = self.move_arm_action_client.get_state()
00125                     if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00126                        cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00127                         break
00128 
00129                     #getting right finger tip link in base_link frame
00130                     t = self.tf.getLatestCommonTime("/base_link", "/r_gripper_r_finger_tip_link") 
00131                     finger_point = PointStamped()
00132                     finger_point.header.frame_id = "/r_gripper_r_finger_tip_link"
00133                     finger_point.header.stamp = t
00134                     finger_point.point.x = 0
00135                     finger_point.point.y = 0
00136                     finger_point.point.z = 0
00137                     finger_point_base = self.tf.transformPoint("base_link",finger_point)
00138 
00139                     distance = math.sqrt(math.pow(finger_point_base.point.x-.6,2)+math.pow(finger_point_base.point.y+.6,2))
00140 
00141                     # pole is .1 in diameter
00142                     distance -= .1
00143 
00144                     if distance < min_dist:
00145                         rospy.loginfo("X: %g Y: %g Dist: %g",finger_point_base.point.x,finger_point_base.point.y, distance)  
00146                         min_dist = distance
00147                         
00148                     r.sleep()
00149 
00150                 end_state = self.move_arm_action_client.get_state()
00151 
00152                 if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
00153 
00154             rospy.loginfo("Min dist %d is %g",z,min_dist)
00155 
00156             #should be a .5 buffer, allowing .1 buffer
00157             self.failIf(min_dist < (allow_padd-extra_buffer))
00158 
00159             final_state = self.move_arm_action_client.get_state()
00160 
00161             self.assertEqual(final_state,  actionlib_msgs.msg.GoalStatus.SUCCEEDED)
00162 
00163     def testAllowedNotAllowedInitialContact(self):
00164 
00165         #adding object in collision with base
00166 
00167         obj2 = CollisionObject()
00168 
00169         obj2.header.stamp = rospy.Time.now()
00170         obj2.header.frame_id = "base_link"
00171         obj2.id = "base_object"
00172         obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00173         obj2.shapes = [Shape() for _ in range(1)]
00174         obj2.shapes[0].type = Shape.BOX
00175         obj2.shapes[0].dimensions = [float() for _ in range(3)]
00176         obj2.shapes[0].dimensions[0] = .1
00177         obj2.shapes[0].dimensions[1] = .1
00178         obj2.shapes[0].dimensions[2] = .1
00179         obj2.poses = [Pose() for _ in range(1)]
00180         obj2.poses[0].position.x = 0
00181         obj2.poses[0].position.y = 0
00182         obj2.poses[0].position.z = 0
00183         obj2.poses[0].orientation.x = 0
00184         obj2.poses[0].orientation.y = 0
00185         obj2.poses[0].orientation.z = 0
00186         obj2.poses[0].orientation.w = 1
00187 
00188         self.obj_pub.publish(obj2)
00189 
00190         rospy.sleep(5.)
00191 
00192         joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
00193         goal = MoveArmGoal()
00194 
00195         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00196 
00197         goal.motion_plan_request.group_name = "right_arm"
00198         goal.motion_plan_request.num_planning_attempts = 1
00199         goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00200         goal.motion_plan_request.planner_id = ""
00201         goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00202 
00203         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00204         for i in range(len(joint_names)):
00205             goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
00206             goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
00207             goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
00208             goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
00209 
00210         goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
00211         goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00212         goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00213 
00214         self.move_arm_action_client.send_goal(goal)
00215 
00216         r = rospy.Rate(10)
00217 
00218         while True:
00219             cur_state = self.move_arm_action_client.get_state()
00220             if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00221                cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00222                 break
00223 
00224         #should still have succeedeed
00225         final_state = self.move_arm_action_client.get_state()
00226         self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)
00227 
00228         # but we can still overwrite
00229         coll = CollisionOperation()
00230         coll.object1 = "base_link"
00231         coll.object2 = coll.COLLISION_SET_OBJECTS
00232         coll.operation = coll.ENABLE
00233 
00234         goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)
00235 
00236         goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
00237         goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00238         goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00239 
00240         self.move_arm_action_client.send_goal(goal)
00241 
00242         r = rospy.Rate(10)
00243 
00244         while True:
00245             cur_state = self.move_arm_action_client.get_state()
00246             if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00247                cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00248                 break
00249 
00250         #should still have succeedeed
00251         final_state = self.move_arm_action_client.get_state()
00252         self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
00253 
00254 if __name__ == '__main__':
00255 
00256     import rostest
00257     rostest.unitrun('test_motion_execution_buffer', 'test_motion_execution_buffer', TestMotionExecutionBuffer)
00258 
00259 
00260     


move_arm
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Mon Dec 2 2013 12:35:17