Python MoveGroupCommander.set_planner_id方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
class GenericDualArmClient():
    _safe_kinematicsolver = "RRTConnectkConfigDefault"

    def __init__(self, *args, **kwargs):
        larm_name = args[0]
        rarm_name = args[1]  # "arm_right" for Motoman SDA10F
        self.r_arm = MoveGroupCommander(rarm_name)
        self.r_arm.set_planner_id(self._safe_kinematicsolver)

    def move_rarm_shift_forward(self, axis, value):
        '''
        Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
        '''
        self.r_arm.shift_pose_target(axis, value)
        self.r_arm.plan()
        self.r_arm.go()

    def move_rarm_fixedpose_forward(self):
        tpose = Pose()
        #TODO Currently the following position may work with SDA10F but not with NXO
        tpose.position.x = 0.599
        tpose.position.y = -0.379
        tpose.position.z = 1.11
        self.r_arm.set_pose_target(tpose)
        self.r_arm.plan()
        self.r_arm.go()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
def get_move_group_commander(group):
    '''
    Gets the move_group_commander for this process.

    '''
    global _mgc_dict
    with _mgc_dict_creation_lock:
        if not group in _mgc_dict:
            _mgc_group = MoveGroupCommander(group)
            _mgc_group.set_planner_id('RRTConnectkConfigDefault')
            _mgc_dict[group] = _mgc_group
	    add_ground()
        return _mgc_dict[group]

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
def main():
    rospy.init_node('arm_start_up', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    left_arm = MoveGroupCommander("left_arm")
    right_arm.set_planner_id("RRTConnect:"); 
    left_arm.set_planner_id("RRTConnect:")
    
  
    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)
    right_arm.set_named_target("right_start")
    right_arm.go()
    rospy.sleep(1)
    left_arm.set_named_target("left_start")
    left_arm.go()
    rospy.sleep(1)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('move_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("manipulator")

    # specify the planner
    group.set_planner_id("RRTkConfigDefault")

    rospy.sleep(3)

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

    print ">>>>> remove scenes"

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("ball")

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
def close_hand():
    msg = Float64()
    msg.data = -1.0
    for i in range(30):
        pub.publish(msg)
        rospy.sleep(0.1)

if __name__ == '__main__':
    rospy.init_node('task_2_cheat', anonymous=True)

    # for for 5 sec
    rospy.sleep(5)

    rospy.loginfo("start program %f" % rospy.get_time())
    arm = MoveGroupCommander("ur5_arm")
    arm.set_planner_id('RRTConnectkConfigDefault')
    pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1)
    client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    client.wait_for_server()

    rospy.loginfo("init pose")
    msg = FollowJointTrajectoryGoal()
    msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
    msg.trajectory.joint_names = ['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']
    msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
                                                      time_from_start=rospy.Duration(2)))

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [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()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault");
        left_arm.set_planner_id("KPIECEkConfigDefault");
        rospy.sleep(1)
        
        # 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)
        
        # 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 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # 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)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # 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 height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # 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.56
        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 = 0.51
        box1_pose.pose.position.y = -0.1
        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 = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
 
 # 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)
 
 # set the hand to a safe target

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    GRIPPER_FRAME = 'right_gripper_link'
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    right_gripper = MoveGroupCommander("right_gripper")
    right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    #rospy.logwarn("cleaning world")
    #right_arm.set_named_target("r_start")
    #right_arm.go()
   
    #right_gripper.set_named_target("open")
    #right_gripper.go()
   
    rospy.sleep(3)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node("moveit_py_demo", anonymous=True)

        scene = PlanningSceneInterface()
        robot = RobotCommander()

        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        right_arm.set_planner_id("KPIECEkConfigDefault")
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        eef = right_arm.get_end_effector_link()

        rospy.sleep(2)

        scene.remove_attached_object(GRIPPER_FRAME, "part")

        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")

        # right_arm.set_named_target("r_start")
        # right_arm.go()

        # right_gripper.set_named_target("right_gripper_open")
        # right_gripper.go()

        rospy.sleep(1)

        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()

        # add a table
        # p.pose.position.x = 0.42
        # p.pose.position.y = -0.2
        # p.pose.position.z = 0.3
        # scene.add_box("table", p, (0.5, 1.5, 0.6))

        # add an object to be grasped
        p.pose.position.x = 0.40
        p.pose.position.y = -0.0
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))

        rospy.sleep(1)

        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME

        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.37636
        start_pose.pose.position.y = 0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1

        right_arm.set_pose_target(start_pose)
        right_arm.go()

        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)

        result = False
        n_attempts = 0

        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)

        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import set_planner_id [as 别名]
class Arm_Controller:
	def __init__(self):
		# Give the launch a chance to catch up
		# rospy.sleep(5)

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

		rospy.init_node('Arm_Controller')
		rospy.loginfo("Launched Arm Controller")

		# constants
		self.GROUP_NAME_ARM = 'arm'
		self.GRIPPER_FRAME = 'gripper_link'
		self.REFERENCE_FRAME = 'base_link'
		self.ARM_BASE_FRAME = 'arm_base_link'

		self.done = True

		self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped)

		rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1)
		self.robot_name = "gatlin"
		move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm)

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

		# Initialize the move group for the right arm
		self.arm = MoveGroupCommander(self.GROUP_NAME_ARM)
		self.gripper = Gripper()

		self.robot = moveit_commander.RobotCommander()

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

		# Set the planner
		self.arm.set_planner_id("RRTConnectkConfigDefault")

		# Set the right arm reference frame
		self.arm.set_pose_reference_frame(self.REFERENCE_FRAME)

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

		# Allow some leeway in position (meters) and orientation (radians)
		# USELESS; do not work on pick and place! Explained on this issue:
		# https://github.com/ros-planning/moveit_ros/issues/577
		self.arm.set_goal_position_tolerance(0.005)
		self.arm.set_goal_orientation_tolerance(0.05)

		# Allow 2 seconds per planning attempt
		self.arm.set_planning_time(2.0)

		# Create a quaternion from the Euler angles
		#q = quaternion_from_euler(0, pitch, yaw)
		# horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555)
		# deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861)
		down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
		# back_pos = Point(-0.03, 0.0313, 0.476)

		# init rest pose
		self.rest_pose = PoseStamped()
		self.rest_pose.header.frame_id = self.REFERENCE_FRAME
		self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35)
		self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981)

		# init place_upper pose
		self.place_upper_pose = PoseStamped()
		self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME
		self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998)
		self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889)

		# init current pose
		self.current_pose = PoseStamped()
		self.current_pose.header.frame_id = self.REFERENCE_FRAME
		self.current_pose.pose.position = Point(0,0,0)
		self.current_pose.pose.orientation = down

		# Open the gripper
		rospy.loginfo("Set Gripper: open")
		self.gripper.set(1.0)

		# self.arm.set_pose_target(self.rest_pose)
		# self.arm.go()
		# rospy.sleep(1)

		# self.ar = ArbotixROS()

		# rate = rospy.Rate(30)
		# while not rospy.is_shutdown():
			
		# 	# rospy.logerr(self.ar.getVoltage(4))
		# 	# rospy.logerr(self.ar.getSpeed(5))
		# 	rospy.logerr(self.ar.getPosition(1))

		# 	rate.sleep()

		rospy.spin()
#.........这里部分代码省略.........