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 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 stateCallback(self, data):
		self.newPose = data
		#newPose = self.group.get_current_pose().pose
		#newPose.position.x = newPose.position.x +0.0005
		#newPose.position.y = newPose.position.y +0.0005
		#newPose.position.z = newPose.position.z +0.0005
		self.group.set_pose_target(self.newPose)

		# Compute plan and visualize if successful
		print self.newPose
		newPlan = self.group.plan()

		# Wait for Rviz to display
		print "Rviz displaying"
		rospy.sleep(5)

		# Perform plan on Robot
		self.group.go(wait=True)

		rospy.sleep(5)
		self.group.clear_pose_targets()
		#print  self.group.get_planning_frame()
		## Adding/Removing Objects and Attaching/Detaching Objects
		## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
		## First, we will define the collision object message
		collision_object = moveit_msgs.msg.CollisionObject()

		## When finished shut down moveit_commander.
		moveit_commander.roscpp_shutdown()

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_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 main():
    rospy.init_node('smach_example_actionlib')


    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
    sm.userdata.tgt_pose = geometry_msgs.msg.Pose()
    
    with sm:

        # Initialize robot pose
        smach.StateMachine.add('INIT', Init(),
                               transitions={'done':'READ_NEXT_POSE'})

        # Read next pose from KB
        smach.StateMachine.add('READ_NEXT_POSE', ReadNextGoal(),
                               transitions={'cont':'MOVE_ARM',
                                            'finished':'succeeded'},
                               remapping={'next_pose':'tgt_pose'})

        # Move to pose
        smach.StateMachine.add('MOVE_ARM', MoveIt(),
                               transitions={'done':'READ_NEXT_POSE'},
                               remapping={'goal_pose':'tgt_pose'})

    # start visualizer and introspection server
    sis = smach_ros.IntrospectionServer('smach_tests', sm, '/SM_ROOT')
    sis.start()

    # Execute SMACH plan
    outcome = sm.execute()
    
    rospy.spin()
    sis.stop()
    moveit_commander.roscpp_shutdown()
    rospy.signal_shutdown('All done.')

    def __init__(self):
        # rospy.sleep(10) # wait for other files to be launched successfully

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cw3_q1')

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        # self.touch = [0,0,0,0]

        # rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff)
        # rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf)
        # rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf)
        # rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th)
        # # Robot contains the entire state of the robot (iiwa and shadow hand)
        # robot = moveit_commander.RobotCommander()
        # # We can get a list of all the groups in the robot
        # print('============ Robot Groups:')
        # print('{}\n\n'.format(robot.get_group_names()))

        self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
        self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')

        rospy.sleep(1)

        # object tracking
        [all_object_name,all_object_pose] = self.get_object_position()

        # create a service proxy for add and remove objects to allowed collision matrix
        add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
        remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)

        # open the hand
        self.hand_open()

        for i in range(0,3):
            # i = 2 # object no.

            # move the iiwa to just above the object
            self.move_iiwa_to_object(all_object_pose,i)

            # add object to allowed collision matrix
            add_to_acm(all_object_name[i])

            # close the hand (grasping)
            self.hand_close()

            # hold the grasping position for 10 seconds
            rospy.sleep(10)

            # open the hand (releasing)
            self.hand_open()

            # remove object from allowed collision matrix
            remove_from_acm(all_object_name[i])

        moveit_commander.roscpp_shutdown()
        rospy.loginfo("Task finished")

