Python MoveGroupCommander.get_current_pose方法代码示例

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
    scene.add_box("table", p, (1.0, 2.6, 0.2))

    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.4
    scene.add_plane("ground_plane", p)

    rospy.sleep(20)

    ## We can get the name of the reference frame for this robot
    print ">>>>> Reference frame: %s" % group.get_planning_frame()
    print ">>>>> Printing robot state"
    print robot.get_current_state()

    print ">>>>> Printing robot pose"
    print group.get_current_pose()

    ## Planning to a Pose goal
    print ">>>>> Generating plan"
    pose_target = geometry_msgs.msg.Pose()
    #pose_target.orientation.w = 1.0
    pose_target.position.x = 0.5  #0.4
    pose_target.position.y = 0.2  #0.2
    pose_target.position.z = 0.2  #0.2
    group.set_pose_target(pose_target)

    plan = group.plan()

    #print "============ Waiting while RVIZ displays plan..."
    rospy.sleep(1)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
# Author: Kenji Miyake, Isaac Isao Saito

# This test script needs improved so that it becomes call-able from ROS test
# structure.

from geometry_msgs.msg import Pose, PoseStamped
from moveit_commander import MoveGroupCommander, conversions
import rospy

rospy.init_node("test_hironx_moveit")

rarm = MoveGroupCommander("right_arm")
larm = MoveGroupCommander("left_arm")

rarm_current_pose = rarm.get_current_pose().pose
larm_current_pose = larm.get_current_pose().pose


def _set_target_random(self):
    '''
    @type self: moveit_commander.MoveGroupCommander
    @param self: In this particular test script, the argument "self" is either
                 'rarm' or 'larm'.
    '''
    global current, current2, target
    current = self.get_current_pose()
    print "*current*", current
    target = self.get_random_pose()
    print "*target*", target
    self.set_pose_target(target)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
    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 "============ Waiting while RVIZ displays plan_start..."
#     rospy.sleep(5)
    right_arm.execute(plan_start)
    print "============ Waiting while RVIZ executes plan_start..."
    rospy.sleep(5)
     
    gain = 0.1
    points = 20
    step = points/4
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    for i in xrange(step):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation = waypoints[i-1].orientation
        wpose.position.y = waypoints[i-1].position.y
        wpose.position.z = waypoints[i-1].position.z + gain
        wpose.position.x = waypoints[i-1].position.x
        waypoints.append(copy.deepcopy(wpose))
 
     
    (plan_waypoints, fraction) = right_arm.compute_cartesian_path(
                                                             waypoints,   # waypoints to follow
                                                             0.01,        # eef_step
                                                             0.0)         # jump_threshold
    print fraction*100, "% planned"
    print "============ Waiting while RVIZ displays plan_waypoints..."

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
    p.header.frame_id = REFERENCE_LINK
    p.header.stamp = rospy.Time.now()

    quat = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
    p.pose.orientation = Quaternion(*quat)

    p.pose.position.x = 0.6
    p.pose.position.y = 0.0
    p.pose.position.z = 0.0
    # add table
    scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))

    i = 1
    while i <= 10:

        gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
        # part
        p.pose.position.x = gripper_pose.pose.position.x
        p.pose.position.y = gripper_pose.pose.position.y
        p.pose.position.z = gripper_pose.pose.position.z
        # add part
        scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
        rospy.loginfo("Added object to world")

        # attach object manually
        arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
        rospy.sleep(1)

        #  ===== place start ==== #
        place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())

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

	gripper.set_named_target("pinch")
	gripper.go()

	robotstart = robot.get_current_pose()
	print 'start eef pose:'
	print robotstart
	robotstart_quat = [robotstart.pose.orientation.x,robotstart.pose.orientation.y,robotstart.pose.orientation.z,robotstart.pose.orientation.w]
	M_eef = tf.transformations.quaternion_matrix(robotstart_quat)
	currentbowlpose = p

	#calculate position offset from bowl to eef frames
	xoffset = robotstart.pose.position.x-p.pose.position.x
	yoffset = robotstart.pose.position.y-p.pose.position.y
	zoffset = robotstart.pose.position.z-p.pose.position.z
	print 'x: ',xoffset,' y: ',yoffset,' z: ',zoffset

	print 'finished'

	rate = rospy.Rate(10.0)

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

from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown

from math import sin, copysign, sqrt, pi
   
if __name__ == '__main__':
    print "============ Dynamic hand gestures: Right"
    roscpp_initialize(sys.argv)
    rospy.init_node('pumpkin_planning', anonymous=True)

    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)

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
    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'
    scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")
    scene.add_mesh("punch", punch_pose, "punch.STL")
    rospy.sleep(1)

