demo_graspEventdetect.py 文件源码

def limb_pose(limb_name):
    """Get limb pose at time of OK cuff button press."""
    button = CuffOKButton(limb_name)
    rate = rospy.Rate(20)  # rate at which we check whether button was
                           # pressed or not
    rospy.loginfo(
        'Waiting for %s OK cuff button press to save pose' % limb_name)
    while not button.pressed and not rospy.is_shutdown():
        rate.sleep()
    joint_pose = baxter_interface.Limb(limb_name).joint_angles()
    # Now convert joint coordinates to end effector cartesian
    # coordinates using forward kinematics.
    kinematics = baxter_kinematics(limb_name)
    endpoint_pose = kinematics.forward_position_kinematics(joint_pose)
    # How is this different from
    # baxter_interface.Limb(limb_name).endpoint_pose()
    print()
    print('baxter_interface endpoint pose:')
    print(baxter_interface.Limb(limb_name).endpoint_pose())
    print('pykdl forward kinematics endpoint pose:')
    print(endpoint_pose)
    print()
    return endpoint_pose