pos_tools.py 文件源码

def go_to_pose(lmb, pose_msg, timeout=15.0):

    """
    Given a limb (right or left) and a desired pose as a stamped message,
    run inverse kinematics to attempt to find a joint configuration to yield
    the pose and then move limb to the configuration.
    After 5 seconds of attempts to solve the IK, raise an exception.
    """

    node = "ExternalTools/" + lmb + "/PositionKinematicsNode/IKService"
    ik_service = rospy.ServiceProxy(node, SolvePositionIK)
    ik_request = SolvePositionIKRequest()
    ik_request.pose_stamp.append(pose_msg)

    # Allow 5 seconds to find an IK solution
    try:
        rospy.wait_for_service(node, 5.0)
        ik_response = ik_service(ik_request)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1

    # Check if result valid
    if ik_response.isValid[0]:
        # convert response to joint position control dictionary
        limb_joints = dict(zip(ik_response.joints[0].name,
                               ik_response.joints[0].position))
        # send limb to joint positions
        baxter_interface.limb.Limb(lmb).move_to_joint_positions(
            limb_joints, timeout=timeout)
    else:
        raise Exception("Failed to find valid joint configuration.")