Python moveit_commander.roscpp_initialize函数代码示例

def move_group_python_interface_tutorial():

  ## First initialize moveit_commander and rospy.
  print "============ Starting tutorial setup"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('move_group_python_interface_tutorial',
                  anonymous=True)

  ## Instantiate a RobotCommander object.  This object is an interface to
  ## the robot as a whole.
  robot = moveit_commander.RobotCommander()

  ## Instantiate a PlanningSceneInterface object.  This object is an interface
  ## to the world surrounding the robot.
  scene = moveit_commander.PlanningSceneInterface()

  ## Instantiate a MoveGroupCommander object.  This object is an interface
  ## to one group of joints.  In this case the group is the joints in the left
  ## arm.  This interface can be used to plan and execute motions on the left
  ## arm.
  group = moveit_commander.MoveGroupCommander("arm")

  ## Sometimes for debugging it is useful to print the entire state of the

  print "============ Printing robot end-effector pose"
  #print group.get_current_pose()
  print group.get_current_pose().pose.position.x, group.get_current_pose().pose.position.y, group.get_current_pose().pose.position.z
  print group.get_current_rpy()
  ## When finished shut down moveit_commander.
  moveit_commander.roscpp_shutdown()

def init():
  global robot
  global scene
  global left_arm
  global right_arm
  global left_gripper
  global right_gripper

  robot = None
  scene = None
  left_arm = None
  right_arm = None
  left_gripper = None
  right_gripper = None
  gc.collect()

  #Initialize moveit_commander
  moveit_commander.roscpp_initialize(sys.argv)

  #Initialize both arms
  robot = moveit_commander.RobotCommander()
  scene = moveit_commander.PlanningSceneInterface()
  left_arm = moveit_commander.MoveGroupCommander('left_arm')
  right_arm = moveit_commander.MoveGroupCommander('right_arm')
  left_arm.set_planner_id('RRTConnectkConfigDefault')
  left_arm.set_planning_time(20)
  right_arm.set_planner_id('RRTConnectkConfigDefault')
  right_arm.set_planning_time(20)

	def __init__(self):
		## First initialize moveit_commander and rospy.
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node('move_piece_interface',
				anonymous=True)

		## Instantiate a RobotCommander object.  This object is an interface to
		## the robot as a whole.
		self.robot = moveit_commander.RobotCommander()

		## Instantiate a PlanningSceneInterface object.  This object is an interface
		## to the world surrounding the robot.
		self.scene = moveit_commander.PlanningSceneInterface()

		## Instantiate a MoveGroupCommander object.  This object is an interface
		## to one group of joints.  In this case the group is the joints in the left
		## arm.  This interface can be used to plan and execute motions on the left
		## arm.
		self.left_group = moveit_commander.MoveGroupCommander("left_arm")
		self.right_group = moveit_commander.MoveGroupCommander("right_arm")
		#self.listener = tf.TransformListener()
		#self.listener.waitForTransform("/base", 'left_gripper', rospy.Time(0), rospy.Duration(3.0));
		#self._limb = baxter_interface.Limb('left')
		self._left_gripper = baxter_interface.Gripper('left')
		self._right_gripper = baxter_interface.Gripper('right')
		self.gripper_close()

    def __init__(self):
        smach.State.__init__(self, outcomes=['done'],
                                   input_keys=['goal_pose'])
        
        moveit_commander.roscpp_initialize(sys.argv)

        ## Instantiate a RobotCommander object.  This object is an interface to
        ## the robot as a whole.
        self.robot = moveit_commander.RobotCommander()

        ## Instantiate a PlanningSceneInterface object.  This object is an interface
        ## to the world surrounding the robot.
        rospy.loginfo('Instantiate PlanningSceneInterface')
        self.scene = moveit_commander.PlanningSceneInterface()

        ## Instantiate a MoveGroupCommander object.  This object is an interface
        ## to one group of joints.  In this case the group is the joints in the left
        ## arm.  This interface can be used to plan and execute motions on the left
        ## arm.
        rospy.loginfo('Instantiate MoveGroupCommander')
        self.group = moveit_commander.MoveGroupCommander("manipulator")


        ## We create this DisplayTrajectory publisher which is used below to publish
        ## trajectories for RVIZ to visualize.
        rospy.loginfo('Create display_trajectory_publisher')
        self.display_trajectory_publisher = rospy.Publisher(
                                            '/move_group/display_planned_path',
                                            moveit_msgs.msg.DisplayTrajectory)

