Python MoveGroupCommander.set_joint_value_target方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
class HeadMover():
    """ Moves head to specified joint values """
    def __init__(self):
        self.group_head = MoveGroupCommander('head')
        
    def move_head(self, name, joint_values):
        rospy.loginfo('Moving head to specified position')
        self.group_head.set_joint_value_target(joint_values)
        self.group_head.go()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
class MoveitCommanderMoveGroupState(EventState):
  """
  Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
  """
  
  def __init__(self, planning_group, joint_names):
    """Constructor"""
    super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
                    input_keys=['target_joint_config'])

    self._planning_group = planning_group
    self._joint_names = joint_names
    Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
    self.group_to_move = MoveGroupCommander(self._planning_group)
    Logger.loginfo("finished making mgc in init.")

    self._done = False


  def execute(self, userdata):
    """Execute this state"""
    if self._done is not False:
      return self._done
  
  def on_enter(self, userdata):
    # create the motion goal
    Logger.loginfo("Entering MoveIt Commander code!")

    if len(self._joint_names) != len(userdata.target_joint_config):
      Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
                      % (len(self._joint_names), len(userdata.target_joint_config)))

    self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))               


    # execute the motion
    try: 
      Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
      result = self.group_to_move.go()
    except Exception as e:
      Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
      self._done = "failed"

    if result:
      self._done = "reached"
    else:
      self._done = "failed"

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

        # Initialize the ROS node
        rospy.init_node('moveit_constraints_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the arm move group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Increase the planning time since constraint planning can take a while
        arm.set_planning_time(5)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

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

        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

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

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.263803774718
        target_pose.pose.position.y = 0.295405791959
        target_pose.pose.position.z = 0.690438884208
        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 start state and target pose, then plan and execute
        arm.set_start_state(robot.get_current_state())
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # Close the gripper
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = arm.get_current_pose(end_effector_link)

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 1.0
        # q = quaternion_from_euler(0, 0, -1.57079633)
        # orientation_constraint.orientation.x = q[0]
        # orientation_constraint.orientation.y = q[1]
        # orientation_constraint.orientation.z = q[2]
        # orientation_constraint.orientation.w = q[3]

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the arm
        arm.set_path_constraints(constraints)

        # Set a target pose for the arm
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.39000848183
        target_pose.pose.position.y = 0.185900663329
        target_pose.pose.position.z = 0.732752341378
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
 def __init__(self):
     rospy.init_node('arm_tracker')
     
     rospy.on_shutdown(self.shutdown)
     
     # Maximum distance of the target before the arm will lower
     self.max_target_dist = 1.2
     
     # Arm length to center of gripper frame
     self.arm_length = 0.4
     
     # Distance between the last target and the new target before we move the arm
     self.last_target_threshold = 0.01
     
     # Distance between target and end-effector before we move the arm
     self.target_ee_threshold = 0.025
     
     # Initialize the move group for the right arm
     self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
     
     # Initialize the move group for the right gripper
     right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
     
     # Set the reference frame for pose targets
     self.reference_frame = REFERENCE_FRAME
     
     # Keep track of the last target pose
     self.last_target_pose = PoseStamped()
     
     # Set the right arm reference frame accordingly
     self.right_arm.set_pose_reference_frame(self.reference_frame)
                     
     # Allow replanning to increase the chances of a solution
     self.right_arm.allow_replanning(False)
             
     # Set a position tolerance in meters
     self.right_arm.set_goal_position_tolerance(0.05)
     
     # Set an orientation tolerance in radians
     self.right_arm.set_goal_orientation_tolerance(0.2)
     
     # What is the end effector link?
     self.ee_link = self.right_arm.get_end_effector_link()
     
     # Create the transform listener
     self.listener = tf.TransformListener()
     
     # Queue up some tf data...
     rospy.sleep(3)
     
     # Set the gripper target to closed position using a joint value target
     right_gripper.set_joint_value_target(GRIPPER_CLOSED)
      
     # Plan and execute the gripper motion
     right_gripper.go()
     rospy.sleep(1)
             
     # Subscribe to the target topic
     rospy.wait_for_message('/target_pose', PoseStamped)
     
     # Use queue_size=1 so we don't pile up outdated target messages
     self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
     
     rospy.loginfo("Ready for action!")
     
     while not rospy.is_shutdown():
         try:
             target = self.target
         except:
             rospy.sleep(0.5)
             continue
                     
         # Timestamp the target with the current time
         target.header.stamp = rospy.Time()
         
         # Get the target pose in the right_arm shoulder lift frame
         #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
         target_arm = self.listener.transformPose('right_arm_base_link', target)
         
         # Convert the position values to a Python list
         p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
          
         # Compute the distance between the target and the shoulder link
         dist_target_shoulder = euclidean(p0, [0, 0, 0])
                                      
         # If the target is too far away, then lower the arm
         if dist_target_shoulder > self.max_target_dist:
             rospy.loginfo("Target is too far away")
             self.right_arm.set_named_target('resting')
             self.right_arm.go()
             rospy.sleep(1)
             continue
         
         # Transform the pose to the base reference frame
         target_base = self.listener.transformPose(self.reference_frame, target)
         
         # Compute the distance between the current target and the last target
         p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
         p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
                 
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_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)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # 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(REFERENCE_FRAME)
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        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 "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
    arm.plan() and arm.go()
    # rotate
    for i in range(4):
        # close
        rospy.loginfo("close")
        close_hand()
        # rotate
        angles = arm.get_current_joint_values()
        import numpy
        start_angle = angles[5]
        print("current angles=", start_angle)
        for r in numpy.arange(start_angle, start_angle-3.14*2, -1.0):
            rospy.loginfo(angles)
            angles[5] = r
            rospy.loginfo("rotate (%f)" % (r))
            msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
            msg.trajectory.points = [JointTrajectoryPoint(positions=angles, time_from_start=rospy.Duration(1))]
            client.send_goal(msg)
            client.wait_for_result()

        # open
        rospy.loginfo("open")
        open_hand()
        # back
        angles[5] = start_angle
        arm.set_joint_value_target(angles)
        rospy.loginfo("rotate (%f)" % (r))
        arm.plan() and arm.go()

    rospy.loginfo("done")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

        # 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(REFERENCE_FRAME)
        
        # Allow 10 seconds per planning attempt
        right_arm.set_planning_time(10)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 10
        
        # Set a limit on the number of place attempts
        max_place_attempts = 5
                
        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name        
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        target_id = 'target'
        tool_id = 'tool'
                
        # 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)
        scene.remove_world_object(target_id)
        scene.remove_world_object(tool_id)
        
        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)

        # Set the height of the table off the ground
        table_ground = 0.65
        
        # Set the dimensions of the scene objects [l, w, h]
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Set the target size [l, w, h]
        target_size = [0.02, 0.01, 0.12]
        
        # 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.55
        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()
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
        
        self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
        if self.last_hydra_message.paddles[1].buttons[0] == True:
            self.pub_right_hand_pose.publish(self.tmp_pose_right)
        if self.last_hydra_message.paddles[0].buttons[0] == True:
            self.pub_left_hand_pose.publish(self.tmp_pose_left)
            
        self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
        self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
        self.read_message = False

    def run(self):
        rospy.loginfo("Press LB / RB to send the current pose")
        
        while self.last_hydra_message == None:
            rospy.sleep(0.1)
            
        rospy.loginfo("Got the first data of the razer... Now we can do stuff")
        
        sleep_rate=0.05 # check at 20Hz
        counter = 0
        while True:
            counter += 1
            rospy.loginfo("Loop #" + str(counter))
            if not self.read_message:
                self.read_message = True
                if self.last_hydra_message.paddles[1].buttons[0] == True:
                    # send curr left paddle pos to move_group right
                    rospy.loginfo("sending curr right hand")
                    self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
                    self.right_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[0].buttons[0] == True:
                    # send curr right paddle pos to move_group left
                    rospy.loginfo("sending curr left hand")
                    self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
                    self.left_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[1].trigger > 0.0:
                    # send goal right hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
                    right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
                    self.right_hand_as.send_goal(right_hand_goal)
                    
                if self.last_hydra_message.paddles[0].trigger > 0.0:
                    # send goal left hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
                    left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
                    self.left_hand_as.send_goal(left_hand_goal)
                    
                if self.last_hydra_message.paddles[1].joy[0] != 0.0:
                    # send torso rotation left(neg)/right (pos)
                    rospy.loginfo("Rotation torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Rotation torso sent!")
                    
                if self.last_hydra_message.paddles[1].joy[1] != 0.0:
                    # send torso inclination front(pos)/back(neg)
                    rospy.loginfo("Inclination torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Inclination torso sent!")
                    
                if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
                    twist_goal = Twist()
                    twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
                    twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
                    self.pub_move_base.publish(twist_goal)
                    # move base rotate left (neg)/ right(pos)
                    rospy.loginfo("Move base")
                            
                    
                if self.last_hydra_message.paddles[1].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Right thumb up")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
                    self.right_hand_as.send_goal(right_thumb_up)
                  
                if self.last_hydra_message.paddles[0].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Left thumb up") 
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
                    self.left_hand_as.send_goal(left_thumb_up)
                
                if self.last_hydra_message.paddles[1].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Right thumb down")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
                    self.right_hand_as.send_goal(right_thumb_up)
     
                if self.last_hydra_message.paddles[0].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Left thumb down")
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
                    self.left_hand_as.send_goal(left_thumb_up)            
                
            rospy.sleep(sleep_rate)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
    rospy.loginfo(str(path))

    #waypoints_pa = PoseArray()
    #path = RobotTrajectory()

    for point in path.joint_trajectory.points: # THIS ONLY SENDS THE MESSAGE FOR THE FIRST JOINT
        joint_stt = JointState()
        joint_stt.header.frame_id = 'base_link'
        joint_stt.header.stamp = rospy.Time.now()
