Python MoveGroupCommander.execute方法代码示例

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

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.0176318609849
    wpose.orientation.x = 0.392651866792
    wpose.orientation.y = 0.918465607415
    wpose.orientation.z = 0.0439835989663
    wpose.position.y = 0.227115629827
    wpose.position.z = 1.32344046934
    wpose.position.x = -0.358178766726
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
        wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
        wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
        wpose.orientation.z = waypoints[i-1].orientation.z - 0.308
        wpose.position.y = waypoints[i-1].position.y - 0.1
        wpose.position.z = waypoints[i-1].position.z 

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

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
	while rospy.get_time() == 0.0: pass
	
	### Create a handle for the Planning Scene Interface
	psi = PlanningSceneInterface()
	rospy.sleep(1.0)
	
	### Create a handle for the Move Group Commander
	mgc = MoveGroupCommander("arm")
	rospy.sleep(1.0)
	
	
	### Add virtual obstacle
	pose = gen_pose(pos=[-0.2, -0.1, 1.2])
	psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
	rospy.sleep(1.0)
	
	### Move to stored joint position
	mgc.set_named_target("left")
	mgc.go()
	
	### Move to Cartesian position
	goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
	mgc.go(goal_pose.pose)
	
	### Move Cartesian linear
	goal_pose.pose.position.z -= 0.1
	(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
	mgc.execute(traj)
	
	print "Done"

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.445677565944
    wpose.orientation.x = -0.326257686982
    wpose.orientation.y = -0.770762217792
    wpose.orientation.z = -0.31757366472
    wpose.position.y = 0.172181465244
    wpose.position.z = 1.57438900065
    wpose.position.x = -0.136240808288
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose
    
    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()

        wpose.orientation.w = waypoints[i-1].orientation.w 
        wpose.orientation.x = waypoints[i-1].orientation.x 
        wpose.orientation.y = waypoints[i-1].orientation.y 
        wpose.orientation.z = waypoints[i-1].orientation.z 
        wpose.position.y = waypoints[i-1].position.y - 0.15

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
	right_arm = MoveGroupCommander("right_arm")

	start_pose = geometry_msgs.msg.Pose()
        start_pose.position.x = -0.0206330384032
    	start_pose.position.y = 0.077582282778
    	start_pose.position.z = 1.39283677496
    	start_pose.orientation.x = 0.5
    	start_pose.orientation.y = 0.5
    	start_pose.orientation.z = 0.5
    	start_pose.orientation.w = 0.5

	right_arm.set_pose_target(start_pose)	
	plan_start = right_arm.plan()
   	print "Plan start"
    	rospy.sleep(5)
    	right_arm.execute(plan_start)
    	print "Execute start"
    	rospy.sleep(5)

	end_pose = geometry_msgs.msg.Pose()
        end_pose.position.x = -0.0434279649929
    	end_pose.position.y = -0.0562017053887
    	end_pose.position.z = 1.48763433664
    	end_pose.orientation.x = 0.5
    	end_pose.orientation.y = 0.5
    	end_pose.orientation.z = 0.5
    	end_pose.orientation.w = 0.5

	right_arm.set_pose_target(end_pose)	
	plan_end = right_arm.plan()
   	print "Plan end"

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

#.........这里部分代码省略.........
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                                        
        # Start in the "straight_forward" configuration stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = right_arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

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

        # Move normally back to the 'resting' position
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(1)
        
        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)

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

