Python MoveGroupCommander.set_start_state_to_current_state方法代码示例

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

#.........这里部分代码省略.........
        # 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)

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

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

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

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

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

#.........这里部分代码省略.........
                rospy.sleep(1)
                continue
            
            # Transform the pose to the base reference frame
            target_base = self.listener.transformPose(self.reference_frame, target)
            
            # Compute the distance between the current target and the last target
            p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
            p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
                    
            dist_last_target = euclidean(p1, p2)
            
            # Move the arm only if we are far enough away from the previous target
            if dist_last_target < self.last_target_threshold:
                rospy.loginfo("Still close to last target")
                rospy.sleep(0.5)
                continue
            
            # Get the pose of the end effector in the base reference frame
            ee_pose = self.right_arm.get_current_pose(self.ee_link)
            
            # Convert the position values to a Python list
            p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
            
            # Compute the distance between the target and the end-effector
            dist_target = euclidean(p1, p3)
            
            # Only move the arm if we are far enough away from the target
            if dist_target < self.target_ee_threshold:
                rospy.loginfo("Already close enough to target")
                rospy.sleep(1)
                continue
            
            # We want the gripper somewhere on the line connecting the shoulder and the target.
            # Using a parametric form of the line, the parameter ranges from 0 to the
            # minimum of the arm length and the distance to the target.
            t_max = min(self.arm_length, dist_target_shoulder)
            
            # Bring it back 10% so we don't collide with the target
            t = 0.9 * t_max
            
            # Now compute the target positions from the parameter
            try:
                target_arm.pose.position.x *= (t / dist_target_shoulder)
                target_arm.pose.position.y *= (t / dist_target_shoulder)
                target_arm.pose.position.z *= (t / dist_target_shoulder)
            except:
                rospy.sleep(1)
                rospy.loginfo("Exception!")
                continue
            
            # Transform to the base_footprint frame
            target_ee = self.listener.transformPose(self.reference_frame, target_arm)
            
            # Set the target gripper orientation to be horizontal
            target_ee.pose.orientation.x = 0
            target_ee.pose.orientation.y = 0
            target_ee.pose.orientation.z = 0
            target_ee.pose.orientation.w = 1
            
            # Update the current start state
            self.right_arm.set_start_state_to_current_state()
            
            # Set the target pose for the end-effector
            self.right_arm.set_pose_target(target_ee, self.ee_link)
            
            # Plan and execute the trajectory
            success = self.right_arm.go()
            
            if success:
                # Store the current target as the last target
                self.last_target_pose = target
            
            # Pause a bit between motions to keep from locking up
            rospy.sleep(0.5)
            
                    
    def update_target_pose(self, target):
        self.target = target

    def relax_all_servos(self):
        command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
        args = shlex.split(command)
        subprocess.Popen(args)
           
    def shutdown(self):
        # Stop any further target messages from being processed
        self.target_subscriber.unregister()
        
        # Stop any current arm movement
        self.right_arm.stop()
        
        # Move to the resting position
        self.right_arm.set_named_target('resting')
        self.right_arm.go()
        
        # Relax the servos
        self.relax_all_servos()
        
        os._exit(0) 

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

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('right_arm')
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_footprint')
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        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...")
                         
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_start_state_to_current_state [as 别名]
	p2.pose.orientation.y = b2[1]
	p2.pose.orientation.z = b2[2]
	p2.pose.orientation.w = b2[3]

	#detach/remove current scene objects
	print 'removing objects...'
	robot.detach_object("bowl")
	rospy.sleep(1)
	scene.remove_world_object("bowl")
	scene.remove_world_object("punch")
	scene.remove_world_object("glovebox")

	rospy.sleep(2)

	#reset the gripper and arm position to home
	robot.set_start_state_to_current_state()
	robot.set_named_target("start_glove")
	robot.go()
	gripper.set_start_state_to_current_state()
	gripper.go()

	#add scene objects	
	print 'adding scene objects'
	scene.add_mesh("bowl", p, "8inhemi.STL")
	scene.add_mesh("punch", p1, "punch.STL")
	scene.add_mesh("glovebox", p2, "GloveBox.stl")
	print 'attaching bowl...'
	robot.attach_object("bowl")
	rospy.sleep(2)
	currentbowlpose = p;

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

