ik_execute.py 文件源码

def get_ik_joints(target_x, target_y, initial_z, target_z, target_theta, n_steps):
    ns = "ExternalTools/left/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest(seed_mode=SolvePositionIKRequest.SEED_CURRENT)
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')

    qx = np.sin(target_theta * 0.5)
    qy = np.cos(target_theta * 0.5)

    for z in np.arange(initial_z, target_z, (target_z-initial_z)/n_steps):
        pose = PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point( x=target_x, y=target_y, z=z, ),
                orientation=Quaternion( x=qx, y=qy, z=0., w=0., ),
            ),
        )
        ikreq.pose_stamp.append(pose)
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1

    return [j for v, j in zip(resp.isValid, resp.joints) if v]