def main():
    try:
        crui_manager = CRUIManager()
        loop = rospy.Rate(10)

        while not rospy.is_shutdown():
            loop.sleep()
        moveit_commander.roscpp_shutdown()
    except rospy.ROSInterruptException:
        rospy.signal_shutdown(reason="Interrupted")

    def cleanup(self):
        rospy.loginfo("Stopping the robot")

        # Stop any current arm movement
        self._arm.stop()

        #Shut down MoveIt! cleanly
        rospy.loginfo("Shutting down Moveit!")
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def __init__(self):
        rospy.init_node('xm_arm_moveit_ik_test', anonymous=True)
        moveit_commander.roscpp_initialize(sys.argv)
        xm_arm = moveit_commander.MoveGroupCommander('xm_arm')
        end_effector_link = xm_arm.get_end_effector_link()
        reference_frame = 'base_footprint'

        xm_arm.set_pose_reference_frame(reference_frame)
        xm_arm.allow_replanning(True)
        xm_arm.set_goal_position_tolerance(5)
        xm_arm.set_goal_orientation_tolerance(5)
        xm_arm.set_planner_id("RRTConnectkConfigDefault")

        xm_arm.set_named_target('initial')
        xm_arm.go()
        rospy.sleep(2)

        print "-------------------- Test Arm Moveit IK ----------------------"
        print "Input the position of object(reference frame is base's center)"
        print "Such as [object/o x y z x y z w]                                "
        while not rospy.is_shutdown():
            print ">>> ",
            keyboard_cmd = raw_input().split(" ")
            try:
                if keyboard_cmd[0] == "object" or keyboard_cmd[0] == "o":
                    target_pose = PoseStamped()
                    target_pose.header.frame_id = reference_frame
                    target_pose.header.stamp = rospy.Time.now()
                    target_pose.pose.position.x = float(keyboard_cmd[1])
                    target_pose.pose.position.y = float(keyboard_cmd[2])
                    target_pose.pose.position.z = float(keyboard_cmd[3])
                    target_pose.pose.orientation.x = float(keyboard_cmd[4])
                    target_pose.pose.orientation.y = float(keyboard_cmd[5])
                    target_pose.pose.orientation.z = float(keyboard_cmd[6])
                    target_pose.pose.orientation.w = float(keyboard_cmd[7])
                    print ("position[x y z] %s, %s, %s" % (keyboard_cmd[1],
                                                           keyboard_cmd[2],
                                                           keyboard_cmd[3]))
                    print "orientation[x y z w] %s, %s, %s, %s" % (
                        keyboard_cmd[4],
                        keyboard_cmd[5],
                        keyboard_cmd[6],
                        keyboard_cmd[7])
                    xm_arm.set_start_state_to_current_state()
                    xm_arm.set_pose_target(target_pose, end_effector_link)
                    trajectory = xm_arm.plan()
                    xm_arm.execute(trajectory)
                    rospy.sleep(1)
                elif keyboard_cmd[0] == "exit" or keyboard_cmd[0] == "e":
                    exit()
            except Exception as exce:
                print "Error!", exce

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    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)

        GRIPPER_OPEN = [0.030, 0.030]
        GRIPPER_CLOSED = [0.002, 0.002]
        GRIPPER_NEUTRAL = [0.01, 0.01]

        # Connect to the arm move group
        arm = moveit_commander.MoveGroupCommander('arm')

        # Connect to the gripper move group
        gripper = moveit_commander.MoveGroupCommander('gripper')

        rospy.loginfo("mw gripper name= " + gripper.get_name())
        rospy.loginfo("mw gripper active joints= " + str(gripper.get_active_joints()))
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

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

        # Display the name of the end_effector link
        rospy.loginfo("The end effector link is: " + str(end_effector_link))

        # Set a small tolerance on joint angles
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.002)

        # Set the gripper target to neutal position using a joint value target
        gripper.set_joint_value_target(GRIPPER_OPEN)

        # Plan and execute the gripper motion
        gripper.go()
        rospy.sleep(0.1)
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

        start_time = datetime.datetime.now()

        # gripper.set_joint_value_target(GRIPPER_CLOSED)
        # gripper.go()
        # rospy.sleep(0.1)

        stop_time = datetime.datetime.now()

        print "took: %s\n" % str(stop_time - start_time)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

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

    def __init__(self):
        # Initialize the move_group API and node
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_fk_demo', anonymous=True)

        # Use the groups of SmartPal5
        arm = moveit_commander.MoveGroupCommander(GROUP_NAME_ARM)
        gripper_left = moveit_commander.MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Set a goal joint tolerance
        arm.set_goal_joint_tolerance(0.001)
        gripper_left.set_goal_joint_tolerance(0.001)

        # Use the pose stored in the SRDF file
        # 1. Set the target pose
        # 2. Plan a trajectory
        # 3. Execute the planned trajectory
        arm.set_named_target('l_arm_init')
        traj = arm.plan()
        arm.execute(traj)
        rospy.sleep(1)

        gripper_left.set_named_target('l_gripper_init')
        gripper_left.go()
        rospy.sleep(1)

        # Use the joint positions with FK
        # 1. Set the target joint values
        # 2. Plan a trajectory
        # 3. Execute the planned trajectory
        joint_positions = [0.0, 0.07, 0.0, 1.5707, 0.0, 0.0, 0.0]
        arm.set_joint_value_target(joint_positions)
        arm.go()
        rospy.sleep(1)

        gripper_left.set_named_target('l_gripper_open')
        gripper_left.go()
        rospy.sleep(1)

        # Return
        arm.set_named_target('l_arm_init')
        traj = arm.plan()
        arm.execute(traj)
        rospy.sleep(1)

        gripper_left.set_named_target('l_gripper_init')
        gripper_left.go()
        rospy.sleep(1)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

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

    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_demo')
                
        right_arm = moveit_commander.MoveGroupCommander('right_arm')
        right_arm.set_named_target('resting')
        right_arm.go()   

        end_effector_link = right_arm.get_end_effector_link()         

        reference_frame = '/odom'
        
        right_arm.set_pose_reference_frame(reference_frame)
	rospy.loginfo(str(end_effector_link))
	rospy.loginfo(right_arm.get_planning_frame())
	rospy.loginfo(right_arm.get_pose_reference_frame())
	rospy.loginfo(right_arm.get_current_pose())
        
        right_arm.allow_replanning(True)

        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)

        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.441402636719
        target_pose.pose.position.y = -0.279191735455
        target_pose.pose.position.z = 0.863870298601
        target_pose.pose.orientation.x = -0.0281863349718
        target_pose.pose.orientation.y = 0.063611003310
        target_pose.pose.orientation.z = 0.765618014453
        target_pose.pose.orientation.w = 0.63952187353

        right_arm.set_start_state_to_current_state()

        right_arm.set_pose_target(target_pose, end_effector_link)

        traj = right_arm.plan()

        right_arm.execute(traj)

        rospy.sleep(1)

        right_arm.set_named_target('resting')

        right_arm.go()


        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_test_cartesian', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)

        rospy.sleep(2)

        pose = PoseStamped().pose

        # same orientation for all
        q = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
        pose.orientation = Quaternion(*q)

        i = 1
        while i <= 10:
            waypoints = list()
            j = 1
            while j <= 5:
                # random pose
                rand_pose = arm.get_random_pose(arm.get_end_effector_link())
                pose.position.x = rand_pose.pose.position.x
                pose.position.y = rand_pose.pose.position.y
                pose.position.z = rand_pose.pose.position.z
                waypoints.append(copy.deepcopy(pose))
                j += 1

            (plan, fraction) = arm.compute_cartesian_path(
                                      waypoints,   # waypoints to follow
                                      0.01,        # eef_step
                                      0.0)         # jump_threshold

            print "====== waypoints created ======="
            # print waypoints

            # ======= show cartesian path ====== #
            arm.go()
            rospy.sleep(10)
            i += 1

        print "========= end ========="

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
                
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.02)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        
        # Move to the named "straight_forward" pose stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)
        
        # Move back to the resting position at roughly 1/4 speed
        right_arm.set_named_target('resting')

        # Get back the planned trajectory
        traj = right_arm.plan()
        
        # Scale the trajectory speed by a factor of 0.25
        new_traj = scale_trajectory_speed(traj, 0.25)

        # Execute the new trajectory     
        right_arm.execute(new_traj)
        rospy.sleep(1)

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script
        moveit_commander.os._exit(0)