#.........这里部分代码省略.........
                              joint_names=joint_names, 
                              joint_positions=joint_positions)
            
        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}

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

	def __init__(self, planArm="left_arm"):
		# Initialize the move_group API
		#moveit_commander.roscpp_initialize(sys.argv)

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

		# Connect to the arm move group
		if planArm == "right_arm":
			self.arm = MoveGroupCommander('right_arm')
			self.hand = Gripper(1)
		else:
			self.arm = MoveGroupCommander('left_arm')
			self.hand = Gripper(0)

		rospy.sleep(1)
		self.hand.release()
		
		# Allow replanning to increase the odds of a solution
		self.arm.allow_replanning(True)

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

	def pick(self):
		self.graspFromSide()
	
	def graspFromSide(self):

		targetPose=Pose()
		targetPose.position.x = 0.96
		targetPose.position.y = 0.10
		targetPose.position.z = -0.0

		targetPose.orientation.x = 0.044
		targetPose.orientation.y = 0.702
		targetPose.orientation.z = 0.045
		targetPose.orientation.w = 0.708

		# right side grasp
		# Position
		#x = 0.636
		#y = 0.832
		#z = 0.192

		# Orientation
		#x = -0.382
		#y = 0.922
		#z = 0.021
		#w= 0.049
		
		#xyz 0.871 0.428 0.038 
		#orientation 0.119 0.698 -0.083 0.700

		self.moveToTargetPose(targetPose)

		while self.hand.range.state() == 65535.0:
			targetPose.position.x += 0.05
			self.moveToTargetPose(targetPose)
		
		while self.hand.range.state() > 70:
			targetPose.position.x += 0.02
			self.moveToTargetPose(targetPose)
		
		self.hand.grasp()
		rospy.sleep(1)
		
		targetPose.position.x -= 0.05
		targetPose.position.y -= 0.05
		targetPose.position.z += 0.1
		self.moveToTargetPose(targetPose)

		self.hand.release()
		rospy.sleep(1)
		
	def moveToTargetPose(self, targetPose):
		# move to the defined pose
		self.arm.set_start_state_to_current_state()
		self.arm.set_pose_target(targetPose)
		self.arm.go()
		rospy.sleep(0.5)	

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

