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


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:11