def main():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_quadrotor', anonymous=True)

    q = Quadrotor(display=False, debug=True)

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        p = q.get_current_pose()
        p.x += random.uniform(-2.5, 2.5)
        p.y += random.uniform(-2.5, 2.5)
        p.z += random.uniform(-2.5, 2.5)
        p.yaw += random.uniform(angles.d2r(-90.0), angles.d2r(90.0))
        p.z = max(p.z, 0.5)
        q.moveto_pose(p, wait=True)
        rate.sleep()

    moveit_commander.roscpp_shutdown()

    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('cobra_move_position', anonymous=True)

        arm = MoveGroupCommander(ARM_GROUP)

        arm.allow_replanning(True)
        arm.set_planning_time(10)

        arm.set_named_target(START_POSITION)
        arm.go()
        rospy.sleep(2)

        print "======== create new goal position ========"

        start_pose = PoseStamped()
        start_pose.header.frame_id = arm.get_planning_frame()

        # Test Position
        start_pose.pose.position.x = 0.2  # 20 cm
        start_pose.pose.position.y = -0.11  # -11 cm
        start_pose.pose.position.z = 0.6  # 60 cm
        q = quaternion_from_euler(0.0, 0.0, 0.0)
        start_pose.pose.orientation = Quaternion(*q)

        print start_pose

        print "====== move to position ======="


        # KDL
        # arm.set_joint_value_target(start_pose, True)

        # IK Fast
        arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
                                arm.get_end_effector_link())

        arm.go()
        rospy.sleep(2)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def FindBlock(self):
        print "==============="
        print "FIND BLOCK"
        print "==============="
        #Get information about the markers found
        rospy.Subscriber("/demo/found_markers",Int16MultiArray, callback1)
        msg1 = rospy.wait_for_message('/demo/found_markers',Pose)
        print msg1.data
        detectedMarkers = msg1.data

        chosenMarker = self.GetTargetBlock(detectedMarkers)

        if chosenMarker != -1:
            try:
                msg = Pose()
                position = Vector3()
                orientation = Quaternion()
                targetMarker = 'ar_marker_'+str(chosenMarker)
                print "the marker frame chosen", targetMarker               
                (trans,rot) = self.listener.lookupTransform('/base',targetMarker,rospy.Time(0))

                Px = trans[0]
                Py = trans[1]
                Pz = -0.171 #this value dependent on height of table
                Qx = 0.0
                Qy = 1.0
                Qz = 0.0
                Qw = 0.0

                # Return the position/orientation of the target
                self.Block_Position = [Px,Py,Pz] 
                self.Block_Quaternion = [Qx,Qy,Qz,Qw]
                print self.Block_Position 

            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                pass
        else:
            user_input = raw_input('No blocks left. Please give me more blocks. Press ''Enter'' when ready!!!')
            if user_input == 's':
                moveit_commander.roscpp_shutdown()
            else:   
                self.FindBlock()