def move_robot():
	print "Starting rospy and moveit..."
	moveit_commander.roscpp_initialize(sys.argv)
	rospy.init_node('move_group_py')

	robot = moveit_commander.RobotCommander()
	scene = moveit_commander.PlanningSceneInterface()
	group1 = moveit_commander.MoveGroupCommander("both_arms")



	display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory, queue_size=1)
	group1.allow_replanning(True)
	group1.set_planning_time(10.0)


	print "Waiting for RViz"
	rospy.sleep(10)
	print "Initializing..."

	print "Reference frame: %s" %group1.get_planning_frame()

	print "Generating plan..."
	group_values = group1.get_current_joint_values()
	group_values[1] = 0
	group_values[2] = 0
	group1.set_joint_value_target(group_values)


	plan1 = group1.plan()
	rospy.sleep(5)

	print "Executing..."
	plan1 = group1.go()

    def __init__(self):
        self.state = "INITIALIZING"
        moveit_commander.roscpp_initialize(sys.argv)

        #Start a node
        rospy.init_node('moveit_node')
        
        #Initialize subscriber
        self.sub = rospy.Subscriber("game_state", String, self.state_machine)
        
        #Initialize both arms
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()
        self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
        self.right_arm = moveit_commander.MoveGroupCommander('right_arm')
        self.left_arm.set_planner_id('RRTConnectkConfigDefault')
        self.left_arm.set_planning_time(10)
        self.right_arm.set_planner_id('RRTConnectkConfigDefault')
        self.right_arm.set_planning_time(10)

        #Set up the grippers
        self.left_gripper = baxter_gripper.Gripper('left')
        self.right_gripper = baxter_gripper.Gripper('right')

        #Calibrate the gripper (other commands won't work unless you do this first)
        print('Calibrating...')
        self.right_gripper.calibrate()
        rospy.sleep(0.5)

        #Initialize class variables
        self.all_pieces = [0,1,2,3,4,5,6,7,8,9,13,14,15,16,20]
        self.available_pieces = self.all_pieces

        self.state = "PLAY"

def starting_pose():
	rospy.sleep(20)
	moveit_commander.roscpp_initialize(sys.argv)
	rospy.init_node('move_to_starting_pose',
		              anonymous=True)
		              
	robot = moveit_commander.RobotCommander()

	scene = moveit_commander.PlanningSceneInterface()

	group = moveit_commander.MoveGroupCommander("arm_gp")

	display_trajectory_publisher = rospy.Publisher(
		                                  '/move_group/display_planned_path',
		                                  moveit_msgs.msg.DisplayTrajectory)
		                                  
	print "============ Waiting for RVIZ..."
	
	
	pose_target = geometry_msgs.msg.Pose()
	pose_target.orientation.x = 0.0
	pose_target.orientation.y = -1.0
	pose_target.orientation.z = 0.0
	pose_target.orientation.w = 0.0
	pose_target.position.x = 0.3
	pose_target.position.y = 0.0
	pose_target.position.z = 0.38
	group.set_pose_target(pose_target)
	
	group.go(wait=True)
	
	## When finished shut down moveit_commander.
	moveit_commander.roscpp_shutdown()

