Python MoveGroupCommander.set_pose_target方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
class GenericDualArmClient():
    _safe_kinematicsolver = "RRTConnectkConfigDefault"

    def __init__(self, *args, **kwargs):
        larm_name = args[0]
        rarm_name = args[1]  # "arm_right" for Motoman SDA10F
        self.r_arm = MoveGroupCommander(rarm_name)
        self.r_arm.set_planner_id(self._safe_kinematicsolver)

    def move_rarm_shift_forward(self, axis, value):
        '''
        Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
        '''
        self.r_arm.shift_pose_target(axis, value)
        self.r_arm.plan()
        self.r_arm.go()

    def move_rarm_fixedpose_forward(self):
        tpose = Pose()
        #TODO Currently the following position may work with SDA10F but not with NXO
        tpose.position.x = 0.599
        tpose.position.y = -0.379
        tpose.position.z = 1.11
        self.r_arm.set_pose_target(tpose)
        self.r_arm.plan()
        self.r_arm.go()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
	def run(self, cycle, verbose = 2):

		#enable robot if not already done
		enableProcess = subprocess.Popen(["rosrun", "baxter_tools", 
		"enable_robot.py", "-e"])
		enableProcess.wait()
		
		#start moveit servers. Since we do not know how long it will take them 
		#to start, pause for 15 seconds
		jointServer = subprocess.Popen(["rosrun", "baxter_interface", 
		"joint_trajectory_action_server.py"])
		planServer = subprocess.Popen(["roslaunch", "baxter_moveit_config", 
		"move_group.launch"])
		time.sleep(15)
		
		raw_input("press enter")
		
		try:
			#left = MoveGroupCommander("left_arm")
			right = MoveGroupCommander("right_arm")
			
			upRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			x = 0.9667369015076861, y = -0.1190874231664102, z = -0.07489146157788866 ))
			
			#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.35, z = 0.8))
			
			#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = -0.45, z = 0.3))
			
			#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.45, z = 0.3))
			
			right.set_pose_target(upRight)
			#left.set_pose_target(upLeft)
			right.go()
			#left.go()
			
			#right.set_pose_target(downRight)
			#left.set_pose_target(downLeft)
			
			right.go()
			#left.go()
			
		finally:
			#clean up - kill servers
			jointServer.kill()
			planServer.kill()
			
			sys.exit()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
def main():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    
    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')
        
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_hand"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"
        
        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = 0.5
        request.ik_request.pose_stamped.pose.position.y = 0.5
        request.ik_request.pose_stamped.pose.position.z = 0.3        
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0
        
        try:
            #Send the request to the service
            response = compute_ik(request)
            
            #Print the response HERE
            print(response)
            group = MoveGroupCommander("left_arm")
            
            #this command tries to achieve a pose: which is position, orientation
            group.set_pose_target(request.ik_request.pose_stamped)

            #then, this command tries to achieve a position only.  orientation is airbitrary.
            group.set_position_target([0.5,0.5,0.3])
            group.go()
            
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
    def __init__(self):
        group = MoveGroupCommander("arm")
        
        #group.set_orientation_tolerance([0.3,0.3,0,3])
        
        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492
        
        
        q = quaternion_from_euler(2, 0, 0)
        
        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]
        


        
        print "Planning frame: " ,group.get_planning_frame()
        print "Pose reference frame: ",group.get_pose_reference_frame()
        
        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
        
        group.set_pose_target(p, "katana_gripper_tool_frame")
        
        group.go()
        print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
		marker2.pose.position.x = eef_pose.pose.position.x
		marker2.pose.position.y = eef_pose.pose.position.y
		marker2.pose.position.z = eef_pose.pose.position.z

		# We add the new marker to the MarkerArray, removing the oldest
		# marker from it when necessary
		if(count > MARKERS_MAX):
			markerArray.markers.pop(0)

		markerArray.markers.append(marker)
		markerArray.markers.append(marker2)

		print 'how far away: ', (goal_pose.pose.position.x-eef_pose.pose.position.x), ' ',(goal_pose.pose.position.y-eef_pose.pose.position.y), ' ',(goal_pose.pose.position.z-eef_pose.pose.position.z)

		# Renumber the marker IDs
		id = 0
		for m in markerArray.markers:
			m.id = id
			id += 1

#	while not rospy.is_shutdown():
		# Publish the MarkerArray
		publisher.publish(markerArray)

		robot.set_pose_target(eef_pose)
		robot.go()

	rospy.sleep(1)

	rospy.spin()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]

