motion_editor.py 文件源码

def compute_static_stability_thread():
    rate = rospy.Rate(60)
    global kron_com_vertices
    while not rospy.is_shutdown():
        if 'COM-static' in support_area_handles \
                and not gui.show_com_support_area:
            delete_support_area_display('COM-static')
        if not motion_plan or not motion_plan.started \
                or 'COM-static' in support_area_handles \
                or not gui.show_com_support_area:
            rate.sleep()
            continue
        color = (0.1, 0.1, 0.1, 0.5)
        req = contact_stability.srv.StaticAreaRequest(
            contacts=convert_contacts_to_ros(motion_plan.cur_stance.contacts),
            mass=robot.mass, z_out=motion_plan.com_height)
        try:
            res = compute_com_area(req)
            vertices = [array([v.x, v.y, v.z]) for v in res.vertices]
            update_support_area_display('COM-static', vertices, [], color)
        except rospy.ServiceException:
            delete_support_area_display('COM-static')
        rate.sleep()