#.........这里部分代码省略.........
    
            # Get the support surface ID
            support_surface = find_result.objects[the_object].object.support_surface
                        
            # Set the support surface name to the table object
            right_arm.set_support_surface_name(support_surface)
            
            # 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 = target_pose.pose.position.x
            place_pose.pose.position.y = 0.03
            place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015
            place_pose.pose.orientation.w = 1.0
                            
            # Initialize the grasp pose to the target pose
            grasp_pose = target_pose
             
            # Shift the grasp pose half the size of the target to center it in the gripper
            try:
                grasp_pose.pose.position.x += target_size[0] / 2.0
                grasp_pose.pose.position.y -= 0.01
                grasp_pose.pose.position.z += target_size[2] / 2.0
            except:
                rospy.loginfo("Invalid object size so skipping")
                continue

            # Generate a list of grasps
            grasps = self.make_grasps(grasp_pose, [target_id])
     
            # Publish the grasp poses so they can be viewed in RViz
            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
            right_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 = right_arm.pick(target_id, grasps)
                n_attempts += 1
                rospy.loginfo("Pick attempt: " +  str(n_attempts))
                rospy.sleep(1.0)
             
            # If the pick was successful, attempt the place operation   
            if result == MoveItErrorCodes.SUCCESS:
                result = None
                n_attempts = 0
                  
                # Generate valid place poses
                places = self.make_places(place_pose)
                
                # Set the start state to the current state
                #right_arm.set_start_state_to_current_state()
                  
                # Repeat until we succeed or run out of attempts
                while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
                    for place in places:
                        result = right_arm.place(target_id, place)
                        if result == MoveItErrorCodes.SUCCESS:
                            break
                    n_attempts += 1
                    rospy.loginfo("Place attempt: " +  str(n_attempts))
                    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.")

            rospy.sleep(2)
            
            # Open the gripper to the neutral position
            right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
            right_gripper.go()
            
            rospy.sleep(2)
            
            # Return the arm to the "resting" pose stored in the SRDF file
            right_arm.set_named_target('resting')
            right_arm.go()
             
            rospy.sleep(2)
            
            # Give the servos a rest
            arbotix_relax_all_servos()
            
            rospy.sleep(2)
        
            if args.once:
                # Shut down 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 set_start_state_to_current_state [as 别名]
 scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
 
 # modify the pose
 p.pose.position.x = 0.72
 p.pose.position.z = 0.05
 # add the pen
 scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
 rospy.sleep(1)
  
 # print the existing groups 
 robot = RobotCommander()
 print "Available groups: ",robot.get_group_names()
 
 # setup the arm group and its planner
 arm = MoveGroupCommander("arm")
 arm.set_start_state_to_current_state()
 arm.set_planner_id("RRTstarkConfigDefault") 
 arm.set_planning_time(5.0)
 arm.detach_object("pen")
 
 # set the arm to a safe target
 arm.set_named_target("gamma")
 # plan and execute the motion 
 arm.go()
 
 # setup the hand group and its planner
 hand = MoveGroupCommander("hand")
 hand.set_start_state_to_current_state()
 hand.set_planner_id("LBKPIECEkConfigDefault") 
 hand.set_planning_time(10.0)
 

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

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the arm move group
        arm = MoveGroupCommander('arm')
        
        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)
        
        # Set the right arm reference frame
        arm.set_pose_reference_frame('base_link')
                
        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)
        
        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()
                                        
        # Set an initial position for the arm
        start_position = [0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069]

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

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

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.025,       # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_start_state_to_current_state [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                                
        # Initialize the MoveIt! commander for the right arm
        right_arm = MoveGroupCommander('right_arm')
                
        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander('right_gripper')
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Remove leftover objects from a previous run
        scene.remove_attached_object(end_effector_link, 'tool')
        scene.remove_world_object('table') 
        scene.remove_world_object('box1')    
        scene.remove_world_object('box2')
        scene.remove_world_object('target')

        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table
        table_size = [0.2, 0.7, 0.01]
        
        # Set the length, width and height of the object to attach
        tool_size = [0.3, 0.02, 0.02]
        
        # Create a pose for the tool relative to the end-effector
        p = PoseStamped()
        p.header.frame_id = end_effector_link
        
        scene.attach_mesh
        
        # Place the end of the object within the grasp of the gripper
        p.pose.position.x = tool_size[0] / 2.0 - 0.025
        p.pose.position.y = 0.0
        p.pose.position.z = 0.0
        
        # Align the object with the gripper (straight out)
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        
        # Attach the tool to the end-effector
        scene.attach_box(end_effector_link, 'tool', p, tool_size)

        # Add a floating table top
        table_pose = PoseStamped()
        table_pose.header.frame_id = 'base_footprint'
        table_pose.pose.position.x = 0.35
        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', table_pose, table_size)
        
        # Update the current state
        right_arm.set_start_state_to_current_state()

        # Move the arm with the attached object to the 'straight_forward' position
        right_arm.set_named_target('straight_forward')
        right_arm.go()
        rospy.sleep(2)  
        
        # Return the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(2)
         
        scene.remove_attached_object(end_effector_link, 'tool')   
        
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_start_state_to_current_state [as 别名]
    punch_pose.pose.position.y = 0
    punch_pose.pose.position.z = 0
    temp_eul = None

    #detach/remove current scene objects
    print 'removing objects...'
    robot.detach_object("bowl")
    rospy.sleep(0.5)
    scene.remove_world_object("bowl")
    scene.remove_world_object("punch")
    scene.remove_world_object("flat")

    rospy.sleep(0.5)

    #reset the gripper and arm position to home
    robot.set_start_state_to_current_state()
    robot.set_named_target("home")
    robot.go()
    startpose = PoseStamped()
    startpose.header.frame_id = 'world'
    startpose.header.stamp = rospy.Time.now()
    startpose = robot.get_current_pose()
    startpose.pose.position.x -= 0.3
    robot.set_pose_target(startpose)
    robot.go()
#    gripper.set_start_state_to_current_state()
#    gripper.set_named_target("default")
#    gripper.go()

    #add scene objects
    print 'adding scene objects'

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_start_state_to_current_state [as 别名]
def simple_function():
        rc = RobotCommander()
        mgc = MoveGroupCommander("manipulator")
        # print(rc.get_group_names())
        # print(rc.get_group('manipulator'))
        
        
        # exit()
        
        eef_step = 0.01
        jump_threshold = 2
        ### Create a handle for the Planning Scene Interface
        psi = PlanningSceneInterface()
        
        
        rc.get_current_state()
        rospy.logwarn(rc.get_current_state())
        sss.wait_for_input()
        #rate = rospy.Rate(0.1) # 10hz
        rate = rospy.Rate(1) # 10hz
        rospy.sleep(1)
        
        
        theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
        
        while not rospy.is_shutdown():
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = "table"
            approach_pose.header.stamp = rospy.Time(0)
            approach_pose.pose.position.x = 0.146
            approach_pose.pose.position.y = 0.622
            approach_pose.pose.position.z = 0.01
            quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
            approach_pose.pose.orientation.x = quat[0]
            approach_pose.pose.orientation.y = quat[1]
            approach_pose.pose.orientation.z = quat[2]
            approach_pose.pose.orientation.w = quat[3]