#.........这里部分代码省略.........
        timer = Timer(0.0, self.__start_ctrl)
        timer.start()
        
        time.sleep(0.1)
        self.__unpause_physics.call()

        self.__reset_world.call()

    def get_object_pose(self):
        """
        Gets the pose of the ball in the world frame.
        
        @return The pose of the ball.
        """
        return self.__get_pose_srv.call(self.__current_model_name, "world").pose

    def get_tip_pose(self):
        """
        Gets the current pose of the robot's tooltip in the world frame.
        @return the tip pose
        """
        return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose

    def move_tip_absolute(self, target):
        """
        Moves the tooltip to the absolute target in the world frame

        @param target is a geometry_msgs.msg.Pose
        @return True on success
        """
        self.arm_commander.set_start_state_to_current_state()
        self.arm_commander.set_pose_targets([target])
        plan = self.arm_commander.plan()
        if not self.arm_commander.execute(plan):
            return False
        return True
        
    def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
        """
        Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. 

        @return True on success
        """
        transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
                                PyKDL.Vector(-x, -y, -z))
        
        tip_pose = self.get_tip_pose()
        tip_pose_kdl = posemath.fromMsg(tip_pose)
        final_pose = toMsg(tip_pose_kdl * transform)
            
        self.arm_commander.set_start_state_to_current_state()
        self.arm_commander.set_pose_targets([final_pose])
        plan = self.arm_commander.plan()
        if not  self.arm_commander.execute(plan):
            return False
        return True

    def send_command(self, command, duration=0.2):
        """
        Send a dictionnary of joint targets to the arm and hand directly.
        
        @param command: a dictionnary of joint names associated with a target:
                        {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
        @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
        """
        hand_goal = None

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.46822292532
    wpose.orientation.x = -0.0855260625632
    wpose.orientation.y = -0.781945671575
    wpose.orientation.z = -0.402509070125
    wpose.position.y = 0.0924247084452
    wpose.position.z = 1.64720495919
    wpose.position.x = -0.0834555596048
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    points = 10
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w 
        wpose.orientation.x = waypoints[i-1].orientation.x 
        wpose.orientation.y = waypoints[i-1].orientation.y 
        wpose.orientation.z = waypoints[i-1].orientation.z 
        wpose.position.y = waypoints[i-1].position.y 
        wpose.position.z = waypoints[i-1].position.z - 0.05

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.468593286845
    wpose.orientation.x = -0.0856909255342
    wpose.orientation.y = -0.781529322291
    wpose.orientation.z = -0.40285148033
    wpose.position.y = 0.0925025791124
    wpose.position.z = 1.33093409609
    wpose.position.x = -0.0834208108295
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w
        wpose.orientation.x = waypoints[i-1].orientation.x
        wpose.orientation.y = waypoints[i-1].orientation.y
        wpose.orientation.z = waypoints[i-1].orientation.z
        wpose.position.y = waypoints[i-1].position.y
        wpose.position.z = waypoints[i-1].position.z + 0.4

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.570398010274
    wpose.orientation.x = -0.279748061111
    wpose.orientation.y = -0.719670670452
    wpose.orientation.z = -0.280109368412
    wpose.position.y = -0.0349149588619
    wpose.position.z = 1.69789257029
    wpose.position.x =  0.0117939632591
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose
    gain = 0.05;
    r = 0.05
    a = 0.0117939632591 # waypoints[0].position.x
    b = 1.64789257 # waypoints[0].position.z
    #waypoints[0].position.x = a + r
    

    points = 10
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()

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

#.........这里部分代码省略.........
        arm.set_pose_reference_frame('base_link')
                
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
                                        
        # Set an initial position for the arm
        start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]

        # Set the goal pose of the end effector to the stored pose
        arm.set_joint_value_target(start_position)
        
        # Plan and execute a trajectory to the goal configuration
        arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Move end effector to the right 0.3 meters
        wpose.position.y -= 0.3

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

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.025,       # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            # If we have a complete plan, execute the trajectory
            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.")  
        
        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import execute [as 别名]
    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.409003402335
    wpose.orientation.x = -0.330487715391
    wpose.orientation.y = -0.782324118504
    wpose.orientation.z = -0.333860839963
    wpose.position.y = 0.0776977107894
    wpose.position.z = 1.25950497489
    wpose.position.x = -0.199191527741
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose
    

    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w + 0.428513
        wpose.orientation.x = waypoints[i-1].orientation.x 
        wpose.orientation.y = waypoints[i-1].orientation.y 
        wpose.orientation.z = waypoints[i-1].orientation.z - 0.175202
        wpose.position.y = waypoints[i-1].position.y - 0.025