def move_arm():
    print "Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_arm_node')
    print "move_arm_node should be initialized"
    print "Initializing Robot Commander"
    robot = moveit_commander.RobotCommander()
    print "Initializing Planning Scene Interface"
    scene = moveit_commander.PlanningSceneInterface()
    print "Let's move left arm first"
    group = moveit_commander.MoveGroupCommander("left_arm")
    
    print "Create publisher to display_planned_path"
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
    
    robot_pose = geometry_msgs.msg.Pose()
    robot_pose.orientation.x = 1

    robot_pose.position.x = 0.7
    robot_pose.position.y = 0.4
    robot_pose.position.z = 0.3
    
    group.set_pose_target(robot_pose)
    plan1 = group.plan()
    rospy.sleep(5)
    group.go(wait=True)
    
    moveit_commander.roscpp_shutdown()

    print "Finishing"

    def setUp(self):
        # create a action client of move group
        self._move_client = SimpleActionClient('move_group', MoveGroupAction)
        self._move_client.wait_for_server()

        moveit_commander.roscpp_initialize(sys.argv)
        group_name = moveit_commander.RobotCommander().get_group_names()[0]
        group = moveit_commander.MoveGroupCommander(group_name)

        # prepare a joint goal
        self._goal = MoveGroupGoal()
        self._goal.request.group_name = group_name
        self._goal.request.max_velocity_scaling_factor = 0.1
        self._goal.request.max_acceleration_scaling_factor = 0.1
        self._goal.request.start_state.is_diff = True

        goal_constraint = Constraints()
        joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
        joint_names = group.get_active_joints()
        for i in range(len(joint_names)):
            joint_constraint = JointConstraint()
            joint_constraint.joint_name = joint_names[i]
            joint_constraint.position = joint_values[i]
            joint_constraint.weight = 1.0
            goal_constraint.joint_constraints.append(joint_constraint)

        self._goal.request.goal_constraints.append(goal_constraint)
        self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True

 def __init__(self):
     # Initialize the move_group API
     moveit_commander.roscpp_initialize(sys.argv)
     
     rospy.init_node('moveit_demo')
             
     # Initialize the MoveIt! commander for the right arm
     arm = moveit_commander.MoveGroupCommander('arm_with_torso')
     
     arm.set_end_effector_link('wrist_roll_link')
     
     # Get the name of the end-effector link
     end_effector_link = arm.get_end_effector_link()
                     
     # Set the reference frame for pose targets
     reference_frame = 'base_link'
     
     # Set the right arm reference frame accordingly
     arm.set_pose_reference_frame(reference_frame)
             
     # Allow replanning to increase the odds of a solution
     arm.allow_replanning(True)
     
     # Allow some leeway in position (meters) and orientation (radians)
     arm.set_goal_position_tolerance(0.01)
     arm.set_goal_orientation_tolerance(0.05)
            
     # Set the start pose.  This particular pose has the gripper oriented horizontally
     # 0.75 meters above the base and 0.72 meters in front of the robot.
     target_pose = PoseStamped()
     target_pose.header.frame_id = reference_frame
     target_pose.header.stamp = rospy.Time.now()     
     target_pose.pose.position.x = 0.609889
     target_pose.pose.position.y = 0.207002
     target_pose.pose.position.z = 1.10677
     target_pose.pose.orientation.x = 0.0
     target_pose.pose.orientation.y = 0.0
     target_pose.pose.orientation.z = 0.0
     target_pose.pose.orientation.w = 1.0
     
     # Set the start state to the current state
     arm.set_start_state_to_current_state()
     
     # Set the goal pose of the end effector to the stored pose
     arm.set_pose_target(target_pose, end_effector_link)
     
     # Plan the trajectory to the goal
     traj = arm.plan()
     
     # Execute the planned trajectory
     arm.execute(traj)
 
     # Pause for a couple of seconds
     rospy.sleep(2)
      
     # Shut down MoveIt cleanly
     moveit_commander.roscpp_shutdown()
     
     # Exit MoveIt
     moveit_commander.os._exit(0)

    def __init__(self):
        rospy.init_node("moveit_cartesian_path", anonymous=False)
        rospy.loginfo("Starting node moveit_cartesian_path")
        rospy.on_shutdown(self.cleanup)
        moveit_commander.roscpp_initialize(sys.argv)
        self._arm = moveit_commander.MoveGroupCommander('manipulator')
        rospy.loginfo("End effector: " + self._arm.get_end_effector_link())
        self._arm.set_pose_reference_frame("/base")

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

        # Allow some leeway in position (meters) and orientation (radians)
        self._arm.set_goal_position_tolerance(0.01)
        self._arm.set_goal_orientation_tolerance(0.1)

        self._joint_order = ('shoulder_pan_joint',
                             'shoulder_lift_joint',
                             'elbow_joint',
                             'wrist_1_joint',
                             'wrist_2_joint',
                             'wrist_3_joint')
        self._joint_sub = rospy.Subscriber("/joint_states", JointState, self.cb_js)
        self._last_check = 0
        self._tcp_location = None
        self._ur_kin = ur5()