def move_group_python_interface():
    print "============ Starting move_group_python_interface setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', 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.
    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.sleep(1)
    group = moveit_commander.MoveGroupCommander("arm")
    
    ## Getting Basic Information
    ## ^^^^^^^^^^^^^^^^^^^^^^^^^
    ##
    ## We can get the name of the reference frame for this robot
    # print "============ Reference frame: %s" % group.get_planning_frame()
    
    # ## We can also print the name of the end-effector link for this group
    # print "============ Reference frame: %s" % group.get_end_effector_link()
    
    ## We can get a list of all the groups in the robot
    print "============ Robot Groups:"
    print robot.get_group_names()
    
    ## Sometimes for debugging it is useful to print the entire state of the
    ## robot.
    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"
    rospy.spin()
    moveit_commander.roscpp_shutdown()

def launch_script():

    

#======================================INVERSE LEG=====================================================

    #print("====================================================")
    #print("Kill node l_leg_ik")
    #t = time.time()-t_start
    #print(t)
    #print("====================================================")
    #os.system('rosnode kill l_leg_ik')
    #print("====================================================")
    #print("Killed in")
    #print(time.time()-t_start-t)
    #print("seconds")
    #print("====================================================")


    print "============ SPINNING NOW ========================"


    moveit_commander.roscpp_shutdown()
    rospy.spin()