#         joint_stt.name.append('arm_right_2_joint')
#         joint_stt.position.append(0.5)
#         joint_stt.velocity.append(0.0)
        joint_stt.name.extend(path.joint_trajectory.joint_names)
        joint_stt.position.extend(point.positions)
        joint_stt.velocity.extend([0.0]*len(path.joint_trajectory.joint_names))
        #joint_stt.effort.append([0.0]*len(path.joint_trajectory.joint_names))
        rospy.loginfo("jointstate is: \n" + str(joint_stt))
        right_arm_mgc.set_joint_value_target(joint_stt)
        right_arm_mgc.go()
    
#     rospy.loginfo("Publishing waypoints in PoseArray /waypoints")
#     pub_waypoints = rospy.Publisher('/waypoints', PoseArray, latch=True)
#     while True:
#         pub_waypoints.publish(waypoints_pa)
#         rospy.sleep(0.1)

    
    #rospy.loginfo("go()")
    #right_arm_mgc.go()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
                
        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface("base_link")
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
 
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)

        # 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(REFERENCE_FRAME)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(15)
        
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5
        
        # Set a limit on the number of place attempts
        max_place_attempts = 3
                
        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Connect to the UBR-1 find_objects action server
        rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
        find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
        find_objects.wait_for_server()
        rospy.loginfo("...connected")
        
        # Give the scene a chance to catch up    
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Open the gripper to the neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
       
        rospy.sleep(1)
        
        # Begin the main perception and pick-and-place loop
        while not rospy.is_shutdown():
            # Initialize the grasping goal
            goal = FindGraspableObjectsGoal()
            
            # We don't use the UBR-1 grasp planner as it does not work with our gripper
            goal.plan_grasps = False
            
            # Send the goal request to the find_objects action server which will trigger
            # the perception pipeline
            find_objects.send_goal(goal)
            
            # Wait for a result
            find_objects.wait_for_result(rospy.Duration(5.0))
            
            # The result will contain support surface(s) and objects(s) if any are detected
            find_result = find_objects.get_result()
    
            # Display the number of objects found
            rospy.loginfo("Found %d objects" % len(find_result.objects))
    
            # Remove all previous objects from the planning scene
            for name in scene.getKnownCollisionObjects():
                scene.removeCollisionObject(name, False)
            for name in scene.getKnownAttachedObjects():
                scene.removeAttachedObject(name, False)
            scene.waitForSync()
            
            # Clear the virtual object colors
            scene._colors = dict()
    
            # Use the nearest object on the table as the target
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_target [as 别名]
    roscpp_initialize(sys.argv)
    rospy.init_node('pumpkin_planning', anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)

    group_variable_values = right_arm.get_current_joint_values()
    print "============ Joint values: ", group_variable_values

    group_variable_values[0] = -0.357569385437786
    group_variable_values[1] = -0.6320268016619928
    group_variable_values[2] = 0.24177580736353846
    group_variable_values[3] = 1.586101553004471
    group_variable_values[4] = -0.5805943181752088
    group_variable_values[5] = -1.1821499952996368
    right_arm.set_joint_value_target(group_variable_values)

    right_arm.go(wait=True)

    print "============ Waiting while RVIZ displays Up..."

    group_variable_values[0] = 1.0
    right_arm.set_joint_value_target(group_variable_values)

    right_arm.go(wait=True)

    group_variable_values = right_arm.get_current_joint_values()

    for i in xrange(6):
        group_variable_values[i] = 0
    right_arm.set_joint_value_target(group_variable_values)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_joint_value_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 arm move group
        arm = MoveGroupCommander('arm')
        
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Set the right arm reference frame
        arm.set_pose_reference_frame('base_link')
                
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
                                        
        # Set an initial position for the arm
        start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]

        # Set the goal pose of the end effector to the stored pose
        arm.set_joint_value_target(start_position)
        
        # Plan and execute a trajectory to the goal configuration
        arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = 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)
                
        # Move end effector to the right 0.3 meters
        wpose.position.y -= 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        # Move end effector up and back
        wpose.position.x -= 0.2
        wpose.position.z += 0.3

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.025,       # 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...")
#.........这里部分代码省略.........