testMotionExecutionIk.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 planning_environment_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 motion_planning_msgs.msg import CollisionOperation, MotionPlanRequest, PositionConstraint, OrientationConstraint
00018 from move_arm_msgs.msg import MoveArmGoal, MoveArmAction
00019 from geometric_shapes_msgs.msg import Shape
00020 from geometry_msgs.msg import Pose, PointStamped
00021 from tf import TransformListener
00022 from motion_planning_msgs.msg import JointConstraint
00023 
00024 padd_name = "ompl_planning/robot_padd"
00025 extra_buffer = .1
00026 
00027 class TestMotionExecutionBuffer(unittest.TestCase):
00028 
00029     def setUp(self):
00030 
00031         rospy.init_node('test_motion_execution_buffer')
00032         
00033         self.tf = TransformListener()        
00034 
00035         self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
00036         
00037         self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
00038         self.move_arm_action_client.wait_for_server()
00039 
00040         obj1 = CollisionObject()
00041     
00042         obj1.header.stamp = rospy.Time.now()
00043         obj1.header.frame_id = "base_link"
00044         obj1.id = "obj1";
00045         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00046         obj1.shapes = [Shape() for _ in range(1)]
00047         obj1.shapes[0].type = Shape.BOX
00048         obj1.shapes[0].dimensions = [float() for _ in range(3)]
00049         obj1.shapes[0].dimensions[0] = .1
00050         obj1.shapes[0].dimensions[1] = .1
00051         obj1.shapes[0].dimensions[2] = 2.0
00052         obj1.poses = [Pose() for _ in range(1)]
00053         obj1.poses[0].position.x = .9
00054         obj1.poses[0].position.y = -.5
00055         obj1.poses[0].position.z = 1.0
00056         obj1.poses[0].orientation.x = 0
00057         obj1.poses[0].orientation.y = 0
00058         obj1.poses[0].orientation.z = 0
00059         obj1.poses[0].orientation.w = 1
00060         
00061         #self.obj_pub.publish(obj1)
00062 
00063         #rospy.sleep(1.0)
00064         
00065     def tearDown(self):
00066         obj1 = CollisionObject()
00067         obj1.header.stamp = rospy.Time.now()
00068         obj1.header.frame_id = "base_link"
00069         obj1.id = "all";
00070         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
00071 
00072         self.obj_pub.publish(obj1)
00073         
00074         #rospy.sleep(2.0)
00075 
00076     def testMotionExecutionBuffer(self):
00077         
00078         global padd_name
00079         global extra_buffer
00080         
00081         #too much trouble to read for now
00082         allow_padd = .05#rospy.get_param(padd_name)
00083         
00084         motion_plan_request = MotionPlanRequest()
00085 
00086         motion_plan_request.group_name = "right_arm"
00087         motion_plan_request.num_planning_attempts = 1
00088         motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00089         motion_plan_request.planner_id = ""
00090         planner_service_name = "ompl_planning/plan_kinematic_path"
00091 
00092         motion_plan_request.goal_constraints.position_constraints = [PositionConstraint() for _ in range(1)]
00093         motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time.now()
00094         motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link"
00095 
00096         motion_plan_request.goal_constraints.position_constraints[0].link_name = "right_arm_hand_link"
00097         motion_plan_request.goal_constraints.position_constraints[0].position.x = .65
00098         motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.55
00099         motion_plan_request.goal_constraints.position_constraints[0].position.z = .9
00100     
00101         motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = Shape.BOX
00102         motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [float(.02) for _ in range(3)]
00103         
00104         motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0
00105         motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0
00106         
00107         motion_plan_request.goal_constraints.orientation_constraints = [OrientationConstraint() for _ in range(1)]
00108         motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time.now()
00109         motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link"    
00110         motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "right_arm_hand_link"
00111 
00112         vals = [float() for _ in range(4)]
00113         vals = [-.895249, -.187638, 0.092325, .393443]
00114         mag = math.sqrt(vals[0]*vals[0]+vals[1]*vals[1]+vals[2]*vals[2]+vals[3]*vals[3])
00115         vals[0] /= mag
00116         vals[1] /= mag
00117         vals[2] /= mag
00118         vals[3] /= mag
00119 
00120         motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = vals[0]
00121         motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = vals[1]
00122         motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = vals[2]
00123         motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = vals[3]
00124         
00125         motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04
00126         motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04
00127         motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04
00128 
00129         motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0
00130 
00131         goal = MoveArmGoal()
00132         goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00133         goal.motion_plan_request = motion_plan_request
00134 
00135         self.move_arm_action_client.send_goal(goal)
00136 
00137         while True:
00138              cur_state = self.move_arm_action_client.get_state()
00139              if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00140                 cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00141                  break 
00142 
00143 if __name__ == '__main__':
00144 
00145     import rostest
00146     rostest.unitrun('test_motion_execution_buffer', 'test_motion_execution_buffer', TestMotionExecutionBuffer)
00147 
00148 
00149     
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


rosie_arm_navigation_launch
Author(s): E. Gil Jones
autogenerated on Thu May 23 2013 14:11:54