#             mgc.set_start_state_to_current_state()
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed before adding collision objects")
#             else:
#                 rospy.logwarn("Planning succeeded before adding collision objects")
# 
#             rospy.logwarn("waiting for input to add dummy box")
#             sss.wait_for_input()
#             
            box_dummy_pose = PoseStamped()
            box_dummy_pose.header.frame_id =  "table"
            box_dummy_pose.pose.position.x = 0.147
            box_dummy_pose.pose.position.y = 0.636
            box_dummy_pose.pose.position.z = 0
            psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC  
#             rospy.logwarn("I have added the box")
#             rospy.sleep(1)
#             rospy.logwarn("waiting for input to try planning with dummy box")
#             sss.wait_for_input()
#             
#             (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
#             if(frac_approach != 1):
#                 rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
#             else:
#                 rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#                 
            rospy.logwarn("waiting for input to add end effector box box")
            sss.wait_for_input()
            # end effector collision object
            link_attached_to_ef = "ee_link"
            mb_ef_collisonobj = "metal_bracket"
            mb_ef_pose = PoseStamped()
            mb_ef_pose.header.frame_id =  link_attached_to_ef
            mb_ef_pose.pose.position.x = 0.065/2.0
            mb_ef_pose.pose.position.y = 0.0
            mb_ef_pose.pose.position.z = 0.0
            mb_ef_size = (0.065,0.06,0.06)



            psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
            
            
            raw_input("0 hi")
            
            mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
            

            
            
            rospy.logwarn("I have added the attached box to the end effector")
            
            
            rospy.sleep(1)
            raw_input("1 hi")           
            
            rospy.logwarn(rc.get_current_state())
            # mgc.attach_object(object_name, link_name, touch_links)
            mgc.set_start_state_to_current_state()
            rospy.logwarn(rc.get_current_state())
            raw_input("2 hi")
            rospy.logwarn("waiting for input to try planning with end effector box")
#.........这里部分代码省略.........

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

#.........这里部分代码省略.........
        self.io_srv(r)

    def close_gripper(self):
        r = SetIORequest()
        r.fun = 1
        r.pin = 0
        r.state = 1.0
        self.io_srv(r)

    def plan_and_execute(self, userdata):
    
        ### DEBUG PERCEPTION ONLY
        #sss.wait_for_input()
        #userdata.new_box = True
        #return True
        ### END DEBUG
#         print "The pose reference frame is",self.mgc.get_pose_reference_frame()
#         self.mgc.set_pose_reference_frame("camera_rgb_optical_frame")
#         print "The pose reference frame is",self.mgc.get_pose_reference_frame()
        start_plan = rospy.Time.now()
        


        ### Set next (virtual) start state
        start_state = RobotState()
        (pre_grasp_config, error_code) = sss.compose_trajectory("arm","pre_grasp")
        if error_code != 0:
            rospy.logerr("unable to parse pre_grasp configuration")
            return False
        start_state.joint_state.name = pre_grasp_config.joint_names
        start_state.joint_state.position = pre_grasp_config.points[0].positions
        start_state.is_diff = True
#         self.mgc.set_start_state(start_state)
        self.mgc.set_start_state_to_current_state()

        ### Plan Approach
        approach_pose_offset = PoseStamped()
        approach_pose_offset.header.frame_id = "current_box"
        approach_pose_offset.header.stamp = rospy.Time(0)
#         approach_pose_offset.pose.position.x -= 0.1
        approach_pose_offset.pose.position.z += 0.05
        #quat = tf.transformations.quaternion_from_euler(0, 1.57, 1.57)
        #approach_pose_offset.pose.orientation.x = quat[0]
        #approach_pose_offset.pose.orientation.y = quat[1]
        #approach_pose_offset.pose.orientation.z = quat[2]
        #approach_pose_offset.pose.orientation.w = quat[3]
        
        try:
            approach_pose = self.listener.transformPose("table", approach_pose_offset)
            approach_pose.pose.position.x += -0.01
#             approach_pose = self.listener.transformPose("camera_rgb_optical_frame", approach_pose_offset)
        except Exception, e:
            rospy.logerr("could not transform pose. Exception: %s", str(e))
            return False
            
        rospy.logwarn("Approach planninig without collision object")
        (traj_approach,frac_approach) = self.mgc.compute_cartesian_path([approach_pose.pose], self.eef_step, self.jump_threshold, True)
        
        if not (frac_approach == 1.0):
            rospy.logerr("Unable to plan approach trajectory during approach before grasp. frac = %f", frac_approach)
            #for i in range(len(traj_approach.joint_trajectory.points)):
            #    traj_approach.joint_trajectory.points[i].velocities = [0]*7
            #sss.wait_for_input()
            #sss.move("arm","pre_grasp")#DELETE
            #self.mgc.execute(traj_approach)#DELETE
#             return False