def InitializeMoveItCommander():
    moveit_commander.roscpp_initialize(sys.argv)
    # Instantiate a RobotCommander object.  This object is an interface to
    # the robot as a whole.
    robot = moveit_commander.RobotCommander()
    rospy.sleep(1)

    ## Instantiate a PlanningSceneInterface object.  This object is an interface
    ## to the world surrounding the robot.
    global scene
    scene = moveit_commander.PlanningSceneInterface()
    rospy.sleep(1)

    ## Instantiate a MoveGroupCommander object.  This object is an interface
    ## to one group of joints.  In this case the group is the joints in the left
    ## arm.  This interface can be used to plan and execute motions on the left
    ## arm.
    global group
    group = MoveGroupCommander("left_arm")
    
    global left_gripper
    left_gripper = baxter_interface.Gripper('left')
    left_gripper.calibrate()

    rospy.sleep(2)

def launch_script():
    print "====== l_leg_inverse_ik.py started ========"
    print "======= Start initializing move commander for l_leg_inverse ==========="
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('l_leg_inverse_ik', anonymous=True)


    lleg_group = moveit_commander.MoveGroupCommander("l_leg_inverse")
    lleg_group.set_planner_id("RRTConnectkConfigDefault")

    print "\n =========== Some basic informations of Move Commander=================="
    print "Current Pose of end effector %s %s" %(lleg_group.get_end_effector_link(), lleg_group.get_current_pose())
    lleg_group.set_end_effector_link("pelvis")
    print "The end effector is: %s" %lleg_group.get_end_effector_link()
    current_pose = lleg_group.get_current_pose()

    random_target =    lleg_group.get_random_pose()
    print "==============================================================="
    print "Random pose set: %s" %random_target
    lleg_group.set_pose_target(random_target)
    print "==============================================================="
    print "Planning"
    lleg_group.plan()
    print "==============================================================="
    print "Running"
    lleg_group.go(wait=True)

    print "==============================================================="

    def followCartTrajMoveit(self, x_vec, dt):

        moveit_commander.roscpp_initialize(sys.argv)
        group = moveit_commander.MoveGroupCommander("left_arm")

        waypoints = []
        waypoints.append(group.get_current_pose().pose)
        

        for i in range(len(x_vec)): 
        
            #Make sure quaternion is normalized
            x_vec[i][3:7] = x_vec[i][3:7] / la.norm(x_vec[i][3:7])

            wpose = geometry_msgs.msg.Pose()
            wpose.position.x = x_vec[i][0]
            wpose.position.y = x_vec[i][1]
            wpose.position.z = x_vec[i][2]
            wpose.orientation.x = x_vec[i][3]
            wpose.orientation.y = x_vec[i][4]
            wpose.orientation.z = x_vec[i][5]
            wpose.orientation.w = x_vec[i][6]
            waypoints.append(copy.deepcopy(wpose))

        (plan, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             dt,          # eef_step
                             0.0)         # jump_threshold
        print "fraction: ", fraction
        print "plan: ", plan
        group.execute(plan)        

def main():
    global left_arm
    global left_gripper
    #Initialize moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)

    #Start a node
    rospy.init_node('moveit_interface')

    #Set up Baxter Arms
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    left_arm = moveit_commander.MoveGroupCommander('left_arm')
    left_arm.set_planner_id('RRTConnectkConfigDefault')
    left_arm.set_planning_time(10)

    #Calibrate the end effector on the left arm
    left_gripper = baxter_gripper.Gripper('left')
    print('Calibrating...')
    left_gripper.calibrate()
    rospy.sleep(2.0)
    print('Ready to go')


    #make subscriber to position topic
    rospy.Subscriber('new_position', Pose, move_arm)

    #make subscriber to gripper topic
    rospy.Subscriber('gripper_control', Bool, actuate_gripper)

    rospy.spin()