Пример #1
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 __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)
Пример #3
0
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()
Пример #5
0
	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()
Пример #6
0
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")
Пример #8
0
def main():
    try:
        grasp_analyzer_node = GraspAnalyzerNode()
        loop = rospy.Rate(10)

        while not rospy.is_shutdown():
            loop.sleep()
        moveit_commander.roscpp_shutdown()
    except rospy.ROSInterruptException:
        rospy.signal_shutdown()
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")
Пример #10
0
    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)
Пример #12
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)
Пример #13
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)
Пример #14
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)
Пример #17
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()
Пример #18
0
    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)
Пример #19
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()
Пример #20
0
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()
Пример #21
0
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()
Пример #22
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('manipulator')

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.01)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述,
        # 姿态使用四元数描述,基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = 0.2593
        target_pose.pose.position.y = 0.0636
        target_pose.pose.position.z = 0.1787
        target_pose.pose.orientation.x = 0.70692
        target_pose.pose.orientation.y = 0.0
        target_pose.pose.orientation.z = 0.0
        target_pose.pose.orientation.w = 0.70729

        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()

        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(target_pose, end_effector_link)

        # 规划运动路径
        traj = arm.plan()

        # 按照规划的运动路径控制机械臂运动
        arm.execute(traj)
        rospy.sleep(1)

        # 控制机械臂终端向右移动5cm
        # arm.shift_pose_target(1, 0.1, end_effector_link)
        # arm.go()
        # rospy.sleep(1)

        # 控制机械臂终端反向旋转90度
        # arm.shift_pose_target(3, -1.57, end_effector_link)
        # arm.go()
        # rospy.sleep(1)

        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #23
0



p = PoseStamped()
p.header.frame_id = acHan.planning_frame()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.0
p.pose.orientation.w = 1.0
#size = [0.1,0.1,0.1]
#scene.remove_world_object("1")

#x = scene.add_box("1", p, size)


pos = acHan.current_jointState()
print(pos)
pos.position[0] = pos.position[0] + 0.5

acHan.Transport_Empty(4, p)

rospy.signal_shutdown("Done")
os._exit(0)
roscpp_shutdown()





Пример #24
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_dem')

        # Initialize the move group for the right arm
        right_arm = moveit_commander.MoveGroupCommander('arm')

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

        # 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 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.01)
        right_arm.set_goal_orientation_tolerance(0.05)

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)

        # Set the target pose.  This particular pose has the gripper oriented horizontally
        # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
        # the center of the robot base.
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = 0.10
        target_pose.pose.position.y = 0.10
        target_pose.pose.position.z = 0.85
        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
        right_arm.set_start_state_to_current_state()

        # Set the goal pose of the end effector to the stored pose
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Plan the trajectory to the goal
        traj = right_arm.plan()

        # Execute the planned trajectory
        right_arm.execute(traj)

        # Pause for a second
        rospy.sleep(1)

        # Shift the end-effector to the right 5cm
        right_arm.shift_pose_target(1, -0.05, end_effector_link)
        right_arm.go()
        rospy.sleep(1)

        # Rotate the end-effector 90 degrees
        right_arm.shift_pose_target(3, -1.57, end_effector_link)
        right_arm.go()
        rospy.sleep(1)

        # Store this pose as the new target_pose
        saved_target_pose = right_arm.get_current_pose(end_effector_link)

        # Move to the named pose "wave"
        #right_arm.set_named_target('wave')
        #right_arm.go()
        #rospy.sleep(1)

        # Go back to the stored target
        #right_arm.set_pose_target(saved_target_pose, end_effector_link)
        #right_arm.go()
        #rospy.sleep(1)

        # Finish up in the resting position
        #right_arm.set_named_target('resting')
        #right_arm.go()

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
Пример #25
0
 def close(self):
     print("closing GazeboSmartBotPincherKinectEnv")
     # When finished, shut down moveit_commander.
     moveit_commander.roscpp_shutdown()
     super(GazeboSmartBotPincherKinectEnv, self).close()
Пример #26
0
def main():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node("joint_state_player_node")

    waypoint_file = rospy.get_param('wpt_file', 'scan_waypoints2.npy')
    limb_name = rospy.get_param('limb_name', 'left_arm')

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()

    arm = moveit_commander.MoveGroupCommander(limb_name)

    waypoints = np.load(waypoint_file)  #calib_sim3.npy #calib_real_right03.npy

    rate = rospy.Rate(100)
    joint_angle_target = np.zeros(7)
    selected_idx = -1
    while not rospy.is_shutdown():

        c = getch()

        if c in ['n']:
            selected_idx = (selected_idx + 1) % len(waypoints)

            joint_angle_target = waypoints[selected_idx]
            print("Selected joint goal %d: " % (selected_idx, ),
                  joint_angle_target)

        elif c in ['b']:
            selected_idx -= 1
            if selected_idx < 0:
                selected_idx = len(waypoints) - 1

            joint_angle_target = waypoints[selected_idx]
            print("Selected joint goal %d: " % (selected_idx, ),
                  joint_angle_target)

        elif c in ['a']:
            current_joint_angles = arm.get_current_joint_values()
            joint_angles = np.asarray(current_joint_angles)

            waypoints = np.vstack([waypoints, joint_angles])
            print("Added new waypoint. Number of waypoints %d." %
                  (len(waypoints, )))
            print("Joint angles: ", joint_angles)

        elif c in ['d']:

            if len(waypoints) > 0:
                waypoints = np.delete(waypoints, (selected_idx), axis=0)
                print("Deleted %dth waypoint. Number of waypoints %d." %
                      (selected_idx + 1, len(waypoints, )))
            else:
                print("Waypoint list is empty.")

        elif c in ['e']:

            current_joint_angles = np.asarray(arm.get_current_joint_values())

            before = np.array(waypoints[selected_idx])
            waypoints[selected_idx] = current_joint_angles
            print("Editting joint angles %d: " % (selected_idx, ), before)
            print("Set to", current_joint_angles)

        elif c in ['s']:
            print("Saving current waypoint list.")
            filename = raw_input("Filename:")
            np.save('%s.npy' % (filename, ), waypoints)
            print("Saved.")

        elif c in ['g']:

            try:
                arm.set_joint_value_target(joint_angle_target)
                arm.set_max_velocity_scaling_factor(0.35)
                arm.set_max_acceleration_scaling_factor(0.35)
                arm.go()
            except:
                print("Not possible to go to waypoint. Try another one")
        elif c in ['\x1b', '\x03']:
            rospy.signal_shutdown("Bye.")

        rate.sleep()

    moveit_commander.roscpp_shutdown()
Пример #27
0
def move():
    # Initialize the move_group API
    moveit_commander.roscpp_initialize(sys.argv)
    # Initialize the move group for the ur5_arm
    arm = moveit_commander.MoveGroupCommander("ur5_arm")
    try:
        cjs = rospy.get_param('current_joint_state')
    except:
        cjs = [0, 0, 0, 0, 0, 0]
    jt = RobotState()
    jt.joint_state.header.frame_id = '/base_link'
    jt.joint_state.name = [
        'front_left_wheel', 'front_right_wheel', 'rear_left_wheel',
        'rear_right_wheel', 'ur5_arm_shoulder_pan_joint',
        'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint',
        'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint',
        'ur5_arm_wrist_3_joint', 'left_tip_hinge', 'right_tip_hinge'
    ]
    jt.joint_state.position = [
        0, 0, 0, 0, cjs[0], cjs[1], cjs[2], cjs[3], cjs[4], cjs[5], 0, 0
    ]
    # Set the start state to the current state
    arm.set_start_state(jt)

    arm.set_joint_value_target('ur5_arm_shoulder_pan_joint', 1.57)
    arm.set_joint_value_target('ur5_arm_shoulder_lift_joint', 1.57)
    arm.set_joint_value_target('ur5_arm_elbow_joint', -1.57)
    arm.set_joint_value_target('ur5_arm_wrist_1_joint', 1.57)
    arm.set_joint_value_target('ur5_arm_wrist_2_joint', cjs[4])
    arm.set_joint_value_target('ur5_arm_wrist_3_joint', cjs[5])  #3.14159)

    traj = arm.plan()
    arm.execute(traj)

    rospy.sleep(5.0)
    # Stop any current arm movement
    arm.stop()

    pub = rospy.Publisher('gripper/cmd_vel', Twist, queue_size=1)
    twist = Twist()
    speed = .5
    turn = 1
    x = 0
    y = 0
    z = 0
    th = 1  # To open gripper (1) use th = 1
    twist.linear.x = x * speed
    twist.linear.y = y * speed
    twist.linear.z = z * speed
    twist.angular.x = 0
    twist.angular.y = 0
    twist.angular.z = th * turn

    ct = 0
    rest_time = 0.1
    tot_time = 1

    while ct * rest_time < tot_time:
        pub.publish(twist)
        rospy.sleep(0.1)
        ct = ct + 1

    #rospy.set_param('smach_state','turnedValve')

    #Shut down MoveIt! cleanly
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
Пример #28
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_cartesian_demo', anonymous=True)

        # 是否需要使用笛卡尔空间的运动规划
        cartesian = True

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('left_manipulator')

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame('leftbase_link')

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.1)

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 获取当前位姿数据最为机械臂运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose

        # 初始化路点列表
        waypoints = []

        # 将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)

        # 设置第二个路点数据,并加入路点列表
        # 第二个路点需要向后运动0.2米,向右运动0.2米
        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 设置第三个路点数据,并加入路点列表
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 设置第四个路点数据,回到初始位置,并加入路点列表
        if cartesian:
            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  #已经尝试规划次数

            # 设置机器臂当前的状态作为运动初始状态
            arm.set_start_state_to_current_state()

            # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path(
                    waypoints,  # waypoint poses,路点列表
                    0.01,  # eef_step,终端步进值
                    0.0,  # jump_threshold,跳跃阈值
                    True)  # avoid_collisions,避障规划

                # 尝试次数累加
                attempts += 1

                # 打印运动规划进程
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) +
                                  " attempts...")

            # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)
                rospy.loginfo("Path execution complete.")
            # 如果路径规划失败,则打印失败信息
            else:
                rospy.loginfo("Path planning failed with only " +
                              str(fraction) + " success after " +
                              str(maxtries) + " attempts.")

        # # 控制机械臂回到初始化位置
        # arm.set_named_target('home')
        # arm.go()
        # rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #29
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_pick_and_place_demo')

        # 初始化场景对象
        scene = PlanningSceneInterface()

        # 创建一个发布场景变化信息的发布者
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=10)

        # 创建一个发布抓取姿态的发布者
        self.gripper_pose_pub = rospy.Publisher('gripper_pose',
                                                PoseStamped,
                                                queue_size=10)

        # 创建一个存储物体颜色的字典对象
        self.colors = dict()

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 设置目标位置所使用的参考坐标系
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # 设置每次运动规划的时间限制:5s
        arm.set_planning_time(5)

        # 设置pick和place阶段的最大尝试次数
        max_pick_attempts = 5
        max_place_attempts = 5
        rospy.sleep(2)

        # 设置场景物体的名称
        table_id = 'table'
        target_id = 'target'

        # 移除场景中之前运行残留的物体
        scene.remove_world_object(table_id)
        scene.remove_world_object(target_id)

        # 移除场景中之前与机器臂绑定的物体
        scene.remove_attached_object(GRIPPER_FRAME, target_id)
        rospy.sleep(1)

        # 控制机械臂和夹抓先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        gripper.set_named_target('finger_home')
        gripper.go()
        rospy.sleep(3)

        # 设置桌面的高度
        table_ground = 0.1

        # 设置table、box1和box2的三维尺寸[长, 宽, 高]
        table_size = [1, 2, 0.01]

        # 将三个物体加入场景当中
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 1.5
        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)

        # 将桌子设置成红色,两个box设置成橙色
        self.setColor(table_id, 0.8, 0, 0, 1.0)

        # 设置目标物体的尺寸
        target_size = [0.5, 0.05, 0.2]

        # 设置目标物体的位置,位于桌面之上两个盒子之间
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 1.5
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0

        # 将抓取的目标物体加入场景中
        scene.add_box(target_id, target_pose, target_size)

        # 将目标物体设置为黄色
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

        # 将场景中的颜色设置发布
        self.sendColors()

        # 设置支持的外观
        arm.set_support_surface_name(table_id)

        # 设置一个place阶段需要放置物体的目标位置
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 1.5
        place_pose.pose.position.y = 0.3
        place_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        # 将目标位置设置为机器人的抓取目标位置
        grasp_pose = target_pose

        # 生成抓取姿态
        grasps = self.make_grasps(grasp_pose, [target_id])

        # 将抓取姿态发布,可以在rviz中显示
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # 追踪抓取成功与否,以及抓取的尝试次数
        result = None
        n_attempts = 0

        # 重复尝试抓取,直道成功或者超多最大尝试次数
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            result = arm.pick(target_id, grasps)
            rospy.sleep(0.2)

        # 如果pick成功,则进入place阶段
        if result == MoveItErrorCodes.SUCCESS:
            result = None
            n_attempts = 0

            # 生成放置姿态
            places = self.make_places(place_pose)

            # 重复尝试放置,直道成功或者超多最大尝试次数
            while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                n_attempts += 1
                rospy.loginfo("Place attempt: " + str(n_attempts))
                for place in places:
                    result = arm.place(target_id, place)
                    if result == MoveItErrorCodes.SUCCESS:
                        break
                rospy.sleep(0.2)

            if result != MoveItErrorCodes.SUCCESS:
                rospy.loginfo("Place operation failed after " +
                              str(n_attempts) + " attempts.")
        else:
            rospy.loginfo("Pick operation failed after " + str(n_attempts) +
                          " attempts.")

        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 控制夹爪回到张开的状态
        # gripper.set_joint_value_target(GRIPPER_OPEN)
        gripper.set_named_target('finger_home')
        gripper.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #30
