testMovingOutOfAttachedContact.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, 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 move_arm_msgs.msg import MoveArmGoal, MoveArmAction
00021 from tf import TransformListener
00022 from arm_navigation_msgs.msg import JointConstraint
00023 
00024 class TestMotionExecutionBuffer(unittest.TestCase):
00025 
00026     def setUp(self):
00027 
00028         self.tf = TransformListener()
00029         
00030         self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
00031 
00032 
00033 
00034         att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
00035         obj_pub = rospy.Publisher('collision_object',CollisionObject)
00036         
00037         rospy.init_node('test_motion_execution_buffer')
00038         
00039         #let everything settle down
00040         rospy.sleep(5.)
00041         
00042         obj1 = CollisionObject()
00043     
00044         obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
00045         obj1.header.frame_id = "base_footprint"
00046         obj1.id = "obj2";
00047         obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00048         obj1.shapes = [Shape() for _ in range(3)]
00049         obj1.poses = [Pose() for _ in range(3)]
00050 
00051         obj1.shapes[0].type = Shape.BOX
00052         obj1.shapes[0].dimensions = [float() for _ in range(3)]
00053         obj1.shapes[0].dimensions[0] = .5
00054         obj1.shapes[0].dimensions[1] = 1.0
00055         obj1.shapes[0].dimensions[2] = .2
00056         obj1.poses[0].position.x = .95
00057         obj1.poses[0].position.y = -.25
00058         obj1.poses[0].position.z = .62
00059         obj1.poses[0].orientation.x = 0
00060         obj1.poses[0].orientation.y = 0
00061         obj1.poses[0].orientation.z = 0
00062         obj1.poses[0].orientation.w = 1
00063 
00064         obj1.shapes[2].type = Shape.BOX
00065         obj1.shapes[2].dimensions = [float() for _ in range(3)]
00066         obj1.shapes[2].dimensions[0] = .5
00067         obj1.shapes[2].dimensions[1] = .1
00068         obj1.shapes[2].dimensions[2] = 1.0
00069         obj1.poses[2].position.x = .95
00070         obj1.poses[2].position.y = -.14
00071         obj1.poses[2].position.z = 1.2
00072         obj1.poses[2].orientation.x = 0
00073         obj1.poses[2].orientation.y = 0
00074         obj1.poses[2].orientation.z = 0
00075         obj1.poses[2].orientation.w = 1
00076 
00077         obj1.shapes[1].type = Shape.BOX
00078         obj1.shapes[1].dimensions = [float() for _ in range(3)]
00079         obj1.shapes[1].dimensions[0] = .5
00080         obj1.shapes[1].dimensions[1] = .1
00081         obj1.shapes[1].dimensions[2] = 1.0
00082         obj1.poses[1].position.x = .95
00083         obj1.poses[1].position.y = .12
00084         obj1.poses[1].position.z = 1.2
00085         obj1.poses[1].orientation.x = 0
00086         obj1.poses[1].orientation.y = 0
00087         obj1.poses[1].orientation.z = 0
00088         obj1.poses[1].orientation.w = 1
00089         
00090         obj_pub.publish(obj1)
00091 
00092         att_object = AttachedCollisionObject()
00093         att_object.object.header.stamp = rospy.Time.now()
00094         att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
00095         att_object.link_name = "r_gripper_r_finger_tip_link"
00096         att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
00097         att_object.object = CollisionObject();
00098 
00099         att_object.object.header.stamp = rospy.Time.now()
00100         att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
00101         att_object.object.id = "pole"
00102         att_object.object.shapes = [Shape() for _ in range(1)]
00103         att_object.object.shapes[0].type = Shape.CYLINDER
00104         att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
00105         att_object.object.shapes[0].dimensions[0] = .02
00106         att_object.object.shapes[0].dimensions[1] = .1
00107         att_object.object.poses = [Pose() for _ in range(1)]
00108         att_object.object.poses[0].position.x = -.02
00109         att_object.object.poses[0].position.y = .04
00110         att_object.object.poses[0].position.z = 0
00111         att_object.object.poses[0].orientation.x = 0
00112         att_object.object.poses[0].orientation.y = 0
00113         att_object.object.poses[0].orientation.z = 0
00114         att_object.object.poses[0].orientation.w = 1
00115 
00116         att_pub.publish(att_object)
00117 
00118         rospy.sleep(5.0)        
00119 
00120     def testMotionExecutionBuffer(self):
00121         
00122         global padd_name
00123         global extra_buffer
00124         
00125         #too much trouble to read for now
00126         allow_padd = .05#rospy.get_param(padd_name)
00127         
00128 
00129         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']]
00130         goal = MoveArmGoal()
00131 
00132         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00133 
00134         goal.motion_plan_request.group_name = "right_arm"
00135         goal.motion_plan_request.num_planning_attempts = 1
00136         goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
00137         goal.motion_plan_request.planner_id = ""
00138         goal.planner_service_name = "chomp_planner_longrange/plan_path"
00139         goal.disable_collision_monitoring = True;
00140 
00141         r = rospy.Rate(10)
00142 
00143         goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
00144         for z in range(6):
00145             for i in range(len(joint_names)):
00146                 goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
00147                 goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
00148                 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
00149                 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08
00150 
00151             if(z%2==0):
00152                 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0
00153                 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
00154                 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2
00155             else:
00156                 goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22
00157                 goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51
00158                 goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05
00159                 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41
00160                 goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06
00161                 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1
00162                 goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0
00163 
00164             for x in range(3):
00165 
00166                 self.move_arm_action_client.send_goal(goal)
00167             
00168                 while True:
00169                     cur_state = self.move_arm_action_client.get_state()
00170                     if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
00171                        cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
00172                         break
00173                     r.sleep()
00174                 
00175                 end_state = self.move_arm_action_client.get_state()
00176              
00177                 if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break
00178                 
00179             final_state = self.move_arm_action_client.get_state()
00180 
00181             self.assertEqual(final_state,  actionlib_msgs.msg.GoalStatus.SUCCEEDED)
00182 
00183 if __name__ == '__main__':
00184 
00185     import rostest
00186     rostest.unitrun('test_motion_execution_buffer', 'test_motion_execution_buffer', TestMotionExecutionBuffer)
00187 
00188 
00189     


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