#.........这里部分代码省略.........
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame accordingly
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.65

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.3
        box1_pose.pose.position.y = 0
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.3
        box2_pose.pose.position.y = 0.25
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.22
        target_pose.pose.position.y = 0.14
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05

        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the target pose for the arm
        arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logwarn(e)

        if pose_st_target:
            pub.publish(pose_st_target)
            rospy.loginfo(trans)

        rate.sleep()

    if raw_input() == 'q':
        sys.exit(1)

    # move to a way point
    pose_st_target.pose.position.z += 0.1
    rospy.loginfo("set target to {}".format(pose_st_target.pose))
    group.set_pose_target(pose_st_target.pose)
    plan = group.plan()
    rospy.loginfo("plan is {}".format(plan))
    ret = group.go()
    rospy.loginfo("executed ... {}".format(ret))

    pose_st_target.pose.position.z -= 0.1
    rospy.loginfo("set target to {}".format(pose_st_target.pose))
    group.set_pose_target(pose_st_target.pose)
    plan = group.plan()
    rospy.loginfo("plan is {}".format(plan))
    ret = group.go()
    rospy.loginfo("executed ... {}".format(ret))
    gripper_close()

    pose_st_target.pose.position.y -= 0.2

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
    start_pose = geometry_msgs.msg.Pose()
    start_pose.position.x = -0.0988490064784
    start_pose.position.y = 0.272349904278
    start_pose.position.z = 1.18864499931
    start_pose.orientation.x = 0.393751611087
    start_pose.orientation.y = 0.918424640162
    start_pose.orientation.z = -0.0150455838492
    start_pose.orientation.w = 0.0350639347048
#     start_pose.orientation.w = 0
#     start_pose.orientation.x = 0
#     start_pose.orientation.y = 1
#     start_pose.orientation.z = 0
#     start_pose.position.y = 0.0256415233819
#     start_pose.position.z = 1.25871460368
#     start_pose.position.x = 0.243500142238
    right_arm.set_pose_target(start_pose)
    plan_start = right_arm.plan()
    print "============ Waiting while RVIZ displays plan_start..."
    rospy.sleep(5)
    right_arm.execute(plan_start)
    print "============ Waiting while RVIZ executes plan_start..."
    rospy.sleep(5)
     
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
 
    gain = 0.2
    points = 5
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander("right_arm")
        right_gripper = MoveGroupCommander("right_gripper")
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object("right_gripper_link", "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        right_arm.set_named_target("start1")
        right_arm.go()
      
        right_gripper.set_named_target("open")
        right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.15

        p.pose.position.y = -0.12
        p.pose.position.z = 0.7
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.0130178
        start_pose.pose.position.y = -0.125155
        start_pose.pose.position.z = 0.597653
        start_pose.pose.orientation.x = 0.0
        start_pose.pose.orientation.y = 0.388109
        start_pose.pose.orientation.z = 0.0
        start_pose.pose.orientation.w = 0.921613
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        #grasps = self.make_grasps(start_pose)
   
        #result = False
        #n_attempts = 0
       
        # repeat until will succeed
        #while result == False:
            #result = robot.right_arm.pick("part", grasps)      
            #n_attempts += 1
            #print "Attempts: ", n_attempts
            #rospy.sleep(0.2)
          
        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
    # right_arm.set_position_target([.75,-0.3, 1])
    # right_arm.go()
    # rospy.sleep(1)

    grasps = []

    g = Grasp()
    g.id = "test"
    grasp_pose = PoseStamped()
    grasp_pose.header.frame_id = "base_link"
    grasp_pose.pose.position.x = 0.47636
    grasp_pose.pose.position.y = -0.21886
    grasp_pose.pose.position.z = 0.7164
    grasp_pose.pose.orientation.x = 0.00080331
    grasp_pose.pose.orientation.y = 0.001589
    grasp_pose.pose.orientation.z = -2.4165e-06
    grasp_pose.pose.orientation.w = 1

    rospy.logwarn("moving to arm")
    right_arm.set_pose_target(grasp_pose)
    right_arm.go()
    rospy.sleep(3)

    rospy.sleep(2)
    rospy.logwarn("pick part")
    # pick the object
    robot.right_arm.pick("part")

    rospy.spin()
    roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
class Planner(object):
    move_group = None
    goals = None
    jspub = None
    namespace = None

    # These will eventually go to model objects
    robot_data = {
        'group_name': 'right_arm_and_torso',
        'eef_link': 'r_wrist_joint_link'
    }

    # Current state of the 'session' (right now, only one)
    current_scene = None
    status = None
    link_poses = None

    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}

    def get_scene(self):
        return self.current_scene

    def set_scene(self, scene):
        self.current_scene = scene
        psw = PlanningSceneWorld()
        for co_json in scene['objects']:
            # TODO: Fix orientation by using proper quaternions on the client
            pose = self._make_pose(co_json['pose'])
            # TODO: Decide what to do with STL vs. Collada. The client has a Collada
            # loader but the PlanningSceneInterface can only deal with STL.
            # TODO: Proper mapping between filenames and URLs
            # filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
            filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
            co = self.ps.make_mesh(co_json['name'], pose, filename)
            psw.collision_objects.append(co)
        self.psw_pub.publish(psw)


    def get_link_poses(self):
        if self.link_poses is None:
            self.link_poses = self._move_group.get_link_poses_compressed()
        return self.link_poses

    # Create link back to socket.io namespace to allow emitting information
    def set_socket(self, namespace):
        self.namespace = namespace

    def emit(self, event, data=None):
        if self.namespace:
            self.namespace.emit(event, data)

    def emit_new_goal(self, pose):
        self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])

    def set_random_goal(self):
        goal_pose = self.move_group.get_random_pose()
        # goal_pose = self.move_group.get_random_pose('base_footprint')
        self.emit_new_goal(goal_pose)

    def _make_pose(self, json_pose):
        pose = PoseStamped()
        pose.header.frame_id = "odom_combined"
        pp = json_pose['position']
        pose.pose.position.x = pp['x']
        pose.pose.position.y = pp['y']
        pose.pose.position.z = pp['z']
        # TODO: Orientation is not working. See about
        # properly using Quaternions everywhere
        pp = json_pose['orientation']
        pose.pose.orientation.x = pp['x']
        pose.pose.orientation.y = pp['y']
        pose.pose.orientation.z = pp['z']
        pose.pose.orientation.w = pp['w']
        return pose

    def plan_to_poses(self, poses):
        goal_pose = self._make_pose(poses[0])
        self.move_group.set_pose_target(goal_pose)
        # self.move_group.set_pose_target(goal_pose,'base_footprint')
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('right_arm')
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_footprint')
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                                        
        # Start in the "straight_forward" configuration stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = right_arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
         
        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            right_arm.set_pose_target(start_pose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            right_arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]