0
def stop_ros(reason):
    rospy.signal_shutdown(reason)
    roscpp_shutdown()
Пример #31
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        sim = False; # for RPi2
        # sim = True;

        rospy.init_node('moveit_demo')
                
        # Initialize the move group for the right arm
        arm1 = moveit_commander.MoveGroupCommander('arm1')
                
        # Get the name of the end-effector link
        # end_effector_link = arm1.get_end_effector_link()
        arm1.set_end_effector_link('link6')
        end_effector_link = arm1.get_end_effector_link()
                        
        # Set the reference frame for pose targets
        reference_frame = 'base_link'
        # reference_frame = 'link5'
        
        # Set the right arm reference frame accordingly
        arm1.set_pose_reference_frame(reference_frame)
                
        # Allow replanning to increase the odds of a solution
        arm1.allow_replanning(False)
        
        # Allow some leeway in position (meters) and orientation (radians)
        arm1.set_goal_position_tolerance(0.01)
        arm1.set_goal_orientation_tolerance(0.05)
        
        # Start the arm in the "init" pose stored in the SRDF file
        arm1.set_named_target('init')
        arm1.go()
        if sim:
            rospy.sleep(2)
               
#         # Set the target pose.  This particular pose has the gripper oriented horizontally
#         # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of 
#         # the center of the robot base.
#         target_pose = PoseStamped()
#         target_pose.header.frame_id = reference_frame
#         target_pose.header.stamp = rospy.Time.now()     
#         target_pose.pose.position.x = 0.20
#         target_pose.pose.position.y = 0.3
#         target_pose.pose.position.z = 0.55
#         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
        arm1.set_start_state_to_current_state()
        
        # Set the goal pose of the end effector to the stored pose
#        arm1.set_pose_target(target_pose, end_effector_link)

        # Shift the end-effector to the right 5cm
        arm1.shift_pose_target(0, -0.1, end_effector_link)  # roll
        
        # Plan the trajectory to the goal
        traj = arm1.plan()
        
        # Execute the planned trajectory
        arm1.execute(traj)
    
        if sim:
            rospy.sleep(2)
        
        # Shift (3/Roll) the end-effector to the left 5cm
        arm1.shift_pose_target(0, 0.1, end_effector_link)
        
        # Plan the trajectory to the goal
        traj = arm1.plan()
        
        # Execute the planned trajectory
        arm1.execute(traj)
    
        if sim:
            rospy.sleep(2)
        
        # # Shift the end-effector to the right 5cm
        # arm1.shift_pose_target(1, -0.05, end_effector_link)
        # arm1.go()
        # if sim:
        #     rospy.sleep(2)
  
#         # Rotate the end-effector 90 degrees
#         arm1.shift_pose_target(3, -1.57, end_effector_link)
#         arm1.go()
#         rospy.sleep(1)
#           
#         # Store this pose as the new target_pose
#         saved_target_pose = arm1.get_current_pose(end_effector_link)
#           
#         # Move to the named pose "vertical"
#         arm1.set_named_target('vertical')
#         arm1.go()
#         rospy.sleep(1)
#           
#         # Go back to the stored target
#         arm1.set_pose_target(saved_target_pose, end_effector_link)
#         arm1.go()
#         rospy.sleep(1)
           
#        # Finish up in the init position  
#        arm1.set_named_target('init')
#        arm1.go()

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)
Пример #32
0
def move_joints():
    print "============ Starting tutorial setup"
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

    # instantiate robot commander object
    robot = moveit_commander.RobotCommander()

    # instantiate planning scene interface object
    scene = moveit_commander.PlanningSceneInterface()

    # instantiate arm move group commanders
    left = moveit_commander.MoveGroupCommander("left_arm")
    right = moveit_commander.MoveGroupCommander("right_arm")

    # publishes trajectory to visualize
    display_trajectory_publisher = rospy.Publisher(
        '/move_group/display_planned_path',
        moveit_msgs.msg.DisplayTrajectory,
        queue_size=20)

    # Gather basic information
    print "============ Reference frame left: %s" % left.get_planning_frame()
    print "============ Reference frame right: %s" % right.get_planning_frame()

    print "============ End effector left: %s" % left.get_end_effector_link()
    print "============ End effector right: %s" % right.get_end_effector_link()

    print "============ Robot Groups:"
    print robot.get_group_names()

    print "============ Printing robot state"
    print robot.get_current_state()
    print "============"

    print "============ Printing robot variables"
    print robot.get_current_variable_values()
    print "============"

    print "============ Printing left hand values"
    print "%s" % left.get_end_effector_link()

    # current set of left arm joint values
    #group_variable_values = left.get_current_joint_values()
    #print "============ Joint values left: %s" % group_variable_values
    #group_variable_values[0] = 1.0
    d = {
        "right_s0": 0.0,
        "right_s1": 0.0,
        "right_e0": 0.0,
        "right_e1": 0.0,
        "right_w0": 0.0,
        "right_w1": 0.0,
        "right_w2": 0.0
    }
    right.set_joint_value_target(d)

    plan = left.plan()

    print "============ Waiting while RVIZ displays plan2..."
    rospy.sleep(5)

    print "============ Printing robot variables"
    print robot.get_current_variable_values()
    print "============"

    #left.execute(plan2)

    left.clear_pose_targets()

    moveit_commander.roscpp_shutdown()
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # We need a tf2 listener to convert poses into arm reference base
        try:
            self._tf2_buff = tf2_ros.Buffer()
            self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
        except rospy.ROSException as err:
            rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
            raise err

        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
                                                (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
        
        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) 

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface(REFERENCE_FRAME)

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

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

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

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

        # 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 5 seconds per planning attempt
        arm.set_planning_time(15)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 1

        # Set a limit on the number of place attempts
        max_place_attempts = 3

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

        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")

        rospy.sleep(1)
 
        arm.set_named_target('right_up')
        if arm.go() != True:
            rospy.logwarn("  Go failed")

        gripper.set_joint_value_target(self.gripper_opened)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")

        rospy.sleep(1)

        
        # Initialize the grasping goal
        goal = FindGraspableObjectsGoal()

        goal.plan_grasps = False

        find_objects.send_goal(goal)

        find_objects.wait_for_result(rospy.Duration(5.0))

        find_result = find_objects.get_result()

        rospy.loginfo("Found %d objects" %len(find_result.objects))

        for name in self.scene.getKnownCollisionObjects():
            self.scene.removeCollisionObject(name, False)
        for name in self.scene.getKnownAttachedObjects():
            self.scene.removeAttachedObject(name, False)

        self.scene.waitForSync()

        self.scene._colors = dict()

        # Use the nearest object on the table as the target
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_id = 'target'
        target_size = None
        the_object = None
        the_object_dist = 0.30
        the_object_dist_min = 0.10
        count = -1
        
        for obj in find_result.objects:
            count +=1
            
            dx = obj.object.primitive_poses[0].position.x
            dy = obj.object.primitive_poses[0].position.y
            d = math.sqrt((dx * dx) + (dy * dy))

            if d < the_object_dist and d > the_object_dist_min:
                #if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
                #    if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
                rospy.loginfo("object is in the working zone")
                the_object_dist = d

                the_object = count

                target_size = [0.02, 0.02, 0.05]

                target_pose = PoseStamped()
                target_pose.header.frame_id = REFERENCE_FRAME

                target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
                target_pose.pose.position.y = obj.object.primitive_poses[0].position.y 
                target_pose.pose.position.z = 0.04

                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

                # wait for the scene to sync
                self.scene.waitForSync()
                self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
                self.scene.sendColors()

                grasp_pose = target_pose

                grasp_pose.pose.position.y -= target_size[1] / 2.0
                grasp_pose.pose.position.x += target_size[0]

                grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])

                # Track success/failure and number of attempts for pick operation
                for grasp in grasps:
                    self.gripper_pose_pub.publish(grasp.grasp_pose)
                    rospy.sleep(0.2)
                
                # Track success/failure and number of attempts for pick operation
                result = None
                n_attempts = 0
                
                # Set the start state to the current state
                arm.set_start_state_to_current_state()
                    
                # Repeat until we succeed or run out of attempts
                while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
                    result = arm.pick(target_id, grasps)
                    n_attempts += 1
                    rospy.loginfo("Pick attempt: " +  str(n_attempts))
                    rospy.sleep(1.0)
            else:
                rospy.loginfo("object is not in the working zone")
                rospy.sleep(1)

        arm.set_named_target('right_up')
        arm.go()
    
        # Open the gripper to the neutral position
        gripper.set_joint_value_target(self.gripper_neutral)
        gripper.go()

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Пример #34
0
def random_search():
    global locations
    global target
    global flag
    ''' Default configuration for MoveIt'''
    # Initializing the node representing the robot
    moveit_commander.roscpp_initialize(sys.argv)

    flag = False

    rospy.init_node("random_search")
    rospy.Subscriber("aruco_map/locations", Float32MultiArray,
                     handle_map_locations)
    rospy.sleep(2)

    # Provides information such as the robot's kinematic model and the robot's current joint states
    robot = moveit_commander.RobotCommander()

    # Provides a remote interface for getting, setting, and updating the robot's internal understanding of the
    # surrounding world:
    scene = moveit_commander.PlanningSceneInterface()

    # Is an interface to a planning group (group of joints).Used to plan and execute motions
    group_name = "fanuc_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)
    for i in range(0, 1):
        if flag == True:
            #val = random.randint(0,3)        #print("Going to ArUco ID: %d"%randint(0,1))
            #print val
            #
            target = geometry_msgs.msg.Pose()

            target.position.x = locations[
                0 + i] - 0.01  # Coordinate x of aruco maker ID 0
            target.position.y = locations[
                5 + i] + 0.01  # Coordinate y of aruco maker ID 0
            target.position.z = locations[
                10 + i] + 0.08  # Coordinate z of aruco maker ID 0
            print(target.position.x)
            print(target.position.y)
            print(target.position.z)

            target.orientation.x = 0.7071  # target_orientation.x
            target.orientation.y = 0.7071  # target_orientation.y
            target.orientation.z = 0  # target_orientation.z
            target.orientation.w = 0  # target_orientation.w

            #print target

            move_group.set_pose_target(target)
            print('\x1b[6;31;43m' + '>>> Fanuc ARC Mate 120iBe >>' +
                  '\x1b[0m' + '\x1b[6;30;43m' + " " +
                  'Approaching ArUco ID:  %d  ' % i + " " + '\x1b[0m')
            plan = move_group.go(wait=True)
            flag = False

    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = 0
    joint_goal[1] = 0
    joint_goal[2] = 0
    joint_goal[3] = 0
    joint_goal[4] = 0
    joint_goal[5] = 0

    # The go command can be called with joint values, poses, or without any
    # parameters if you have already set the pose or joint target for the group
    plan2 = move_group.go(joint_goal, wait=True)
    ''' STAGE X : Shuting everything down'''
    print('\x1b[6;31;43m' + '>>> Fanuc ARC Mate 120iBe >>' + '\x1b[0m' +
          '\x1b[6;30;43m' + " " + 'Shuting Down...' + " " + '\x1b[0m')
    move_group.stop()
    flag = False
    move_group.clear_pose_targets()
    moveit_commander.roscpp_shutdown()
    exit()
