Python Limb.endpoint_pose方法代碼示例

# 需要導入模塊: from baxter_interface import Limb [as 別名]
# 或者: from baxter_interface.Limb import endpoint_pose [as 別名]
 def endpoint_pose(self):
     """
     Returns the pose of the end effector
     :return: [[x, y, z], [x, y, z, w]]
     """
     pose = Limb.endpoint_pose(self)
     return [[pose['position'].x, pose['position'].y, pose['position'].z],
             [pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]

# 需要導入模塊: from baxter_interface import Limb [as 別名]
# 或者: from baxter_interface.Limb import endpoint_pose [as 別名]
    
    tracker.busy = True
    

    done = False
    ik()
    
    #print("Controlling joints. Press ? for help, Esc to quit.")
    while not done and not rospy.is_shutdown():
        c = baxter_external_devices.getch()
        tracker.track()
        if tracker.busy == False:
            tracker.gripper.close()
            rospy.sleep(0.5)
            arm = Limb("right")
            ep_position = arm.endpoint_pose()['position']
            ep_orientation = arm.endpoint_pose()['orientation']
            place(right,ep_position,ep_orientation,pr,pr_place)
            tracker.gripper.open()
            rospy.sleep(0.5)
            pr_place.position.y += 0.2
            move(right,pr_place)
            #move(right,pr_init)
            print "I am done"
            done = True
        
        if c:
            #catch Esc or ctrl-c
            if c in ['\x1b', '\x03']:
                done = True
                print ("Program finished")

# 需要導入模塊: from baxter_interface import Limb [as 別名]
# 或者: from baxter_interface.Limb import endpoint_pose [as 別名]
class IKHelper(object):
    """An abstraction layer for using Baxter's built in IK service."""

    ############################################################################

    def __init__(self):
        self._left_arm = Limb('left')
        self._left_arm.set_joint_position_speed(0.3)
        self._right_arm = Limb('right')
        self._right_arm.set_joint_position_speed(0.3)

        self._left_iksvc = rospy.ServiceProxy(
            _IKSVC_LEFT_URI,
            SolvePositionIK)

        self._right_iksvc = rospy.ServiceProxy(
            _IKSVC_RIGHT_URI,
            SolvePositionIK)

        self._joint_update_pub = rospy.Publisher(
            '/robot/joint_state_publish_rate', 
            UInt16)
        self._joint_update_pub.publish(250)

    ############################################################################

    def reset(self):
        """Reset both arms to their neutral positions."""   
    	self._left_arm.move_to_neutral()
        self._right_arm.move_to_neutral()

    def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
        """Set the endpoint of the left arm to the supplied coordinates.

        Arguments:
            pos -- Position in space in (x, y, z) format.
            rot -- Rotation in space in (r, p, y) format. (defaults to pointing
                downwards.)

        Keyword arguments:
            wait -- If True, method will block until in position. (default 
                False)
        """
        self._set_arm(self._left_iksvc, self._left_arm, pos, rot, wait)

    def set_left(self, pos, rot=(0, math.pi, math.pi *0.5), wait=False):
        """Set the endpoint of the right arm to the supplied coordinates.

        Arguments:
            pos -- Position in space in (x, y, z) format.
            rot -- Rotation in space in (r, p, y) format. (defaults to pointing
                downwards.)

        Keyword arguments:
            wait -- If True, method will block until in position. (default 
                False)
        """
        self._set_arm(self._right_iksvc, self._right_arm, pos, rot, wait)

    def get_left(self):
        """Return the current endpoint pose of the left arm."""
        return self._left_arm.endpoint_pose()['position']
    
    def get_right(self):
        """Return the current endpoint pose of the left arm."""
        return self._right_arm.endpoint_pose()['position']

    def get_left_velocity(self):
        """Return the current endpoint velocity of the left arm."""
        return self._left_arm.endpoint_velocity()['linear']

    def get_right_velocity(self):
        """Return the current endpoint velocity of the right arm."""
        return self._right_arm.endpoint_velocity()['linear']

    def get_left_force(self):
        """Return the current endpoint force on the left arm."""
        return self._left_arm.endpoint_effort()['force']

    def get_right_force(self):
        """Return the current endpoint force on the right arm."""
        return self._right_arm.endpoint_effort()['force']

    ############################################################################

    def _set_arm(self, iksvc, limb, pos, rot, wait):
        resp = self._get_ik(iksvc, pos, rot)
        positions = resp[0]
        isValid = resp[1]
        if not isValid:
            print('invalid: {0} {1} {2}'.format(x, y, z))

        if not wait:
            limb.set_joint_positions(positions)
        else:
            limb.move_to_joint_positions(positions)

    def _get_ik(self, iksvc, pos, rot):
        q = quaternion_from_euler(rot[0], rot[1], rot[2])

#.........這裏部分代碼省略.........