#.........这里部分代码省略.........
        # A list of yaw angles to try
        yaw_vals = [0]

        # A list to hold the places
        places = []

        # Generate a place pose for each angle and translation
        for y in yaw_vals:
            for p in pitch_vals:
                for y in y_vals:
                    for x in x_vals:
                        place.pose.position.x = init_pose.pose.position.x + x
                        place.pose.position.y = init_pose.pose.position.y + y

                        # Create a quaternion from the Euler angles
                        q = quaternion_from_euler(0, p, y)

                        # Set the place pose orientation accordingly
                        place.pose.orientation.x = q[0]
                        place.pose.orientation.y = q[1]
                        place.pose.orientation.z = q[2]
                        place.pose.orientation.w = q[3]

                        # Append this place pose to the list
                        places.append(deepcopy(place))

        # Return the list
        return places


    def plan_exec(self, pose):

        self.right_arm.clear_pose_targets()
        self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
        self.right_arm.plan()
        rospy.sleep(5)
        self.right_arm.go(wait=True)


    def close_gripper(self):

        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
        self.ac.send_goal(g_close)
        self.ac.wait_for_result()
        rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object


    def open_gripper(self):

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        self.ac.send_goal(g_open)
        self.ac.wait_for_result()
        rospy.sleep(5) # And up to 20 to detach it


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id
        global target_size
        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]

#.........这里部分代码省略.........
        # A list to hold the grasps
        grasps = []
        g = PoseStamped()
        g.header.frame_id = REFERENCE_FRAME
        g.pose = initial_pose.pose
        #g.pose.position.z += 0.18

        # Generate a grasp for each pitch and yaw angle
        for y in yaw_vals:
            for p in pitch_vals:
                # Create a quaternion from the Euler angles
                q = transformations.quaternion_from_euler(0, p, y)

                # Set the grasp pose orientation accordingly
                g.pose.orientation.x = q[0]
                g.pose.orientation.y = q[1]
                g.pose.orientation.z = q[2]
                g.pose.orientation.w = q[3]


                # Append the grasp to the list
                grasps.append(deepcopy(g))
     
        # Return the list
        return grasps





    def plan_exec(self, pose):

        self.right_arm.clear_pose_targets()
        self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
        self.right_arm.plan()
        rospy.sleep(5)
        self.right_arm.go(wait=True)


    def close_gripper(self):

        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
        self.ac.send_goal(g_close)
        self.ac.wait_for_result()
        rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object


    def open_gripper(self):

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        self.ac.send_goal(g_open)
        self.ac.wait_for_result()
        rospy.sleep(5) # And up to 20 to detach it


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id

        next_call = time.time()
        while True:
            next_call = next_call+1
            target_id = 'target'
            self.taid = self.pwh.name.index('wood_cube_5cm')
            table_id = 'table'

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_pose_target [as 别名]

#.........这里部分代码省略.........
        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        rospy.sleep(2)
        
        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.51
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Send the colors to the planning scene
        self.sendColors()    
        
        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.4
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)
        
        # Move the arm to the target pose (if possible)
        right_arm.go()
        
        # Pause for a moment...
        rospy.sleep(2)
        

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script        
        moveit_commander.os._exit(0)