Пример #35
0
    def __init__(self):

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_cartesian', anonymous=True)
        cartesian = rospy.get_param('~cartesian', True)

        #set cartesian parameters
        ur5_manipulator = MoveGroupCommander('ur5_manipulator')
        ur5_manipulator.allow_replanning(True)
        ur5_manipulator.set_pose_reference_frame('base_link')
        ur5_manipulator.set_goal_position_tolerance(0.01)
        ur5_manipulator.set_goal_orientation_tolerance(0.1)
        end_effector_link = ur5_manipulator.get_end_effector_link()

        #set the ur5 initial pose
        ur5_manipulator.set_named_target('ready')
        ur5_manipulator.go()

        #get the end effort information
        start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
        print("The first waypoint:")
        print(start_pose)
        #define waypoints
        waypoints = []
        waypoints.append(start_pose)

        wpose = deepcopy(start_pose)
        wpose.position.z -= 0.3
        print("The second waypoint:")
        print(wpose)
        waypoints.append(deepcopy(wpose))
        print(" ")
        print(waypoints)

        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = ur5_manipulator.compute_cartesian_path(
                    waypoints, 0.01, 0.0, True)
                attempts += 1

                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) +
                                  " attempts...")

            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                ur5_manipulator.execute(plan)
                rospy.sleep(2)
                rospy.loginfo("Path execution complete.")

            else:
                rospy.loginfo("Path planning failed with only " +
                              str(fraction) + " success after " +
                              str(maxtries) + " attempts.")

        #ur5_manipulator.set_named_target('home')
        #ur5_manipulator.go()
        rospy.sleep(3)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #36
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.05]
        GRIPPER_CLOSED = [-0.03]
        GRIPPER_NEUTRAL = [0.01]

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

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

        # Get the name of the end-effector link
        end_effector_link = right_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
        right_arm.set_goal_joint_tolerance(0.001)
        right_gripper.set_goal_joint_tolerance(0.001)

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

        # Plan a trajectory to the goal configuration
        traj = right_arm.plan()

        # Execute the planned trajectory
        right_arm.execute(traj)

        # Pause for a moment
        rospy.sleep(1)

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

        # Plan and execute the gripper motion
        right_gripper.go()
        rospy.sleep(1)

        # Set target joint values for the arm: joints are in the order they appear in
        # the kinematic tree.
        joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]

        # Set the arm's goal configuration to the be the joint positions
        right_arm.set_joint_value_target(joint_positions)

        # Plan and execute the motion
        right_arm.go()
        rospy.sleep(1)

        # Save this configuration for later
        right_arm.remember_joint_values('saved_config', joint_positions)

        # Close the gripper as if picking something up
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)

        # Set the arm target to the named "straight_out" pose stored in the SRDF file
        right_arm.set_named_target('straight_forward')

        # Plan and execute the motion
        right_arm.go()
        rospy.sleep(1)

        # Set the goal configuration to the named configuration saved earlier
        right_arm.set_named_target('saved_config')

        # Plan and execute the motion
        right_arm.go()
        rospy.sleep(1)

        # Open the gripper as if letting something go
        right_gripper.set_joint_value_target(GRIPPER_OPEN)
        right_gripper.go()
        rospy.sleep(1)

        # Return the arm to the named "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(1)

        # Return the gripper target to neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
    rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/block.urdf -urdf -x 0.5 -y -0.0 -z 0.77 -model block
    """

    table_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'table.urdf'
    table_name = 'table'
    table_pose = Pose(position=Point(
        x=0.85, y=0.0,
        z=0.70 + 0.03))  # increase z by 0.03 to make gripper reach block

    table2_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'table2.urdf'
    table2 = 'table2'
    table2_pose = Pose(position=Point(x=0.0, y=0.85, z=0.4 + 0.03))

    block_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'block.urdf'
    block_name = 'block'
    block_pose = Pose(position=Point(x=0.5, y=0.0, z=0.74 + 0.03))

    dropbox_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'dropbox.urdf'
    dropbox = 'dropbox'
    dropbox_pose = Pose(position=Point(x=0, y=-0.85, z=0.24 + 0.03))

    delete_gazebo_model([table_name, table2, block_name, dropbox])
    spawn_gazebo_model(table_path, table_name, table_pose)
    spawn_gazebo_model(table2_path, table2, table2_pose)
    spawn_gazebo_model(block_path, block_name, block_pose)
    spawn_gazebo_model(dropbox_path, dropbox, dropbox_pose)

    main()

    roscpp_shutdown()
Пример #38
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)

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

        # Get the name of the end-effector link
        end_effector_link = right_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
        right_arm.set_goal_tolerance(0.0001)

        right_arm.set_named_target('right')
        right_arm.go()
        rospy.sleep(1)

        start_pose = right_arm.get_current_pose().pose
        start_pose.position.x = 0.4
        start_pose.position.y = 0
        #start_pose.position.z = 0.4
        start_pose.orientation.x = 0.707106781186547
        start_pose.orientation.y = 0
        start_pose.orientation.z = 0
        start_pose.orientation.w = 0.707106781186547

        right_arm.set_pose_target(start_pose)

        right_arm.go()

        # Pause for a moment
        rospy.sleep(1)

        #    joints_positions = [0.7, 0.2, 0.5, 1]
        #    right_arm.set_joint_value_target(joints_positions)

        #    right_arm.go()
        #    rospy.sleep(1)
        star = [[-0.2853, 0.3925], [0.2853, 0.3925], [-0.1763, 0.0573],
                [0, 0.6], [0.1763, 0.0573], [-0.2853, 0.3925]]

        waypoints = []
        wpose = deepcopy(start_pose)
        waypoints.append(deepcopy(wpose))

        end_pose = deepcopy(start_pose)

        for i in range(6):
            wpose.position.x = 0.4
            wpose.position.y = star[i][0]
            wpose.position.z = star[i][1]

            waypoints.append(deepcopy(wpose))
        '''
        wpose.position.z -= 0.15
        waypoints.append(deepcopy(wpose))

        wpose.position.x = 0.4
        wpose.position.y = -0.2853
        wpose.position.z = 0.3925
        waypoints.append(deepcopy(wpose))
        '''
        #waypoints.append(end_pose)

        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...")

        # If we have a complete plan, execute the trajectory
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            right_arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) +
                          " success after " + str(maxtries) + " attempts.")
        ''' 
        with open ("/home/jyk/Desktop/data.txt",'w') as f:
            for point in plan.joint_trajectory.points:
                    f.write(str(point.positions).replace('(','').replace(')','').replace(',',' '))
                    f.write('\n')
        '''

        rospy.sleep(3)
        # Return the arm to the named "resting" pose stored in the SRDF file
        right_arm.set_named_target('right')
        right_arm.go()
        rospy.sleep(1)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Пример #39
0
#!/usr/bin/env python
import sys, rospy, tf, moveit_commander, random
from geometry_msgs.msg import Pose, Point, Quaternion

orient = [Quaternion(*tf.transformations.quaternion_from_euler(3.14, -1.5, -1.57)),
          Quaternion(*tf.transformations.quaternion_from_euler(3.14, -1.5, -1.57))]
pose = [Pose(Point( 0.5, -0.5, 1.3), orient[0]),
        Pose(Point(-0.5, -0.5, 1.3), orient[1])]
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('r2_wave_arm',anonymous=True)
group = [moveit_commander.MoveGroupCommander("left_arm"),
         moveit_commander.MoveGroupCommander("right_arm")]
# now, wave arms around randomly 
while not rospy.is_shutdown():
  pose[0].position.x =  0.5 + random.uniform(-0.1, 0.1)
  pose[1].position.x = -0.5 + random.uniform(-0.1, 0.1)
  for side in [0,1]:
    pose[side].position.z =  1.5 + random.uniform(-0.1, 0.1)
    group[side].set_pose_target(pose[side])
    group[side].go(True)

moveit_commander.roscpp_shutdown()
    def __init__(self):

        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)

        # 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()))

        # Planning groups are used to control seperate aspects of the robot.
        # iiwa_group controls the iiwa arm from the base link to the end effector
        # hand group controls all of the joints of the hand.

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

        # print('\n\nhand_iiwa Group Information')
        # print('============ Reference frame: {}'.format(iiwa_group.get_planning_frame()))
        # print('============ End effector: {}\n\n'.format(iiwa_group.get_end_effector_link()))
        #
        # print('sr_hand Group Information')
        # print('============ Reference frame: {}'.format(hand_group.get_planning_frame()))
        # print('============ End effector: {}'.format(hand_group.get_end_effector_link()))

        rospy.sleep(1)

        # The hand has a very strict controller and will occasionally say it as failed even though it has moved to the right
        # position (just not in the right amount of time), ensure you check the achieved position.


        # Objects in the scene can be found using the object tracker node which publishes on the topic
        # '/recognized_objects_array' and to tf. The list of object names is found in the param '/object_list'

    ## ===================================   Object Tracking  =========================================================
        objects = rospy.get_param('object_list')
        print(objects)
        all_object_pose = []
        no_object = len(objects)

        for i in range(0,no_object):
            while True:
                try:
                    object_pose = self.tf_buffer.lookup_transform('world', objects[i], rospy.Time.now())
                    rospy.sleep(1)
                except (tf2_ros.ExtrapolationException,tf2_ros.LookupException):
                    continue
                break
            print('Found object {} [{}] at: \n{}'.format(i,objects[i],object_pose.transform))
            # store the object pose to new message
            all_object_pose.append(object_pose)

        iiwa_pose = self.tf_buffer.lookup_transform('world','hand_iiwa_link_0',rospy.Time.now())
        rospy.sleep(1)
        print('Found iiwa start Pos at: \n{}'.format(iiwa_pose.transform))
    ## ===================================== end object tracking =================================================
        # TF doesn't always return as transform even when the transform exists, try catching the execption, waiting a second
        # and looking up the transform again.

        # To grasp objects they must first be added to the allowed collision matrix so that the path planner knows to ignore
        #  the collision. You can do this using a service '/add_object_acm', they can also be removed from the acm after
        # grasping to prevent any unintended collisions.

        add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
        remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)
        i = 1 # object no.
        success = add_to_acm(objects[i])
        print success

        ## open the hand
        # units in radian
        ff = [0,0,0,0]
        mf = [0,0,0,0]
        rf = [0,0,0,0]
        # open [-0.6,1.2,0   ,0]
        # grab [0.2 ,1.2,0.65,1]
        th = [-0.6,1.2,0,0]
        self.open_joint_values = ff+mf+rf+th
        self.hand_group.set_joint_value_target(self.open_joint_values)
        if self.hand_group.go() != True:
            rospy.logwarn("Go failed")
            # Retry
            print self.hand_group.go()

        checkPos = self.hand_group.get_current_joint_values()
        print checkPos[-4:]

        # move the iiwa just above object
        pose = self.iiwa_group.get_current_pose(self.iiwa_group.get_end_effector_link())

        obj_quat = (all_object_pose[i].transform.rotation.x,all_object_pose[i].transform.rotation.y,all_object_pose[i].transform.rotation.z,all_object_pose[i].transform.rotation.w)
        euler = euler_from_quaternion(obj_quat)
        new_quat = quaternion_from_euler(euler[0]+math.pi,euler[1],euler[2]+math.pi/2)
        # print new_quat

        pose.pose.position = all_object_pose[i].transform.translation
        pose.pose.position.x -= 0.00 # 0.02 for object 0 and 2, 0 for object 1
        pose.pose.position.y += 0.00 # 0.01 for object 0 and 2, 0 for object 1
        pose.pose.position.z += 0.35 # 0.3 for object 0 and 2, 0.35 for object 1

        pose.pose.orientation.x = new_quat[0]
        pose.pose.orientation.y = new_quat[1]
        pose.pose.orientation.z = new_quat[2]
        pose.pose.orientation.w = new_quat[3]

        self.iiwa_group.set_pose_target(pose)
        if self.iiwa_group.go != True:
            rospy.logwarn(" Arm failed")
            print self.iiwa_group.go()

        object_name = objects[i]+'__link_0'

        ff = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        mf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        rf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        th = [0.22,1.20,0.65,1.1]
        self.closed_joint_values = ff+mf+rf+th


        target_pose_stamped = PoseStamped()
        target_pose_stamped.header.frame_id = 'world'
        target_pose_stamped.pose = pose.pose
        grasp_pose = target_pose_stamped
        grasps = self.make_grasps(grasp_pose, object_name)

        print grasps

        result = self.hand_group.pick(object_name,grasps)
        print result
        # # hand_group.set_named_target('open')
        # # hand_group.set_named_target('fingers_pack_thumb_open')
        #
        # # plan = hand_group.plan()
        #
        # # hand_group.set_named_target('open_grasp')
        # # hand_group.set_named_target('fingers_pack_thumb_open')
        #
        # ## grasping
        #
        # # move three fingers
        # # units in radian
        # ff = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        # mf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        # rf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
        # th = [0.22,1.20,0.65,1.1]
        # joint_values = ff+mf+rf+th
        # # hand_group.pick(object_name,joint_values)
        # plan = hand_group.plan(joint_values)
        # # print('Plan result: {}\n\n'.format(plan))
        # hand_group.execute(plan, wait=True)
        # # # rospy.sleep(10)
        # #
        # # checkPos = hand_group.get_current_joint_values()
        # # print checkPos[-4:]
        #
        # ## attach object
        # object_name = objects[i]+'__link_0'
        # hand_group.attach_object(object_name,'lh_palm')
        #
        # ## Lifting
        # pose = iiwa_group.get_current_pose(iiwa_group.get_end_effector_link())
        # pose.pose.position.z += 0.2
        #
        # # iiwa_group.clear_pose_targets()
        # print('iiwa planning to pose: {}'.format(pose.pose))
        # iiwa_group.set_pose_target(pose)
        # result = iiwa_group.plan()
        # # print('Plan result: {}'.format(result))
        # # rospy.sleep(5)
        # iiwa_group.execute(result, wait=True)
        # rospy.sleep(10)
        #
        # # Put it back to original position
        # pose = iiwa_group.get_current_pose(iiwa_group.get_end_effector_link())
        # pose.pose.position.z -= 0.2
        #
        # # iiwa_group.clear_pose_targets()
        # print('iiwa planning to pose: {}'.format(pose.pose))
        # iiwa_group.set_pose_target(pose)
        # result = iiwa_group.plan()
        # # # print('Plan result: {}'.format(result))
        # # # rospy.sleep(5)
        # iiwa_group.execute(result, wait=True)


        # # check the object position
        # for i in range(0,no_object):
        #     while True:
        #         try:
        #             object_pose = tf_buffer.lookup_transform('world', objects[i], rospy.Time.now())
        #             rospy.sleep(1)
        #         except (tf2_ros.ExtrapolationException,tf2_ros.LookupException):
        #             continue
        #         break
        #     print('Found object {} [{}] at: \n{}'.format(i,objects[i],object_pose.transform))
        #     # store the object pose to new message
        #     all_object_pose.append(object_pose)

        success = remove_from_acm(objects[i])

        moveit_commander.roscpp_shutdown()
Пример #41
0
 def release(self):
     #release planner
     rospy.logwarn("Commander Shutdown")
     moveit_commander.roscpp_shutdown()
Пример #42
0
def picknplace():
    # Define positions.
    pos1 = {
        'left_e0': -1.4580487388850858,
        'left_e1': 1.627553615946424,
        'left_s0': 0.07669903939427068,
        'left_s1': -0.3539660668045592,
        'left_w0': -1.9155585088719105,
        'left_w1': -1.4124128104454947,
        'left_w2': -0.6438884357149024,
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    pos2 = {
        'left_e0': -0.949534106616211,
        'left_e1': 1.4994662184448244,
        'left_s0': -0.6036214393432617,
        'left_s1': -0.7869321432861328,
        'left_w0': -2.4735440176391603,
        'left_w1': -1.212228316241455,
        'left_w2': -0.8690001153442384,
        'right_e0': 1.8342575250183106,
        'right_e1': 1.8668546167236328,
        'right_s0': -0.45674277907104494,
        'right_s1': -0.21667478604125978,
        'right_w0': -1.2712865765075685,
        'right_w1': 1.7472041154052735,
        'right_w2': -2.4582042097778323
    }
    lpos1 = {
        'left_e0': -1.4580487388850858,
        'left_e1': 1.627553615946424,
        'left_s0': 0.07669903939427068,
        'left_s1': -0.3539660668045592,
        'left_w0': -1.9155585088719105,
        'left_w1': -1.4124128104454947,
        'left_w2': -0.6438884357149024
    }

    placegoal = geometry_msgs.msg.Pose()
    placegoal.position.x = 0.55
    placegoal.position.y = 0.28
    placegoal.position.z = 0
    placegoal.orientation.x = 1.0
    placegoal.orientation.y = 0.0
    placegoal.orientation.z = 0.0
    placegoal.orientation.w = 0.0
    # Define variables.
    offset_zero_point = 0.903
    table_size_x = 0.714655654394
    table_size_y = 1.05043717328
    table_size_z = 0.729766045265
    center_x = 0.457327827197
    center_y = 0.145765166941
    center_z = -0.538116977368
    # The distance from the zero point in Moveit to the ground is 0.903 m.
    # The value is not allways the same. (look in Readme)
    center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
    pressure_ok = 0
    j = 0
    k = 0
    start = 1
    locs_x = []
    # Initialize a list for the objects and the stacked cubes.
    objlist = [
        'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
        'obj09', 'obj10', 'obj11'
    ]
    boxlist = [
        'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
        'box09', 'box10', 'box11'
    ]
    # Clear planning scene.
    p.clear()
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move both arms to start state.
    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)
    time.sleep(0.5)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Loop to continue pick and place until all objects are cleared from table.
    while locs_x or start:
        # Only for the start.
        if start:
            start = 0

    # Receive the data from all objects from the topic "detected_objects".
        temp = rospy.wait_for_message("detected_objects", PoseArray)
        locs = temp.poses

        locs_x = []
        locs_y = []
        orien = []
        size = []

        # Adds the data from the objects.
        for i in range(len(locs)):
            locs_x.append(locs[i].position.x)
            locs_y.append(locs[i].position.y)
            orien.append(locs[i].position.z * pi / 180)
            size.append(locs[i].orientation.x)

    # Filter objects list to remove multiple detected locations for same objects.
        ind_rmv = []
        for i in range(0, len(locs)):
            if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
                ind_rmv.append(i)
                continue
            for j in range(i, len(locs)):
                if not (i == j):
                    if sqrt((locs_x[i] - locs_x[j])**2 +
                            (locs_y[i] - locs_y[j])**2) < 0.018:
                        ind_rmv.append(i)

        locs_x = del_meth(locs_x, ind_rmv)
        locs_y = del_meth(locs_y, ind_rmv)
        orien = del_meth(orien, ind_rmv)
        size = del_meth(size, ind_rmv)

        # Do the task only if there are still objects on the table.
        if locs_x:
            # Clear planning scene.
            p.clear()
            # Add table as attached object.
            p.attachBox('table',
                        table_size_x,
                        table_size_y,
                        table_size_z,
                        center_x,
                        center_y,
                        center_z,
                        'base',
                        touch_links=['pedestal'])
            # Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
            ig0 = itemgetter(0)
            sorted_lists = zip(*sorted(
                zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
            locs_x = list(sorted_lists[1])
            locs_y = list(sorted_lists[2])
            orien = list(sorted_lists[3])
            size = list(sorted_lists[0])
            # Initialize the data of the biggest object on the table.
            xn = locs_x[0]
            yn = locs_y[0]
            zn = -0.16
            thn = orien[0]
            sz = size[0]
            if thn > pi / 4:
                thn = -1 * (thn % (pi / 4))

            # Add the detected objects into the planning scene.
            #for i in range(1,len(locs_x)):
            #p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
            # Add the stacked objects as collision objects into the planning scene to avoid moving against them.
            #for e in range(0, k):
            #p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
            if k > 0:
                p.attachBox(boxlist[0],
                            0.07,
                            0.07,
                            0.0275 * k,
                            placegoal.position.x,
                            placegoal.position.y,
                            center_z_cube,
                            'base',
                            touch_links=['cubes'])
            p.waitForSync()
            # Move both arms to the second position.
            both_arms.set_joint_value_target(pos2)
            both_arms.plan()
            both_arms.go(wait=True)
            # Initialize the approach pickgoal (5 cm to pickgoal).
            approach_pickgoal = geometry_msgs.msg.Pose()
            approach_pickgoal.position.x = xn
            approach_pickgoal.position.y = yn
            approach_pickgoal.position.z = zn + 0.05

            approach_pickgoal_dummy = PoseStamped()
            approach_pickgoal_dummy.header.frame_id = "base"
            approach_pickgoal_dummy.header.stamp = rospy.Time.now()
            approach_pickgoal_dummy.pose.position.x = xn
            approach_pickgoal_dummy.pose.position.y = yn
            approach_pickgoal_dummy.pose.position.z = zn + 0.05
            approach_pickgoal_dummy.pose.orientation.x = 1.0
            approach_pickgoal_dummy.pose.orientation.y = 0.0
            approach_pickgoal_dummy.pose.orientation.z = 0.0
            approach_pickgoal_dummy.pose.orientation.w = 0.0

            # Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
            approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles(
                approach_pickgoal_dummy.pose, 0.0, 0.0, thn)
            approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x
            approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y
            approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z
            approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w
            # Move to the approach goal and the pickgoal.
            pickgoal = deepcopy(approach_pickgoal)
            pickgoal.position.z = zn
            move("left", approach_pickgoal, pickgoal)
            time.sleep(0.5)
            # Read the force in z direction.
            b = leftarm.endpoint_effort()
            z_ = b['force']
            z_force = z_.z
            print("----->", z_force)
            # Search again for objects, if the gripper isn't at the right position and presses on an object.
            #print("----->force in z direction:", z_force)
            if z_force > -4:
                leftgripper.close()
                attempts = 0
                pressure_ok = 1
                # If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
                while (leftgripper.force() < 25 and pressure_ok == 1):
                    time.sleep(0.04)
                    attempts += 1
                    if (attempts > 50):
                        leftgripper.open()
                        pressure_ok = 0
                        print("----->pressure is to low<-----")
            else:
                print("----->gripper presses on an object<-----")

    # Move back to the approach pickgoal.
            pickgoal.position.z = zn + 0.05
            move("left", pickgoal)
            # Move to the second position.
            both_arms.set_joint_value_target(pos1)
            both_arms.plan()
            both_arms.go(wait=True)

            if pressure_ok and z_force > -4:
                # Define the approach placegoal.
                # Increase the height of the tower every time by 2.75 cm.
                approached_placegoal = deepcopy(placegoal)
                approached_placegoal.position.z = -0.145 + (k * 0.0275) + 0.08
                # Define the placegoal.
                placegoal.position.z = -0.145 + (k * 0.0275)
                move("left", approached_placegoal, placegoal)
                leftgripper.open()
                while (leftgripper.force() > 10):
                    time.sleep(0.01)
        # Move to the approach placegoal.
                move("left", approached_placegoal)
                k += 1

            # Move left arm to start position.
            left_arm.set_joint_value_target(lpos1)
            left_arm.plan()
            left_arm.go(wait=True)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    p.clear()
    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
Пример #43
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node(NODE_NAME, anonymous=True)
        self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2)
        self.rate = rospy.Rate(2)

        # 是否需要使用笛卡尔运动规划
        self.cartesian = rospy.get_param('~cartesian', True)

        # 初始化需要使用move group控制的机械臂中的self.arm group
        self.arm = MoveGroupCommander(ARM_GROUP)
        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(REFERENCE_FRAME)
        # 获取终端link的名称
        end_effector_link = 'link_6'
        # 当运动规划失败后,允许重新规划
        self.arm.allow_replanning(True)

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.1)

        # 环境建模
        scene = PlanningSceneInterface()
        self.scene_pub = rospy.Publisher('planning_scene',
                                         PlanningScene,
                                         queue_size=5)
        self.colors = dict()
        rospy.sleep(1)

        # 场景物体设置
        table_id = 'table'
        scene.remove_world_object(table_id)
        #
        table_groud = 0.5 - HEIGHT_OF_END_EFFECTOR
        table_size = [0.5, 1.5, 0.04]
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.6
        table_pose.pose.position.y = -0.5
        table_pose.pose.position.z = table_groud - table_size[2] / 2.0 - 0.02
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        self.setColor(table_id, 0, 0.8, 0, 1.0)
        self.sendColors()

        # 第五轴的关节角度旋转,从水平到竖直
        joint_positions = [0, 0, 0, 0, 1.571, 0]
        self.arm.set_joint_value_target(joint_positions)
        self.arm.go()
        rospy.sleep(1)

        # 全局变量,对比目标点位置
        global forward_pose
        forward_pose = Twist()

        # 全局变量,计数
        global item_num
        item_num = 0

        while not rospy.is_shutdown():
            # 开启web端action通讯,等待讯号
            self.server = actionlib.SimpleActionServer('abb_grasp',
                                                       AbbGraspAction,
                                                       self.abb_execute, False)
            self.server.start()
            rospy.spin()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #44
0
def launch_script():
    ## BEGIN_TUTORIAL
  ##
  ## Setup
  ## ^^^^^
  ## CALL_SUB_TUTORIAL imports
  ##
  ## First initialize moveit_commander and rospy.
  print"Starting the script"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('simple_script',
          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
  ## leg.  This interface can be used to plan and execute motions on the left
  ## leg.
  lleg_group_single = moveit_commander.MoveGroupCommander("l_leg_single")
  #rleg_group = moveit_commander.MoveGroupCommander("r_leg")


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

  ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
  print "============ Waiting for RVIZ to startup for 10 seconds"
  rospy.sleep(10)
  print "============ Starting script "

  ## Getting Basic Information
  ## ^^^^^^^^^^^^^^^^^^^^^^^^^
  ##
  ## We can get the name of the reference frame for this robot
  print "\n =========== Some basic informations of Move Commander=================="
  #print "Get active Link %s" 		% lleg_group_single.get_active_joints()
  #print "Current Joint Values %s" 	% lleg_group_single.get_current_joint_values()
  print "Current Pose %s" 		% lleg_group_single.get_current_pose()
  current_pose = lleg_group_single.get_current_pose()
  #current_pose.z = current_pose.z - 0.1 
  #print "Current RPY %s" 		% lleg_group_single.get_current_rpy()
  #print "End effector Link %s" 		% lleg_group_single.get_end_effector_link()
  #print "Setting end effector link to pelvis"
  #lleg_group_single.set_end_effector_link("pelvis")
  #print "End effector Link %s" 		% lleg_group_single.get_end_effector_link()

  #print "New Pose: %s" % current_pose
  #print "Get Joints %s" 		% lleg_group_single.get_joints()
  #print "known_constraints %s" 		% lleg_group_single.get_known_constraints()
  #print "get_name %s" 			% lleg_group_single.get_name()
  #print "path_constraints %s" 		% lleg_group_single.get_path_constraints()
  #print "planning_frame() %s" 		% lleg_group_single.get_planning_frame()
  #print "planning_time %s" 		% lleg_group_single.get_planning_time()
  #print "pose_reference_frame %s" 	% lleg_group_single.get_pose_reference_frame()
  #print "random_joint_values %s" 	% lleg_group_single.get_random_joint_values()
  #print "random_pose %s" 		% lleg_group_single.get_random_pose()
  #print "variable_count %s" 		% lleg_group_single.get_variable_count()


  #print "\n =========== Some basic informations of Robot Commander===================="
  #print "current_variable_values    %s" 	% robot.get_current_variable_values()
  #print "group_names   %s" 			% robot.get_group_names()
  #print "get_joint_names     %s" 		% robot.get_joint_names()
  #print "get_link_names  %s" 			% robot.get_link_names()
  #print "get_planning_frame %s" 		% robot.get_planning_frame()
  #print "get_root_link %s" 			% robot.get_root_link()
  ## Planning to a Pose goal
  ## ^^^^^^^^^^^^^^^^^^^^^^^
  ## We can plan a motion for this group to a desired pose for the 
  ## end-effector
  print "Going to current pose"
  #pos_target = geometry_msgs.msg.Point()
  #pose_target.orientation.w = 1.0
  #pos_target.position.x = 0
  #pos_target.position.y = 0 
  #pos_target.position.z = -0.5
  #pos_target=[0,0,0.33]
  #group.set_position_target(pos_target)
  #pos_target=lleg_group_single.get_random_pose(end_effector_link="pelvis")
  #print "Pos Target: %s" %pos_target
  #lleg_group_single.set_planner_id("RRTConnectkConfigDefault")
  pos_target = lleg_group_single.get_random_pose()
  #pos_target = lleg_group_single.get_current_pose()
  print "Random Pose: %s" % pos_target
  #print "TEST %s" %pos_target.pose.position.x
  #print "TEST %s" %pos_target.pose.position.y
  #print "TEST %s" %pos_target.pose.position.z
  #print "TEST %s" %pos_target.pose.orientation.x
  #print "TEST %s" %pos_target.pose.orientation.y
  #print "TEST %s" %pos_target.pose.orientation.z
  #print "TEST %s" %pos_target.pose.orientation.w
  print "Goal Joint Tolerance %s" 	% lleg_group_single.get_goal_joint_tolerance()
  print "Goal Orientation Tolerance %s" % lleg_group_single.get_goal_orientation_tolerance()
  print "Goal Position Tolerance %s" 	% lleg_group_single.get_goal_position_tolerance()
  #lleg_group_single.set_goal_position_tolerance(0.1)
  #lleg_group_single.set_goal_joint_tolerance(0.1)
  #lleg_group_single.set_goal_orientation_tolerance(0.1)
  #print "Goal Joint Tolerance %s" 	% lleg_group_single.get_goal_joint_tolerance()
  #print "Goal Orientation Tolerance %s" % lleg_group_single.get_goal_orientation_tolerance()
  #print "Goal Position Tolerance %s" 	% lleg_group_single.get_goal_position_tolerance()
  #x = pos_target.pose.position.x
  #y = pos_target.pose.position.y
  #z = pos_target.pose.position.z
  #working_pose = [0.183792065459, 0.0754540293222, 0.3066920494, -0.085881893673, 0.0963357114615, 0.492868689835, 0.860479044263]
  #lleg_group_single.set_position_target([0,0,0.33],end_effector_link = "pelvis")
  #lleg_group_single.set_pose_target(lleg_group_single.get_current_pose(),end_effector_link = "pelvis")
  lleg_group_single.set_pose_target(pos_target,end_effector_link = "pelvis")
  #lleg_group_single.set_pose_target(working_pose,end_effector_link = "pelvis")


  #lleg_group_single.set_position_target(current_pose)

  ## Now, we call the planner to compute the plan
  ## and visualize it if successful
  ## Note that we are just planning, not asking move_group 
  ## to actually move the robot
  plan1 = lleg_group_single.plan()

  #print "============ Waiting while RVIZ displays plan1..."


  ## You can ask RVIZ to visualize a plan (aka trajectory) for you.  But the
  ## group.plan() method does this automatically so this is not that useful
  ## here (it just displays the same trajectory again).
  #print "============ Visualizing plan1"
  #display_trajectory = moveit_msgs.msg.DisplayTrajectory()

  #display_trajectory.trajectory_start = robot.get_current_state()
  #display_trajectory.trajectory.append(plan1)
  #display_trajectory_publisher.publish(display_trajectory);

  #print "============ Waiting while plan1 is visualized (again)..."
  #rospy.sleep(5)


  print "Moving real robot now"
  ### Moving to a pose goal
  ### ^^^^^^^^^^^^^^^^^^^^^
  ###
  ### Moving to a pose goal is similar to the step above
  ### except we now use the go() function. Note that
  ### the pose goal we had set earlier is still active 
  ### and so the robot will try to move to that goal. We will
  ### not use that function in this tutorial since it is 
  ### a blocking function and requires a controller to be active
  ### and report success on execution of a trajectory.

  ## Uncomment below line when working with a real robot
  lleg_group_single.go(wait=True)
  print "============ DONE"

  ### Planning to a joint-space goal 
  ### ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  ###
  ### Let's set a joint space goal and move towards it. 
  ### First, we will clear the pose target we had just set.

  #group.clear_pose_targets()

  ### Then, we will get the current set of joint values for the group
  #group_variable_values = group.get_current_joint_values()
  #print "============ Joint values: ", group_variable_values

  ### Now, let's modify one of the joints, plan to the new joint
  ### space goal and visualize the plan
  #group_variable_values[0] = 1.0
  #group.set_joint_value_target(group_variable_values)

  #plan2 = group.plan()

  #print "============ Waiting while RVIZ displays plan2..."
  #rospy.sleep(5)


  ### Cartesian Paths
  ### ^^^^^^^^^^^^^^^
  ### You can plan a cartesian path directly by specifying a list of waypoints 
  ### for the end-effector to go through.
  #waypoints = []

  ## start with the current pose
  #waypoints.append(group.get_current_pose().pose)

  ## first orient gripper and move forward (+x)
  #wpose = geometry_msgs.msg.Pose()
  #wpose.orientation.w = 1.0
  #wpose.position.x = waypoints[0].position.x + 0.1
  #wpose.position.y = waypoints[0].position.y
  #wpose.position.z = waypoints[0].position.z
  #waypoints.append(copy.deepcopy(wpose))

  ## second move down
  #wpose.position.z -= 0.10
  #waypoints.append(copy.deepcopy(wpose))

  ## third move to the side
  #wpose.position.y += 0.05
  #waypoints.append(copy.deepcopy(wpose))

  ### We want the cartesian path to be interpolated at a resolution of 1 cm
  ### which is why we will specify 0.01 as the eef_step in cartesian
  ### translation.  We will specify the jump threshold as 0.0, effectively
  ### disabling it.
  #(plan3, fraction) = group.compute_cartesian_path(
                               #waypoints,   # waypoints to follow
                               #0.01,        # eef_step
                               #0.0)         # jump_threshold

  #print "============ Waiting while RVIZ displays plan3..."
  #rospy.sleep(5)


  ### 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()
  rospy.spin()

  ### END_TUTORIAL

  print "============ STOPPING"
Пример #45
0
    def __init__(self, robot_name="panda_arm", frame="panda_link0"):
        try:
            moveit_commander.roscpp_initialize(sys.argv)
            rospy.init_node(name="pick_place_test")
            self.scene = PlanningSceneInterface()
            self.scene_pub = rospy.Publisher('planning_scene',
                                             PlanningScene,
                                             queue_size=10)
            # region Robot initial
            self.robot = MoveGroupCommander(robot_name)
            self.robot.set_goal_joint_tolerance(0.00001)
            self.robot.set_goal_position_tolerance(0.00001)
            self.robot.set_goal_orientation_tolerance(0.01)
            self.robot.set_goal_tolerance(0.00001)
            self.robot.allow_replanning(True)
            self.robot.set_pose_reference_frame(frame)
            self.robot.set_planning_time(3)
            # endregion
            self.gripper = MoveGroupCommander("hand")
            self.gripper.set_joint_value_target(GRIPPER_CLOSED)
            self.gripper.go()
            self.gripper.set_joint_value_target(GRIPPER_OPEN)
            self.gripper.go()
            self.gripper.set_joint_value_target(GRIPPER_CLOSED)
            self.gripper.go()

            # Robot go home
            self.robot.set_named_target("home")
            self.robot.go()
            # clear all object in world
            self.clear_all_object()

            table_pose = un.Pose(0, 0, -10, 0, 0, 0)
            table_color = un.Color(255, 255, 0, 100)
            self.add_object_box("table", table_pose, table_color, frame,
                                (2000, 2000, 10))

            bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
            bearing_color = un.Color(255, 0, 255, 255)
            bearing_file_name = "../stl/bearing.stl"
            self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
                                 bearing_file_name)
            obpose = self.scene.get_object_poses(["bearing"])
            # self.robot.set_support_surface_name("table")
            g = Grasp()
            # Create gripper position open or close
            g.pre_grasp_posture = self.open_gripper()
            g.grasp_posture = self.close_gripper()
            g.pre_grasp_approach = self.make_gripper_translation(
                0.01, 0.1, [0, 1.0, 0])
            g.post_grasp_retreat = self.make_gripper_translation(
                0.01, 0.9, [0, 1.0, 0])
            p = PoseStamped()
            p.header.frame_id = "panda_link0"
            p.pose.orientation = obpose["bearing"].orientation
            p.pose.position = obpose["bearing"].position
            g.grasp_pose = p
            g.allowed_touch_objects = ["bearing"]
            a = []
            a.append(g)
            result = self.robot.pick(object_name="bearing", grasp=a)
            print(result)

        except Exception as ex:
            print(ex)
            moveit_commander.roscpp_shutdown()

        moveit_commander.roscpp_shutdown()
Пример #46
0
    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
        target_pose.pose.orientation.w = 1

        # Set the start state and target pose, then plan and execute
        arm.set_start_state_to_current_state()
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        # Clear all path constraints
        arm.clear_path_constraints()

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

        # Return to 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)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
Пример #47
0
def move_group_python_interface_tutorial():
  ## BEGIN_TUTORIAL
  ##
  ## Setup
  ## ^^^^^
  ## CALL_SUB_TUTORIAL imports
  ##
  ## 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("left_arm")
  

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

  ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
  print "============ Waiting for RVIZ..."
  rospy.sleep(10)
  print "============ Starting tutorial "

  ## 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 "============"


  ## Planning to a Pose goal
  ## ^^^^^^^^^^^^^^^^^^^^^^^
  ## We can plan a motion for this group to a desired pose for the
  ## end-effector
  print "============ Generating plan 1"
  pose_target = geometry_msgs.msg.Pose()
  pose_target.orientation.w = 1.0
  pose_target.position.x = 0.7
  pose_target.position.y = -0.05
  pose_target.position.z = 1.1
  group.set_pose_target(pose_target)

  ## Now, we call the planner to compute the plan
  ## and visualize it if successful
  ## Note that we are just planning, not asking move_group
  ## to actually move the robot
  plan1 = group.plan()

  print "============ Waiting while RVIZ displays plan1..."
  rospy.sleep(5)

 
  ## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
  ## group.plan() method does this automatically so this is not that useful
  ## here (it just displays the same trajectory again).
  print "============ Visualizing plan1"
  display_trajectory = moveit_msgs.msg.DisplayTrajectory()

  display_trajectory.trajectory_start = robot.get_current_state()
  display_trajectory.trajectory.append(plan1)
  display_trajectory_publisher.publish(display_trajectory);

  print "============ Waiting while plan1 is visualized (again)..."
  rospy.sleep(5)


  ## Moving to a pose goal
  ## ^^^^^^^^^^^^^^^^^^^^^
  ##
  ## Moving to a pose goal is similar to the step above
  ## except we now use the go() function. Note that
  ## the pose goal we had set earlier is still active
  ## and so the robot will try to move to that goal. We will
  ## not use that function in this tutorial since it is
  ## a blocking function and requires a controller to be active
  ## and report success on execution of a trajectory.

  # Uncomment below line when working with a real robot
  # group.go(wait=True)

  ## Planning to a joint-space goal
  ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  ##
  ## Let's set a joint space goal and move towards it.
  ## First, we will clear the pose target we had just set.

  group.clear_pose_targets()

  ## Then, we will get the current set of joint values for the group
  group_variable_values = group.get_current_joint_values()
  print "============ Joint values: ", group_variable_values

  ## Now, let's modify one of the joints, plan to the new joint
  ## space goal and visualize the plan
  group_variable_values[0] = 1.0
  group.set_joint_value_target(group_variable_values)

  plan2 = group.plan()

  print "============ Waiting while RVIZ displays plan2..."
  rospy.sleep(5)


  ## Cartesian Paths
  ## ^^^^^^^^^^^^^^^
  ## You can plan a cartesian path directly by specifying a list of waypoints
  ## for the end-effector to go through.
  waypoints = []

  # start with the current pose
  waypoints.append(group.get_current_pose().pose)

  # first orient gripper and move forward (+x)
  wpose = geometry_msgs.msg.Pose()
  wpose.orientation.w = 1.0
  wpose.position.x = waypoints[0].position.x + 0.1
  wpose.position.y = waypoints[0].position.y
  wpose.position.z = waypoints[0].position.z
  waypoints.append(copy.deepcopy(wpose))

  # second move down
  wpose.position.z -= 0.10
  waypoints.append(copy.deepcopy(wpose))

  # third move to the side
  wpose.position.y += 0.05
  waypoints.append(copy.deepcopy(wpose))

  ## We want the cartesian path to be interpolated at a resolution of 1 cm
  ## which is why we will specify 0.01 as the eef_step in cartesian
  ## translation. We will specify the jump threshold as 0.0, effectively
  ## disabling it.
  (plan3, fraction) = group.compute_cartesian_path(
                               waypoints, # waypoints to follow
                               0.01, # eef_step
                               0.0) # jump_threshold
                               
  print "============ Waiting while RVIZ displays plan3..."
  rospy.sleep(5)

 
  ## 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()

  ## END_TUTORIAL

  print "============ STOPPING"
Пример #48
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")]
        self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")]
        self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")]

        self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")

        # We need a tf listener to convert poses into arm reference base
        self.tf_listener = tf.TransformListener()

        # 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,
                                         queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('target_pose',
                                                PoseStamped,
                                                queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

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

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

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

        # 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 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 1

        # Set a limit on the number of place attempts
        max_place_attempts = 3
        rospy.loginfo("Scaling for MoveIt timeout=" + str(
            rospy.get_param(
                '/move_group/trajectory_execution/allowed_execution_duration_scaling'
            )))

        # 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 "arm_up" pose stored in the SRDF file
        rospy.loginfo("Set Arm: right_up")
        arm.set_named_target('right_up')
        if arm.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the closed position
        rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed))
        gripper.set_joint_value_target(self.gripper_closed)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the neutral position
        rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral))
        gripper.set_joint_value_target(self.gripper_neutral)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

        # Move the gripper to the open position
        rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
        gripper.set_joint_value_target(self.gripper_opened)
        if gripper.go() != True:
            rospy.logwarn("  Go failed")
        rospy.sleep(2)

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

        # 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.018, 0.018, 0.018]

        # 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.36
        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 = table_pose.pose.position.x - 0.04
        #box1_pose.pose.position.y = 0.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 = table_pose.pose.position.x - 0.06
        #box2_pose.pose.position.y = 0.2
        #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)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = table_pose.pose.position.x - 0.03
        target_pose.pose.position.y = -0.1
        target_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        target_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_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)

        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

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

        # Set the support surface name to the table object
        arm.set_support_surface_name(table_id)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = table_pose.pose.position.x - 0.03
        place_pose.pose.position.y = -0.15
        place_pose.pose.position.z = table_ground + table_size[
            2] + target_size[2] / 2.0
        place_pose.pose.orientation.w = 1.0

        # Initialize the grasp pose to the target pose
        grasp_pose = target_pose

        # Shift the grasp pose by half the width of the target to center it
        grasp_pose.pose.position.y -= target_size[1] / 2.0

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id],
                                  [target_size[1] - self.gripper_tighten])

        # Track success/failure and number of attempts for pick operation
        result = MoveItErrorCodes.FAILURE
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            rospy.loginfo("Pick attempt #" + str(n_attempts))
            for grasp in grasps:
                # Publish the grasp poses so they can be viewed in RViz
                self.gripper_pose_pub.publish(grasp.grasp_pose)
                rospy.sleep(0.2)

                result = arm.pick(target_id, grasps)

                #result = MoveItErrorCodes.SUCCESS
                #if result == MoveItErrorCodes.SUCCESS:
                break

            n_attempts += 1
            rospy.sleep(0.2)

        print 'second'
        result = MoveItErrorCodes.SUCCESS
        # If the pick was successful, attempt the place operation
        if result == MoveItErrorCodes.SUCCESS:
            rospy.loginfo("  Pick: Done!")
            # Generate valid place poses
            places = self.make_places(place_pose)

            success = False
            n_attempts = 0

            # Repeat until we succeed or run out of attempts
            while not success and n_attempts < max_place_attempts:
                rospy.loginfo("Place attempt #" + str(n_attempts))
                for place in places:
                    # Publish the place poses so they can be viewed in RViz
                    self.gripper_pose_pub.publish(place)
                    rospy.sleep(0.2)

                    success = arm.place(target_id, place)
                    #if success:
                    break

                n_attempts += 1
                rospy.sleep(0.2)

            if not success:
                rospy.logerr("Place operation failed after " +
                             str(n_attempts) + " attempts.")
            else:
                rospy.loginfo("  Place: Done!")
        else:
            rospy.logerr("Pick operation failed after " + str(n_attempts) +
                         " attempts.")

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

        arm.set_named_target('resting')
        arm.go()

        # Open the gripper to the neutral position
        gripper.set_joint_value_target(self.gripper_neutral)
        gripper.go()

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Пример #49
0
    def __init__(self):

        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node("moveit_demo")

        # Set three basic gripper openings
        GRIPPER_OPEN = [0.075]
        GRIPPER_CLOSED = [-0.075]
        GRIPPER_NEUTRAL = [0.04]

        # Define Positions
        XVALUE = input("Defina la primer posiciOn para x, \n")
        YVALUE = input("Defina la primer posiciOn para Y, \n")
        ZVALUE = input("Defina la primer posiciOn para Z, \n")

        # Initialize the move group for the right arm
        right_arm = moveit_commander.MoveGroupCommander("right_arm")
        right_gripper = moveit_commander.MoveGroupCommander("right_gripper")

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

        # Set the reference frame for pose targets
        reference_frame = "camera_link"

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

        # 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.01)
        right_arm.set_goal_orientation_tolerance(0.05)

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target("resting")
        right_arm.go()
        rospy.sleep(0.5)

        # Set the gripper to a neural position using a joint value target
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        # Plan and execute a trajectory to the goal configuration
        right_gripper.go()
        rospy.sleep(1)

        # Set the target pose.  This particular pose has the gripper oriented horizontally
        # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
        # the center of the robot base.
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = XVALUE
        target_pose.pose.position.y = YVALUE
        target_pose.pose.position.z = ZVALUE
        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
        right_arm.set_start_state_to_current_state()

        # Set the goal pose of the end effector to the stored pose
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Plan the trajectory to the goal
        traj = right_arm.plan()

        # Execute the planned trajectory
        right_arm.execute(traj)

        # Pause for a second
        rospy.sleep(1)

        # Set the gripper target to a partially closed position
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        # Plan and execute a trajectory to the goal configuration
        right_gripper.go()
        rospy.sleep(1)

        DIRECTION = raw_input("Mover a la izquierda(I) o derecha(D)?")
        DISTANCE = input("Defina, en metros, cuanto se movera el objeto \n")

        if DIRECTION == "D":
            DISTANCE = DISTANCE * (-1)

            # Shift the end-effector to the right 5cm
            # right_arm.shift_pose_target(1, -0.05, end_effector_link)
            # right_arm.go()
            # rospy.sleep(1)

            # Rotate the end-effector 90 degrees
            # right_arm.shift_pose_target(3, 1.57, end_effector_link)
            # right_arm.go()
            # rospy.sleep(0.5)

            # Store this pose as the new target_pose
        saved_target_pose = right_arm.get_current_pose(end_effector_link)

        # Move to the named pose "wave"
        # right_arm.set_named_target('wave')
        # right_arm.go()
        # rospy.sleep(1)

        # Go back to the stored target
        # right_arm.set_pose_target(saved_target_pose, end_effector_link)
        # right_arm.go()
        # rospy.sleep(1)

        # Set the target pose.  This particular pose has the gripper oriented horizontally
        # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
        # the center of the robot base.
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = XVALUE
        target_pose.pose.position.y = YVALUE + DISTANCE
        target_pose.pose.position.z = ZVALUE
        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
        right_arm.set_start_state_to_current_state()

        # Set the goal pose of the end effector to the stored pose
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Plan the trajectory to the goal
        traj = right_arm.plan()

        # Execute the planned trajectory
        right_arm.execute(traj)

        # Pause for a second
        rospy.sleep(1)

        # Set the gripper target to a partially open position
        right_gripper.set_joint_value_target(GRIPPER_OPEN)
        # Plan and execute a trajectory to the goal configuration
        right_gripper.go()
        rospy.sleep(1)

        # Finish up in the resting position
        right_arm.set_named_target("resting")
        right_arm.go()

        # Return the gripper target to neutral position
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
def week4_assignment3():
    ## First initialize moveit_commander and rospy.
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('week4_assignment3', anonymous=True)

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

    ## Action clients to the ExecuteTrajectory action server.
    robot2_client = actionlib.SimpleActionClient(
        'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
    robot2_client.wait_for_server()
    rospy.loginfo('Execute Trajectory server is available for robot2')

    ## Move robot2 to R2Home robot pose with the set_named_target API.
    robot2_group.set_named_target("R2Home")

    ## Plan to the desired joint-space goal using the default planner (RRTConnect).
    plan = robot2_group.plan()
    ## Create a goal message object for the action server.
    robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
    ## Update the trajectory in the goal message.
    robot2_goal.trajectory = plan

    ## Print message
    rospy.loginfo('Go to Home')

    ## Send the goal to the action server.
    robot2_client.send_goal(robot2_goal)
    robot2_client.wait_for_result()

    ## Move robot2 to R2PreGrasp pose
    robot2_group.set_named_target("R2PreGrasp")

    plan = robot2_group.plan()
    robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
    robot2_goal.trajectory = plan

    ## Print message
    rospy.loginfo('Go to Pre Grasp')

    robot2_client.send_goal(robot2_goal)
    robot2_client.wait_for_result()

    # Pick motions with the compute_cartesian_path API.
    waypoints = []
    # start with the current pose
    current_robot2_pose = robot2_group.get_current_pose()
    rospy.sleep(0.5)
    current_robot2_pose = robot2_group.get_current_pose()

    ## create linear offsets to the current pose
    new_robot2_eef_pose = geometry_msgs.msg.Pose()

    # Manual offsets because we don't have a camera to detect objects yet.
    # Create an x-offset of -5cm to the current x position.
    new_robot2_eef_pose.position.x = current_robot2_pose.pose.position.x + 0.5
    # Create a y-offset of +10cm to the current y position.
    new_robot2_eef_pose.position.y = current_robot2_pose.pose.position.y + 0.5
    # Create a z-offset of -10cm to the current z position.
    new_robot2_eef_pose.position.z = current_robot2_pose.pose.position.z - 0.5

    # Retain orientation of the current pose.
    new_robot2_eef_pose.orientation = copy.deepcopy(
        current_robot2_pose.pose.orientation)

    # Update the list of waypoints.
    # First add the new desired pose of the end effector for robot2.
    waypoints.append(new_robot2_eef_pose)

    rospy.loginfo('Cartesian path - Waypoint 1:')
    print(new_robot2_eef_pose.position)

    # Then go back to the pose where we started the linear motion from.
    waypoints.append(current_robot2_pose.pose)

    rospy.loginfo('Cartesian path - Waypoint 2:')
    print(current_robot2_pose.pose.position)

    ## We want the cartesian path to be interpolated at a resolution of 1 cm
    ## which is why we will specify 0.01 as the eef_step in cartesian
    ## translation.  We will specify the jump threshold as 0.0, effectively
    ## disabling it.
    fraction = 0.0
    for count_cartesian_path in range(0, 3):
        if fraction < 1.0:
            (plan_cartesian, fraction) = robot2_group.compute_cartesian_path(
                waypoints,  # waypoints to follow
                0.01,  # eef_step
                0.0)  # jump_threshold
        else:
            break

    robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
    robot2_goal.trajectory = plan_cartesian
    ## Print message
    rospy.loginfo('Cartesian movement')

    robot2_client.send_goal(robot2_goal)
    robot2_client.wait_for_result()
    rospy.sleep(5)

    ## After executing the linear motions, go to the R2Place robot pose using the set_named_target API.
    robot2_group.set_named_target("R2Place")

    plan = robot2_group.plan()
    robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
    robot2_goal.trajectory = plan
    rospy.loginfo('Go to Place')
    robot2_client.send_goal(robot2_goal)
    robot2_client.wait_for_result()

    ## When finished shut down moveit_commander.
    moveit_commander.roscpp_shutdown()
def end():
    rospy.signal_shutdown("Done")
    os._exit(0)
    roscpp_shutdown()
Пример #52
0
 def close(self):
     """ Closes the environment and shuts down the simulation. """
     print("closing GazeboSmartBotPincherKinectEnv")
     # When finished, shut down moveit_commander.
     moveit_commander.roscpp_shutdown()
     super(GazeboSmartBotPincherKinectEnv, self).close()
    def __init__(self):
        # rospy.sleep(10) # wait for other files to be launched successfully

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

        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)

        N = 10 # amount of tracking to average

        combine_obj_pos = np.empty((N,9))
        combine_euler = np.empty((N,9))

        # object tracking
        for cN in range(0,N):
            [all_object_name,all_object_pose] = self.get_object_position()

            # loop through each object and store their respective position and orientation
            for m in range(0,3):
                combine_obj_pos[cN,3*m:3*m+3] = (all_object_pose[m].transform.translation.x,all_object_pose[m].transform.translation.y,all_object_pose[m].transform.translation.z)
                obj_quat = (all_object_pose[m].transform.rotation.x,all_object_pose[m].transform.rotation.y,all_object_pose[m].transform.rotation.z,all_object_pose[m].transform.rotation.w)
                combine_euler[cN,3*m:3*m+3] = euler_from_quaternion(obj_quat)

        print combine_obj_pos

        average_obj_pos = np.mean(combine_obj_pos,axis=0)
        average_euler = np.mean(combine_euler,axis=0)

        # update the object pose with the new averaged value
        for p in range(0,3):
            all_object_pose[p].transform.translation.x = average_obj_pos[3*p]
            all_object_pose[p].transform.translation.y = average_obj_pos[3*p+1]
            all_object_pose[p].transform.translation.z = average_obj_pos[3*p+2]

            new_quat = quaternion_from_euler(average_euler[3*p], average_euler[3*p+1], average_euler[3*p+2])
            all_object_pose[p].transform.rotation.x = new_quat[0]
            all_object_pose[p].transform.rotation.y = new_quat[1]
            all_object_pose[p].transform.rotation.z = new_quat[2]
            all_object_pose[p].transform.rotation.w = new_quat[3]

        print all_object_pose

        # print average_obj_pos
        # 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")
Пример #54
0
def measure_table():
    # This function measure the size and the position of the table. Add the value for offset_zero_point.
    offset_zero_point = 0.903
    # Define positions.
    pos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424,
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }
    lpos1 = {
        'left_e0': -1.69483279891317,
        'left_e1': 1.8669726956453,
        'left_s0': 0.472137005716569,
        'left_s1': -0.38852045702393034,
        'left_w0': -1.9770933862776057,
        'left_w1': -1.5701993084642143,
        'left_w2': -0.6339059781326424
    }
    rpos1 = {
        'right_e0': 1.7238109084167481,
        'right_e1': 1.7169079948791506,
        'right_s0': 0.36930587426147465,
        'right_s1': -0.33249033539428713,
        'right_w0': -1.2160632682067871,
        'right_w1': 1.668587600115967,
        'right_w2': -1.810097327636719
    }

    m_z_start = geometry_msgs.msg.Pose()
    m_z_start.position.x = 0.55
    m_z_start.position.y = 0
    m_z_start.position.z = 0.0
    m_z_start.orientation.x = 1.0
    m_z_start.orientation.y = 0.0
    m_z_start.orientation.z = 0.0
    m_z_start.orientation.w = 0.0

    m_x_start = geometry_msgs.msg.Pose()
    m_x_start.position.x = 0.824555206312
    m_x_start.position.y = -0.101147200174
    m_x_start.position.z = 0
    m_x_start.orientation.x = 0.999078899126
    m_x_start.orientation.y = -0.0370175672297
    m_x_start.orientation.z = 0.0134534998151
    m_x_start.orientation.w = 0.0170310416472

    # For the right arm.
    m_y_start_1 = geometry_msgs.msg.Pose()
    m_y_start_1.position.x = 0.502944492492
    m_y_start_1.position.y = -0.759184953936
    m_y_start_1.position.z = 0
    m_y_start_1.orientation.x = 0.695094141364
    m_y_start_1.orientation.y = -0.718718693339
    m_y_start_1.orientation.z = 0.00984382718873
    m_y_start_1.orientation.w = 0.0138084595126
    # For the left arm.
    m_y_start_2 = geometry_msgs.msg.Pose()
    m_y_start_2.position.x = 0.569637271212
    m_y_start_2.position.y = 0.86081004269
    m_y_start_2.position.z = 0
    m_y_start_2.orientation.x = -0.752397350016
    m_y_start_2.orientation.y = -0.657989479311
    m_y_start_2.orientation.z = -0.0240898615418
    m_y_start_2.orientation.w = 0.0191768447787

    # The start values for the attached table in MoveIt.
    table_size_x = 1.5
    table_size_y = 2
    table_size_z = 0.8
    center_x = 0
    center_y = 0
    center_z = 0

    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)
    # cProfile to measure the performance (time) of the task.
    pr = cProfile.Profile()
    pr.enable()
    # Start to measure the height of the table.
    state = move("right", m_z_start)
    right_ir_sensor = rospy.wait_for_message(
        "/robot/range/right_hand_range/state", Range)
    # At first move with 10 cm steps and if the range is smaller then 25 cm with 1 cm steps.
    while (right_ir_sensor.range > 0.25):
        if not state:
            m_z_start.position.z -= 0.1
        state = move("right", m_z_start)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
        print "-----> The distance to table:", right_ir_sensor.range, "m"
    while (right_ir_sensor.range > 0.12):
        if not state:
            m_z_start.position.z -= 0.01
        state = move("right", m_z_start)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
        print "-----> The distance to table:", right_ir_sensor.range, "m"
    time.sleep(3)
    right_ir_sensor = rospy.wait_for_message(
        "/robot/range/right_hand_range/state", Range)
    distance = right_ir_sensor.range
    print "-----> The distance to table:", distance, "m"
    a = right_arm.get_current_pose()
    z_pos = a.pose.position.z
    # From the ground to the zero point in MoveIt with the base frame are 0.903 m.
    # The z position of the tip of the gripper and the distance between
    # the tip and the table must be substract.
    # (zpos is negative for table < 90 cm)
    # The value 0.099 is the distance from the ir sensor to the gripper tip.
    table_size_z = offset_zero_point + z_pos - (distance - 0.099)
    print "-----> The table size in z direction is:", table_size_z, " m"
    center_z = -offset_zero_point + table_size_z / 2
    print "-----> The center of z is the position:", center_z, "m"
    # Initialize the position z for the next measurements.
    m_y_start_1.position.z = -offset_zero_point + table_size_z + 0.025
    m_y_start_2.position.z = -offset_zero_point + table_size_z + 0.025
    m_x_start.position.z = -offset_zero_point + table_size_z + 0.025
    # Add table as attached object.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move to the start position.
    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)
    # Move to the right and left border point.
    right_arm.set_pose_target(m_y_start_1)
    right_arm.plan()
    right_arm.go(wait=True)
    left_arm.set_pose_target(m_y_start_2)
    left_arm.plan()
    left_arm.go(wait=True)
    # Start to measure the size and position in y direction of the table. At first y1 endpoint.
    right_ir_sensor = rospy.wait_for_message(
        "/robot/range/right_hand_range/state", Range)
    distance = right_ir_sensor.range
    # If the table is at the border point under the gripper the default value -0.8 is used.
    if distance < 0.15:
        y1_endpoint = -0.8
    else:
        y1_endpoint = 0
    while (right_ir_sensor.range > 0.2 and m_y_start_1.position.y < 0.2):
        m_y_start_1.position.y += 0.1
        move("right", m_y_start_1)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
    while right_ir_sensor.range < 0.2 and y1_endpoint == 0:
        m_y_start_1.position.y -= 0.01
        move("right", m_y_start_1)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
    time.sleep(3)
    if y1_endpoint == 0:
        a = right_arm.get_current_pose()
        y_pos = a.pose.position.y
        # The ir sensor is not in the middle of the hand and a small safety distance is necessary.
        y1_endpoint = y_pos - 0.04
    print "-----> y1 endpoint:", y1_endpoint, "m"
    # Start to measure the second y2 endpoint.
    left_ir_sensor = rospy.wait_for_message(
        "/robot/range/left_hand_range/state", Range)
    distance = left_ir_sensor.range
    print "-----> The distance to table:", distance, "m"
    # If the table is at the border point under the gripper the default value 1.2 is used.
    if distance < 0.15:
        y2_endpoint = 1.2
    else:
        y2_endpoint = 0
    while (left_ir_sensor.range > 0.2 and m_y_start_2.position.y > 0.2):
        m_y_start_2.position.y -= 0.1
        move("left", m_y_start_2)
        left_ir_sensor = rospy.wait_for_message(
            "/robot/range/left_hand_range/state", Range)
    while left_ir_sensor.range < 0.2 and y2_endpoint == 0:
        m_y_start_2.position.y += 0.01
        move("left", m_y_start_2)
        left_ir_sensor = rospy.wait_for_message(
            "/robot/range/left_hand_range/state", Range)
    time.sleep(3)
    if y2_endpoint == 0:
        a = left_arm.get_current_pose()
        y_pos = a.pose.position.y
        # The ir sensor is not in the middle of the hand and a small safety distance is necessary.
        y2_endpoint = y_pos + 0.04
        print "-----> y2 endpoint:", y2_endpoint, "m"
    # The y1 endpoint must be negative and the y2 endpoint positive.
    table_size_y = abs(y1_endpoint) + abs(y2_endpoint)
    print "-----> The table size in y direction is:", table_size_y, "m"
    center_y = y1_endpoint + table_size_y / 2 - 0.04
    print "-----> The center of y is the position:", center_y, "m"
    # Remove the old table and add one with the new value.
    p.removeAttachedObject('table')
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move to the start position.
    both_arms.set_joint_value_target(pos1)
    both_arms.plan()
    both_arms.go(wait=True)
    # Move to the border point in the x direction.
    right_arm.set_pose_target(m_x_start)
    right_arm.plan()
    right_arm.go(wait=True)

    right_ir_sensor = rospy.wait_for_message(
        "/robot/range/right_hand_range/state", Range)
    distance = right_ir_sensor.range
    # The x endpoint in the direction of the robot is allway 0.1.
    x1_endpoint = 0.1
    # If the table is at the border point under the gripper the default value 1.1 is used.
    if distance < 0.15:
        x2_endpoint = 1.1
    else:
        x2_endpoint = 0
    while (right_ir_sensor.range > 0.2 and m_x_start.position.x > 0.2):
        m_x_start.position.x -= 0.1
        move("right", m_x_start)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
    while right_ir_sensor.range < 0.2 and x2_endpoint == 0:
        m_x_start.position.x += 0.01
        move("right", m_x_start)
        right_ir_sensor = rospy.wait_for_message(
            "/robot/range/right_hand_range/state", Range)
    time.sleep(3)
    if x2_endpoint == 0:
        a = right_arm.get_current_pose()
        x_pos = a.pose.position.x
        # The ir sensor is not in the middle of the hand and a small safety distance is necessary.
        x2_endpoint = x_pos + 0.04
        print "-----> x2 endpoint:", x2_endpoint, "m"
    table_size_x = abs(x2_endpoint) - abs(x1_endpoint)
    print "-----> The table size in x direction is:", table_size_x, "m"
    center_x = x1_endpoint + table_size_x / 2
    print "-----> The center of x is the position:", center_x, "m"
    # Remove the old table and add one with the new value.
    p.removeAttachedObject('table')
    # Add table as attached object. It is most possible to approach up to approximately 0.5 cm.
    # (Point on the table: -0.175)
    # Size of the box in x-, y- and z-dimension.
    # The x, y, z positions in link_name frame ('base') where the center of the box is.
    # The touch_links are robot links that can touch this object.
    # The value can be determined with right_arm.get_current_pose().
    # The pose value of z must be in every space direct of the table the
    # same height otherwise the pedestal must be configured.
    # The zero point from MoveIt (with right_arm.get_current_pose())
    # and the function from the baxter_interface
    # (rightarm.endpoint_pose()) is different.
    # The distance from the zero point from the link_name frame 'base' to
    # the ground is for MoveIt 0.903 m.
    p.attachBox('table',
                table_size_x,
                table_size_y,
                table_size_z,
                center_x,
                center_y,
                center_z,
                'base',
                touch_links=['pedestal'])
    p.waitForSync()
    # Move to the start position.
    right_arm.set_joint_value_target(rpos1)
    right_arm.plan()
    right_arm.go(wait=True)

    pr.disable()
    sortby = 'cumulative'
    ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
    print "-----> The table size in x direction is:", table_size_x, "m"
    print "-----> The table size in y direction is:", table_size_y, "m"
    print "-----> The table size in z direction is:", table_size_z, "m"
    print "-----> The center of x is the position:", center_x, "m"
    print "-----> The center of y is the position:", center_y, "m"
    print "-----> The center of z is the position:", center_z, "m"
    time.sleep(100)
    moveit_commander.roscpp_shutdown()
    # Exit MoveIt.
    moveit_commander.os._exit(0)
Пример #55
0
		image_num += 1
		pass
	    pass
	print "Scanning ended!"
	pass
    def RotateLeftArm(self):
	pass

def parse_args():	
    parser = argparse.ArgumentParser()
    parser.add_argument('-n', '--name', dest='name', type=str, help='Object name', required = True)
    args = parser.parse_args()
    return args

if __name__ == '__main__':

    args = parse_args()
    
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('object_scanning_node', anonymous=True)
    image_sub = image_subscriber(args.name)
    mover = robot_mover(image_sub)
    mover.gotoStartPoses()   
    mover.Scan()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
    moveit_commander.roscpp_shutdown()
    print "scanning done"
    pass
Пример #56
0
def callback(data):
    if (data.ctrl == 6):
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Пример #57
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 = moveit_commander.MoveGroupCommander('right_arm')
                
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                        
        # Set the reference frame for pose targets
        reference_frame = 'base_link'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
                
        # 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.01)
        right_arm.set_goal_orientation_tolerance(0.05)
        right_arm.set_planner_id("ESTkConfigDefault");
        
        # 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 target pose.  This particular pose has the gripper oriented horizontally
        # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of 
        # the center of the robot base.
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.50
        target_pose.pose.position.y = -0.1
        target_pose.pose.position.z = 0.85
        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
        right_arm.set_start_state_to_current_state()
        
        # Set the goal pose of the end effector to the stored pose
        right_arm.set_pose_target(target_pose, end_effector_link)
        
        # Plan the trajectory to the goal
        traj = right_arm.plan()
        
        # Execute the planned trajectory
        right_arm.execute(traj)
    
        # Pause for a second
        rospy.sleep(1)
         
        # Shift the end-effector to the right 5cm
        right_arm.shift_pose_target(1, -0.05, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
  
        # Rotate the end-effector 90 degrees
        right_arm.shift_pose_target(3, -1.57, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Store this pose as the new target_pose
        saved_target_pose = right_arm.get_current_pose(end_effector_link)
          
          
        # Go back to the stored target
        right_arm.set_pose_target(saved_target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
           
        # Finish up in the resting position  
        right_arm.set_named_target('right_start')
        right_arm.go()

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)
Пример #58
0
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_drawing', anonymous=True)

        #订阅stop话题
        rospy.Subscriber("/probot_controller_ctrl", ControllerCtrl, callback)

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = MoveGroupCommander('manipulator')

        # 当运动规划失败后,允许重新规划
        arm.allow_replanning(True)

        # 设置目标位置所使用的参考坐标系
        reference_frame = 'base_link'
        arm.set_pose_reference_frame('base_link')

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.01)

        # 设置允许的最大速度和加速度
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)

        # 获取终端link的名称
        end_effector_link = arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        star_pose = PoseStamped()
        star_pose.header.frame_id = reference_frame
        star_pose.header.stamp = rospy.Time.now()
        star_pose.pose.position.x = 0.30
        star_pose.pose.position.y = 0.0
        star_pose.pose.position.z = 0.30
        star_pose.pose.orientation.w = 1.0

        # 设置机械臂终端运动的目标位姿
        arm.set_pose_target(star_pose, end_effector_link)
        arm.go()

        radius = 0.05
        centerY = 0.0
        centerX = 0.40 - radius

        # 初始化路点列表
        waypoints = []
        starPoints = []

        pose_temp = star_pose
        for th in numpy.arange(0, 6.2831855, 1.2566371):
            pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
            pose_temp.pose.position.x = centerX + radius * math.cos(th)
            pose_temp.pose.position.z = 0.30
            wpose = deepcopy(pose_temp.pose)
            starPoints.append(deepcopy(wpose))

        # 将圆弧上的路径点加入列表
        waypoints.append(starPoints[0])
        waypoints.append(starPoints[2])
        waypoints.append(starPoints[4])
        waypoints.append(starPoints[1])
        waypoints.append(starPoints[3])
        waypoints.append(starPoints[0])

        fraction = 0.0  #路径规划覆盖率
        maxtries = 100  #最大尝试规划次数
        attempts = 0  #已经尝试规划次数

        # 设置机器臂当前的状态作为运动初始状态
        arm.set_start_state_to_current_state()

        # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点,完成圆弧轨迹
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = arm.compute_cartesian_path(
                waypoints,  # waypoint poses,路点列表
                0.01,  # eef_step,终端步进值
                0.0,  # jump_threshold,跳跃阈值
                True)  # avoid_collisions,避障规划

            # 尝试次数累加
            attempts += 1

            # 打印运动规划进程
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) +
                              " attempts...")

        # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败,则打印失败信息
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) +
                          " success after " + str(maxtries) + " attempts.")

        rospy.sleep(1)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    def __del__(self):

        moveit_commander.roscpp_shutdown()

        rospy.loginfo('\033[94m' + "Object of class Ur5Moveit Deleted." +
                      '\033[0m')
Пример #60
0
 def shutdown():
     """Shutdown moveit!
     """
     moveit_commander.roscpp_shutdown()