#    gripper.set_named_target("pinch")
#    gripper.go()

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [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 get_current_pose [as 别名]
    	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"
    	rospy.sleep(5)
    	right_arm.execute(plan_end)
    	print "Execute end"
    	rospy.sleep(5)

	#rospy.sleep(1)	
	
	current_pose = geometry_msgs.msg.Pose()
	current_pose = right_arm.get_current_pose()
	
	print(current_pose)

	print "---------------------------------------End"

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
import rospy
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from geometry_msgs.msg import Pose, PoseStamped
from moveit_msgs.msg import PlanningSceneWorld, CollisionObject

rospy.init_node('move_web')
mg = MoveGroupCommander('right_arm_and_torso')

p = mg.get_current_pose()
print "Start pose:"
print p

p.pose.position.x += 0.3

#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)

#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)

#psw_pub.publish(psw)

# ps.remove_world_object("test_sphere")

# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)

#p.pose.position.x += 0.3

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

#.........这里部分代码省略.........
        self.__pause_physics.call()
        
        joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 
                       'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 
                       'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
        joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
        
        self.__set_model.call(model_name="smart_grasping_sandbox", 
                              urdf_param_name="robot_description",
                              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):

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [as 别名]
class BipedCommander():
##########################################################################
##########################################################################

    ##########################################################################
    def __init__(self):
    ##########################################################################
            rospy.loginfo("BipedCommander started")
            self.legs_group = MoveGroupCommander("legs")
            self.arms_group = MoveGroupCommander("arms")
            self.rarm_group = MoveGroupCommander("RArm")
            self.larm_group = MoveGroupCommander("LArm")

            
    ##########################################################################
    def move_lfoot(self, x, y, z):
    ##########################################################################
        rospy.loginfo("BipedCommander move_lfoot")
        end_effector_link = "lfoot"
        self.move_foot( end_effector_link, x, y, z)

    ##########################################################################
    def move_rfoot(self, x, y, z):
    ##########################################################################
        rospy.loginfo("BipedCommander move_rfoot")
        end_effector_link = "rfoot"
        self.move_foot( end_effector_link, x, y, z)
        
    ##########################################################################
    def move_foot(self, end_effector_link, x, y, z):
    ##########################################################################
        pose = [x, y, z, 0, 0.7071, 0.7071, 0]
        if len(end_effector_link) > 0 or self.has_end_effector_link():
            rospy.logdebug("setting target")
            r = self.legs_group.set_pose_target(pose, end_effector_link)
            rospy.loginfo("set position target returned %s" % str(r)) 
            rospy.logdebug("going")
            r =  self.legs_group.go()
            rospy.loginfo("go returned %s" % str(r)) 
        else:
            rospy.logerr("There is no end effector to set the pose for")

    ##########################################################################
    def move_larm(self, x, y, z):
    ##########################################################################
        rospy.loginfo("BipedCommander move_rarm")
        end_effector_link = "ltip3"
        group = self.larm_group
        self.move_arm( group, end_effector_link, x, y, z)

    ##########################################################################
    def move_rarm(self, x, y, z):
    ##########################################################################
        rospy.loginfo("BipedCommander move_rarm")
        end_effector_link = "rtip3"
        group = self.arms_group
        self.move_arm( group, end_effector_link, x, y, z)


    ##########################################################################
    def move_arm(self, group, end_effector_link, x, y, z):
    ##########################################################################
        #ose = [x, y, z, -1, 0, 0, 0]
        pose = [x, y, z ]
        if len(end_effector_link) > 0 or self.has_end_effector_link():
            rospy.loginfo("setting target to %s" % pose)
            r = group.set_position_target(pose, end_effector_link)
            rospy.loginfo("set position target returned %s" % str(r)) 
            rospy.logdebug("going")
            r =  group.go()
            rospy.loginfo("go returned %s" % str(r)) 
        else:
            rospy.logerr("There is no end effector to set the pose for")
       
    ##########################################################################
    def move_legs(self, lx, ly, lz, rx, ry, rz): 
    ##########################################################################
        pose = [lx, ly, lz, 0, 0.7071, 0.7071, 0]
        end_effector_link = "lfoot"
        rospy.loginfo("setting lfoot target to %s" % pose)
        r = self.legs_group.set_pose_target(pose, end_effector_link)
        rospy.loginfo("set lfoot position target returned %s" % str(r)) 

        pose = [rx, ry, rz, 0, 0.7071, 0.7071, 0]
        rospy.loginfo("setting rfoot target to %s" % pose)
        end_effector_link = "rfoot"
        r = self.legs_group.set_pose_target(pose, end_effector_link)
        rospy.loginfo("set rfoot position target returned %s" % str(r)) 

        r =  self.legs_group.go()
        rospy.loginfo("go returned %s" % str(r)) 
        
    ##########################################################################
    def pose_print(self):
    ##########################################################################
        end_effector_link = "lfoot"
        r = self.legs_group.get_current_pose( end_effector_link)
        p = r.pose.position
        o = r.pose.orientation
        rospy.loginfo("Left foot pose: [%0.3f, %0.3f, %0.3f], [%0.3f, %0.3f, %0.3f, %0.3f] " % (p.x, p.y, p.z, o.x, o.y, o.z, o.w))
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [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 get_current_pose [as 别名]
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print group.get_end_effector_link()
    print group.get_pose_reference_frame()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"
    tl = tf.TransformListener()
    
    rospy.sleep(1)

    waypoints = []
    
    # start with the current pose
    waypoints.append(group.get_current_pose().pose)
    print waypoints[0]
    currentPose = PoseStamped()
    currentPose.header.frame_id = group.get_pose_reference_frame()
    currentPose.pose = waypoints[0]
    
    currentPoseHandLink = tl.transformPose("Hand_Link", currentPose)
    currentPoseHandLink.pose.position.z= currentPoseHandLink.pose.position.z - 0.1
    
    # first orient gripper and move forward (+x)
    wpose = tl.transformPose(group.get_pose_reference_frame(), currentPoseHandLink).pose
    #wpose = 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

# 需要导入模块: from moveit_commander import MoveGroupCommander [as 别名]
# 或者: from moveit_commander.MoveGroupCommander import get_current_pose [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
#.........这里部分代码省略.........