lto_code/0000775000175000017500000000000013124341100010415 5ustar kekelto_code/LICENSE0000644000175000017500000010451313124341100011424 0ustar keke GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . lto_code/compile_proto.sh0000755000175000017500000000077213124341100013633 0ustar kekePROTO_SRC_DIR=src/proto DST_DIR=build # Hack to compile directly into src folders for now CPP_OUT_DIR=src/gps_agent_pkg/include/gps/proto PROTO_BUILD_DIR=$DST_DIR/$PROTO_SRC_DIR PY_PROTO_BUILD_DIR=python/gps/proto mkdir -p "$PROTO_BUILD_DIR" mkdir -p "$PY_PROTO_BUILD_DIR" touch $PY_PROTO_BUILD_DIR/__init__.py mkdir -p "$CPP_OUT_DIR" protoc -I=$PROTO_SRC_DIR --cpp_out=$CPP_OUT_DIR $PROTO_SRC_DIR/gps.proto protoc -I=$PROTO_SRC_DIR --python_out=$PY_PROTO_BUILD_DIR $PROTO_SRC_DIR/gps.proto echo "Done" lto_code/README0000664000175000017500000000215013124341100011273 0ustar kekeCode for Learning to Optimize This is a Python re-implementation of the method described in our paper, which can be found at https://arxiv.org/abs/1606.01885 It is based on the Guided Policy Search implementation (https://github.com/cbfinn/gps). Copyright (C) 2017 Ke Li, Jitendra Malik This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . =========================================================================================================================================== Requires TensorFlow v0.12 Run compile_proto.sh first before running run_lto.sh lto_code/experiments/0000775000175000017500000000000013124341100012760 5ustar kekelto_code/experiments/lto/0000775000175000017500000000000013124341100013556 5ustar kekelto_code/experiments/lto/hyperparams.py0000644000175000017500000001450313124341100016464 0ustar kekeimport tensorflow as tf import os.path from datetime import datetime import numpy as np import gps from gps import __file__ as gps_filepath from gps.agent.lto.agent_lto import AgentLTO from gps.agent.lto.lto_world import LTOWorld from gps.algorithm.algorithm import Algorithm from gps.algorithm.cost.cost import Cost from gps.algorithm.dynamics.dynamics_lr_prior import DynamicsLRPrior from gps.algorithm.dynamics.dynamics_prior_gmm import DynamicsPriorGMM from gps.algorithm.policy.policy_prior_gmm import PolicyPriorGMM from gps.algorithm.traj_opt.traj_opt import TrajOpt from gps.algorithm.policy_opt.policy_opt import PolicyOpt from gps.algorithm.policy_opt.lto_model import fully_connected_tf_network from gps.algorithm.policy.lin_gauss_init import init_lto_controller from gps.proto.gps_pb2 import CUR_LOC, PAST_OBJ_VAL_DELTAS, PAST_GRADS, CUR_GRAD, PAST_LOC_DELTAS, ACTION from gps.agent.lto.fcn import LogisticRegressionFcnFamily, LogisticRegressionFcn from gps.algorithm.cost.cost_utils import RAMP_CONSTANT try: import cPickle as pickle except: import pickle import copy def gen_fcns(input_dim, num_fcns, session, num_inits_per_fcn = 1, num_points_per_class = 50): fcn_family = LogisticRegressionFcnFamily(input_dim, gpu_id = 0, session = session, tensor_prefix = "logistic_reg") # Dimensionality of the space over which optimization is performed param_dim = fcn_family.get_total_num_dim() fcn_objs = [] for i in range(num_fcns): data = [] for j in range(2): mu = np.random.randn(input_dim) sigma = np.random.randn(input_dim, input_dim) sigma_sq = np.dot(sigma, sigma.T) data.append(np.random.multivariate_normal(mu, sigma_sq, num_points_per_class)) data = np.vstack(data) labels = np.vstack((np.zeros((num_points_per_class,1),dtype=np.int),np.ones((num_points_per_class,1),dtype=np.int))) fcn = LogisticRegressionFcn(fcn_family, data, labels, disable_subsampling = True) for j in range(num_inits_per_fcn): fcn_objs.append(fcn) init_locs = np.random.randn(param_dim,num_fcns*num_inits_per_fcn) fcns = [{'fcn_obj': fcn_objs[i], 'dim': param_dim, 'init_loc': init_locs[:,i][:,None]} for i in range(num_fcns*num_inits_per_fcn)] return fcns,fcn_family def lto_on_exit(config): config['agent']['fcn_family'].destroy() session = tf.Session() history_len = 25 num_fcns = 10 #100 input_dim = 3 cur_dir = os.path.dirname(os.path.abspath(__file__)) dataset_file = cur_dir + "/trainset.pkl" if os.path.isfile(dataset_file): print("Dataset already exists. Loading from %s. " % (dataset_file)) with open(dataset_file, "rb") as f: fcns,fcn_family = pickle.load(f) fcn_family.start_session(session) else: print("Generating new dataset.") fcns,fcn_family = gen_fcns(input_dim, num_fcns, session) with open(dataset_file, "wb") as f: pickle.dump((fcns,fcn_family), f) print("Saved to %s. " % (dataset_file)) param_dim = fcns[0]['dim'] SENSOR_DIMS = { CUR_LOC: param_dim, PAST_OBJ_VAL_DELTAS: history_len, PAST_GRADS: history_len*param_dim, PAST_LOC_DELTAS: history_len*param_dim, CUR_GRAD: param_dim, ACTION: param_dim } BASE_DIR = '/'.join(str.split(gps_filepath, '/')[:-2]) EXP_DIR = BASE_DIR + '/../experiments/lto/' common = { 'experiment_name': 'lto' + '_' + \ datetime.strftime(datetime.now(), '%m-%d-%y_%H-%M'), 'experiment_dir': EXP_DIR, 'data_files_dir': EXP_DIR + 'data_files/', 'log_filename': EXP_DIR + 'log.txt', 'conditions': num_fcns } if not os.path.exists(common['data_files_dir']): os.makedirs(common['data_files_dir']) agent = { 'type': AgentLTO, 'world' : LTOWorld, 'substeps': 1, 'conditions': common['conditions'], 'dt': 0.05, 'T': 40, 'sensor_dims': SENSOR_DIMS, 'state_include': [CUR_LOC, PAST_OBJ_VAL_DELTAS, PAST_GRADS, CUR_GRAD, PAST_LOC_DELTAS], 'obs_include': [PAST_OBJ_VAL_DELTAS, PAST_GRADS, CUR_GRAD, PAST_LOC_DELTAS], 'history_len': history_len, 'fcns': fcns, 'fcn_family': fcn_family # Only used to destroy these at the end } algorithm = { 'type': Algorithm, 'conditions': common['conditions'], 'iterations': 10, 'inner_iterations': 4, 'policy_dual_rate': 0.2, 'init_pol_wt': 0.01, 'ent_reg_schedule': 0.0, 'fixed_lg_step': 3, 'kl_step': 0.2, 'min_step_mult': 0.01, 'max_step_mult': 10.0, 'sample_decrease_var': 0.05, 'sample_increase_var': 0.1, 'policy_sample_mode': 'replace' } algorithm['init_traj_distr'] = { 'type': init_lto_controller, 'init_var': 0.01, 'dt': agent['dt'], 'T': agent['T'], 'all_possible_momentum_params': np.array([0.82, 0.84, 0.86, 0.88, 0.9, 0.92]), 'all_possible_learning_rates': np.array([0.025, 0.05, 0.1, 0.2, 0.4, 0.8, 1.6]) } algorithm['cost'] = { 'type': Cost, 'ramp_option': RAMP_CONSTANT, 'wp_final_multiplier': 1.0, 'weight': 1.0, } algorithm['dynamics'] = { 'type': DynamicsLRPrior, 'regularization': 1e-3, # Increase this if Qtt is not PD during DGD 'clipping_thresh': None, 'prior': { 'type': DynamicsPriorGMM, 'max_clusters': 20, 'min_samples_per_cluster': 20, 'max_samples': 20, 'strength': 1.0 # How much weight to give to prior relative to samples } } algorithm['traj_opt'] = { 'type': TrajOpt, } algorithm['policy_opt'] = { 'type': PolicyOpt, 'network_model': fully_connected_tf_network, 'iterations': 20000, 'init_var': 0.01, 'batch_size': 25, 'solver_type': 'adam', 'lr': 0.0001, 'lr_policy': 'fixed', 'momentum': 0.9, 'weight_decay': 0.005, 'use_gpu': 1, 'weights_file_prefix': EXP_DIR + 'policy', 'network_params': { 'obs_include': agent['obs_include'], 'sensor_dims': agent['sensor_dims'], 'dim_hidden': [50] } } algorithm['policy_prior'] = { 'type': PolicyPriorGMM, 'max_clusters': 20, 'min_samples_per_cluster': 20, 'max_samples': 20, 'strength': 1.0, 'clipping_thresh': None, 'init_regularization': 1e-3, 'subsequent_regularization': 1e-3 } config = { 'iterations': algorithm['iterations'], 'num_samples': 20, 'common': common, 'agent': agent, 'algorithm': algorithm, 'on_exit': lto_on_exit, } lto_code/run_lto.sh0000744000175000017500000000004213124341100012426 0ustar kekepython python/gps/gps_main.py lto lto_code/python/0000755000175000017500000000000013124341100011734 5ustar kekelto_code/python/gps/0000755000175000017500000000000013124341100012525 5ustar kekelto_code/python/gps/algorithm/0000755000175000017500000000000013124341100014513 5ustar kekelto_code/python/gps/algorithm/algorithm.py0000644000175000017500000007674213124341100017073 0ustar keke""" This file defines the BADMM-based GPS algorithm. """ import copy import logging import numpy as np import scipy as sp from gps.algorithm.algorithm_utils import PolicyInfo from gps.algorithm.config import ALG from gps.sample.sample_list import SampleList from gps.algorithm.algorithm_utils import IterationData, TrajectoryInfo from gps.utility.general_utils import extract_condition LOGGER = logging.getLogger(__name__) class Algorithm(object): """ Sample-based joint policy learning and trajectory optimization with BADMM-based guided policy search algorithm. """ def __init__(self, hyperparams): config = copy.deepcopy(ALG) config.update(hyperparams) self._hyperparams = config if 'train_conditions' in hyperparams: self._cond_idx = hyperparams['train_conditions'] self.M = len(self._cond_idx) else: self.M = hyperparams['conditions'] self._cond_idx = range(self.M) self._hyperparams['train_conditions'] = self._cond_idx self._hyperparams['test_conditions'] = self._cond_idx self.iteration_count = 0 # Grab a few values from the agent. agent = self._hyperparams['agent'] self.agent = agent self.T = self._hyperparams['T'] = agent.T self.dU = self._hyperparams['dU'] = agent.dU self.dX = self._hyperparams['dX'] = agent.dX self.dO = self._hyperparams['dO'] = agent.dO init_traj_distr = config['init_traj_distr'] init_traj_distr['x0'] = agent.x0 init_traj_distr['dX'] = agent.dX init_traj_distr['dU'] = agent.dU del self._hyperparams['agent'] # Don't want to pickle this. # IterationData objects for each condition. self.cur = [IterationData() for _ in range(self.M)] self.prev = [IterationData() for _ in range(self.M)] dynamics = self._hyperparams['dynamics'] for m in range(self.M): self.cur[m].traj_info = TrajectoryInfo() self.cur[m].traj_info.dynamics = dynamics['type'](dynamics) cur_init_traj_distr = extract_condition( init_traj_distr, self._cond_idx[m] ) cur_init_traj_distr['cur_cond_idx'] = self._cond_idx[m] self.cur[m].traj_distr = cur_init_traj_distr['type'](cur_init_traj_distr, agent) self.traj_opt = hyperparams['traj_opt']['type']( hyperparams['traj_opt'] ) self.cost = [] for m in range(self.M): cost_hyperparams = hyperparams['cost'].copy() cost_hyperparams['cur_cond_idx'] = self._cond_idx[m] self.cost.append(hyperparams['cost']['type'](cost_hyperparams)) self.base_kl_step = self._hyperparams['kl_step'] policy_prior = self._hyperparams['policy_prior'] for m in range(self.M): self.cur[m].pol_info = PolicyInfo(self._hyperparams) self.cur[m].pol_info.policy_prior = \ policy_prior['type'](policy_prior) self.policy_opt = self._hyperparams['policy_opt']['type']( self._hyperparams['policy_opt'], self.dO, self.dU ) # policies is a list of M policies def print_policy_cost(self, policies, num_samples = 5): for m in range(self.M): all_cs = np.empty((num_samples, self.T)) for i in range(num_samples): sample = self.agent.sample(policies[m], self._cond_idx[m], save=False) # cs has shape of (T,) cs = self.cost[m].eval(sample,True)[0] all_cs[i,:] = cs total_cs = np.sum(all_cs, axis=1) print("[Condition %d] Cumulative Costs: %s, Mean Cumulative Cost: %.4f" % (m,repr(total_cs.tolist()),np.mean(total_cs))) def iteration(self, sample_lists): """ Run iteration of BADMM-based guided policy search. Args: sample_lists: List of SampleList objects for each condition. """ for m in range(self.M): self.cur[m].sample_list = sample_lists[m] if self.iteration_count == 0: print("Initial Trajectories") self.print_policy_cost([self.cur[m].traj_distr for m in range(self.M)]) self._set_interp_values() self._update_dynamics() # Update dynamics model using all sample. self._update_step_size() # KL Divergence step size. for m in range(self.M): # save initial kl for debugging / visualization self.cur[m].pol_info.init_kl = self._policy_kl(m)[0] # Run inner loop to compute new policies. for inner_itr in range(self._hyperparams['inner_iterations']): #TODO: Could start from init controller. if self.iteration_count > 0 or inner_itr > 0: # Update the policy. self._update_policy(inner_itr) for m in range(self.M): self._update_policy_fit(m) # Update policy priors. if self.iteration_count > 0 or inner_itr > 0: step = (inner_itr == self._hyperparams['inner_iterations'] - 1) # Update dual variables. for m in range(self.M): self._policy_dual_step(m, step=step) self._update_trajectories() print("New Trajectories") self.print_policy_cost(self.new_traj_distr) self._advance_iteration_variables() def _set_interp_values(self): """ Use iteration-based interpolation to set values of some schedule-based parameters. """ # Compute temporal interpolation value. t = min((self.iteration_count + 1.0) / (self._hyperparams['iterations'] - 1), 1) # Perform iteration-based interpolation of entropy penalty. if type(self._hyperparams['ent_reg_schedule']) in (int, float): self.policy_opt.set_ent_reg(self._hyperparams['ent_reg_schedule']) else: sch = self._hyperparams['ent_reg_schedule'] self.policy_opt.set_ent_reg( np.exp(np.interp(t, np.linspace(0, 1, num=len(sch)), np.log(sch))) ) # Perform iteration-based interpolation of Lagrange multiplier. if type(self._hyperparams['lg_step_schedule']) in (int, float): self._hyperparams['lg_step'] = self._hyperparams['lg_step_schedule'] else: sch = self._hyperparams['lg_step_schedule'] self._hyperparams['lg_step'] = np.exp( np.interp(t, np.linspace(0, 1, num=len(sch)), np.log(sch)) ) def _update_step_size(self): """ Evaluate costs on samples, and adjust the step size. """ # Evaluate cost function for all conditions and samples. for m in range(self.M): self._update_policy_fit(m, init=True) self._eval_cost(m) # Adjust step size relative to the previous iteration. if self.iteration_count >= 1 and self.prev[m].sample_list: self._stepadjust(m) def _update_policy(self, inner_itr): """ Compute the new policy. """ dU, dO, T = self.dU, self.dO, self.T # Compute target mean, cov, and weight for each sample. obs_data, tgt_mu = np.zeros((0, T, dO)), np.zeros((0, T, dU)) tgt_prc, tgt_wt = np.zeros((0, T, dU, dU)), np.zeros((0, T)) for m in range(self.M): samples = self.cur[m].sample_list X = samples.get_X() N = len(samples) if inner_itr > 0: traj, pol_info = self.new_traj_distr[m], self.cur[m].pol_info else: traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info mu = np.zeros((N, T, dU)) prc = np.zeros((N, T, dU, dU)) wt = np.zeros((N, T)) # Get time-indexed actions. for t in range(T): # Compute actions along this trajectory. prc[:, t, :, :] = np.tile(traj.inv_pol_covar[t, :, :], [N, 1, 1]) for i in range(N): mu[i, t, :] = \ (traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :]) - \ np.linalg.solve( prc[i, t, :, :] / pol_info.pol_wt[t], pol_info.lambda_K[t, :, :].dot(X[i, t, :]) + \ pol_info.lambda_k[t, :] ) wt[:, t].fill(pol_info.pol_wt[t]) tgt_mu = np.concatenate((tgt_mu, mu)) tgt_prc = np.concatenate((tgt_prc, prc)) tgt_wt = np.concatenate((tgt_wt, wt)) obs_data = np.concatenate((obs_data, samples.get_obs())) self.policy_opt.update(obs_data, tgt_mu, tgt_prc, tgt_wt) # Fit linear model for mean policy action given state def _update_policy_fit(self, m, init=False): """ Re-estimate the local policy values in the neighborhood of the trajectory. Args: m: Condition init: Whether this is the initial fitting of the policy. """ dX, dU, T = self.dX, self.dU, self.T # Choose samples to use. samples = self.cur[m].sample_list N = len(samples) pol_info = self.cur[m].pol_info X = samples.get_X() obs = samples.get_obs() pol_mu, pol_sig = self.policy_opt.prob(obs)[:2] # Update policy prior. policy_prior = pol_info.policy_prior if init: samples = SampleList(self.cur[m].sample_list) mode = self._hyperparams['policy_sample_mode'] else: samples = SampleList([]) mode = 'add' # Don't replace with empty samples policy_prior.update(samples, self.policy_opt, mode) # Fit linearization and store in pol_info. pol_info.pol_K, pol_info.pol_k, pol_info.pol_S = \ policy_prior.fit(X, pol_mu, pol_sig) for t in range(T): pol_info.chol_pol_S[t, :, :] = \ sp.linalg.cholesky(pol_info.pol_S[t, :, :]) def _policy_dual_step(self, m, step=False): """ Update the dual variables for the specified condition. Args: m: Condition step: Whether or not to update pol_wt. """ dU, T = self.dU, self.T samples = self.cur[m].sample_list N = len(samples) X = samples.get_X() if 'new_traj_distr' in dir(self): traj, pol_info = self.new_traj_distr[m], self.cur[m].pol_info else: traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info # Compute trajectory action at each sampled state. traj_mu = np.zeros((N, T, dU)) for i in range(N): for t in range(T): traj_mu[i, t, :] = traj.K[t, :, :].dot(X[i, t, :]) + \ traj.k[t, :] obs = samples.get_obs() pol_mu = self.policy_opt.prob(obs, True)[0] # Compute the difference and increment based on pol_wt. for t in range(T): tU, pU = traj_mu[:, t, :], pol_mu[:, t, :] # Increment mean term. pol_info.lambda_k[t, :] -= self._hyperparams['policy_dual_rate'] * \ pol_info.pol_wt[t] * \ traj.inv_pol_covar[t, :, :].dot(np.mean(tU - pU, axis=0)) # Increment covariance term. t_covar, p_covar = traj.K[t, :, :], pol_info.pol_K[t, :, :] pol_info.lambda_K[t, :, :] -= \ self._hyperparams['policy_dual_rate_covar'] * \ pol_info.pol_wt[t] * \ traj.inv_pol_covar[t, :, :].dot(t_covar - p_covar) # Compute KL divergence. kl_m = self._policy_kl(m)[0] if step: lg_step = self._hyperparams['lg_step'] # Increment pol_wt based on change in KL divergence. if self._hyperparams['fixed_lg_step'] == 1: # Take fixed size step. pol_info.pol_wt = np.array([ max(wt + lg_step, 0) for wt in pol_info.pol_wt ]) elif self._hyperparams['fixed_lg_step'] == 2: # (In/De)crease based on change in constraint # satisfaction. if hasattr(pol_info, 'prev_kl'): kl_change = kl_m / pol_info.prev_kl for i in range(len(pol_info.pol_wt)): if kl_change[i] < 0.8: pol_info.pol_wt[i] *= 0.5 elif kl_change[i] >= 0.95: pol_info.pol_wt[i] *= 2.0 elif self._hyperparams['fixed_lg_step'] == 3: # (In/De)crease based on difference from average. if hasattr(pol_info, 'prev_kl'): lower = np.mean(kl_m) - \ self._hyperparams['exp_step_lower'] * np.std(kl_m) upper = np.mean(kl_m) + \ self._hyperparams['exp_step_upper'] * np.std(kl_m) for i in range(len(pol_info.pol_wt)): if kl_m[i] < lower: pol_info.pol_wt[i] *= \ self._hyperparams['exp_step_decrease'] elif kl_m[i] >= upper: pol_info.pol_wt[i] *= \ self._hyperparams['exp_step_increase'] else: # Standard DGD step. pol_info.pol_wt = np.array([ max(pol_info.pol_wt[t] + lg_step * kl_m[t], 0) for t in range(T) ]) pol_info.prev_kl = kl_m def _update_dynamics(self): """ Instantiate dynamics objects and update prior. Fit dynamics to current samples. """ for m in range(self.M): cur_data = self.cur[m].sample_list X = cur_data.get_X() U = cur_data.get_U() # Update prior and fit dynamics. self.cur[m].traj_info.dynamics.update_prior(cur_data) self.cur[m].traj_info.dynamics.fit(X, U) # Fit x0mu/x0sigma. x0 = X[:, 0, :] x0mu = np.mean(x0, axis=0) self.cur[m].traj_info.x0mu = x0mu self.cur[m].traj_info.x0sigma = np.diag( np.maximum(np.var(x0, axis=0), self._hyperparams['initial_state_var']) ) prior = self.cur[m].traj_info.dynamics.get_prior() if prior: mu0, Phi, priorm, n0 = prior.initial_state() N = len(cur_data) self.cur[m].traj_info.x0sigma += \ Phi + (N*priorm) / (N+priorm) * \ np.outer(x0mu-mu0, x0mu-mu0) / (N+n0) def _update_trajectories(self): """ Compute new linear Gaussian controllers. """ if not hasattr(self, 'new_traj_distr'): self.new_traj_distr = [ self.cur[cond].traj_distr for cond in range(self.M) ] for cond in range(self.M): self.new_traj_distr[cond], self.cur[cond].eta = \ self.traj_opt.update(cond, self) def _eval_cost(self, cond): """ Evaluate costs for all samples for a condition. Args: cond: Condition to evaluate cost on. """ # Constants. T, dX, dU = self.T, self.dX, self.dU N = len(self.cur[cond].sample_list) # Compute cost. cs = np.zeros((N, T)) cc = np.zeros((N, T)) cv = np.zeros((N, T, dX+dU)) Cm = np.zeros((N, T, dX+dU, dX+dU)) for n in range(N): sample = self.cur[cond].sample_list[n] # Get costs. l, lx, lu, lxx, luu, lux = self.cost[cond].eval(sample) cc[n, :] = self.cost[cond].weight * dU * l cs[n, :] = l # Assemble matrix and vector. cv[n, :, :] = self.cost[cond].weight * dU * np.c_[lx, lu] Cm[n, :, :, :] = self.cost[cond].weight * dU * np.concatenate( (np.c_[lxx, np.transpose(lux, [0, 2, 1])], np.c_[lux, luu]), axis=1 ) # Adjust for expanding cost around a sample. X = sample.get_X() U = sample.get_U() yhat = np.c_[X, U] rdiff = -yhat rdiff_expand = np.expand_dims(rdiff, axis=2) cv_update = np.sum(Cm[n, :, :, :] * rdiff_expand, axis=1) cc[n, :] += np.sum(rdiff * cv[n, :, :], axis=1) + 0.5 * \ np.sum(rdiff * cv_update, axis=1) cv[n, :, :] += cv_update # Fill in cost estimate. self.cur[cond].traj_info.cc = np.mean(cc, 0) # Constant term (scalar). self.cur[cond].traj_info.cv = np.mean(cv, 0) # Linear term (vector). self.cur[cond].traj_info.Cm = np.mean(Cm, 0) # Quadratic term (matrix). self.cur[cond].cs = cs # True value of cost. def _advance_iteration_variables(self): """ Move all 'cur' variables to 'prev', reinitialize 'cur' variables, and advance iteration counter. """ self.iteration_count += 1 self.prev = self.cur # TODO: change IterationData to reflect new stuff better for m in range(self.M): self.prev[m].new_traj_distr = self.new_traj_distr[m] self.cur = [IterationData() for _ in range(self.M)] for m in range(self.M): self.cur[m].traj_info = TrajectoryInfo() cur_dynamics_prior = self.prev[m].traj_info.dynamics.prior self.prev[m].traj_info.dynamics.prior = None self.cur[m].traj_info.dynamics = copy.deepcopy(self.prev[m].traj_info.dynamics) self.cur[m].traj_info.dynamics.prior = cur_dynamics_prior self.cur[m].step_mult = self.prev[m].step_mult self.cur[m].eta = self.prev[m].eta self.cur[m].traj_distr = self.new_traj_distr[m] delattr(self, 'new_traj_distr') for m in range(self.M): self.cur[m].traj_info.last_kl_step = \ self.prev[m].traj_info.last_kl_step cur_policy_prior = self.prev[m].pol_info.policy_prior self.prev[m].pol_info.policy_prior = None self.cur[m].pol_info = copy.deepcopy(self.prev[m].pol_info) self.cur[m].pol_info.policy_prior = cur_policy_prior def _stepadjust(self, m): """ Calculate new step sizes. Args: m: Condition """ # Compute values under Laplace approximation. This is the policy # that the previous samples were actually drawn from under the # dynamics that were estimated from the previous samples. prev_laplace_obj, prev_laplace_kl = self._estimate_cost( self.prev[m].traj_distr, self.prev[m].traj_info, self.prev[m].pol_info, m ) # This is the policy that we just used under the dynamics that # were estimated from the previous samples (so this is the cost # we thought we would have). new_pred_laplace_obj, new_pred_laplace_kl = self._estimate_cost( self.cur[m].traj_distr, self.prev[m].traj_info, self.prev[m].pol_info, m ) # This is the actual cost we have under the current trajectory # based on the latest samples. new_actual_laplace_obj, new_actual_laplace_kl = self._estimate_cost( self.cur[m].traj_distr, self.cur[m].traj_info, self.cur[m].pol_info, m ) # Measure the entropy of the current trajectory (for printout). ent = self._measure_ent(m) # Compute actual objective values based on the samples. prev_mc_obj = np.mean(np.sum(self.prev[m].cs, axis=1), axis=0) new_mc_obj = np.mean(np.sum(self.cur[m].cs, axis=1), axis=0) # Compute sample-based estimate of KL divergence between policy # and trajectories. new_mc_kl = self._policy_kl(m)[0] if self.iteration_count >= 1 and self.prev[m].sample_list: prev_mc_kl = self._policy_kl(m, prev=True)[0] else: prev_mc_kl = np.zeros_like(new_mc_kl) # Compute full policy KL divergence objective terms by applying # the Lagrange multipliers. pol_wt = self.cur[m].pol_info.pol_wt prev_laplace_kl_sum = np.sum(prev_laplace_kl * pol_wt) new_pred_laplace_kl_sum = np.sum(new_pred_laplace_kl * pol_wt) new_actual_laplace_kl_sum = np.sum(new_actual_laplace_kl * pol_wt) prev_mc_kl_sum = np.sum(prev_mc_kl * pol_wt) new_mc_kl_sum = np.sum(new_mc_kl * pol_wt) LOGGER.debug( 'Trajectory step: ent: %f cost: %f -> %f KL: %f -> %f', ent, prev_mc_obj, new_mc_obj, prev_mc_kl_sum, new_mc_kl_sum ) # Compute predicted and actual improvement. predicted_impr = np.sum(prev_laplace_obj) + prev_laplace_kl_sum - \ np.sum(new_pred_laplace_obj) - new_pred_laplace_kl_sum actual_impr = np.sum(prev_laplace_obj) + prev_laplace_kl_sum - \ np.sum(new_actual_laplace_obj) - new_actual_laplace_kl_sum # Print improvement details. LOGGER.debug('Previous cost: Laplace: %f MC: %f', np.sum(prev_laplace_obj), prev_mc_obj) LOGGER.debug('Predicted new cost: Laplace: %f MC: %f', np.sum(new_pred_laplace_obj), new_mc_obj) LOGGER.debug('Actual new cost: Laplace: %f MC: %f', np.sum(new_actual_laplace_obj), new_mc_obj) LOGGER.debug('Previous KL: Laplace: %f MC: %f', np.sum(prev_laplace_kl), np.sum(prev_mc_kl)) LOGGER.debug('Predicted new KL: Laplace: %f MC: %f', np.sum(new_pred_laplace_kl), np.sum(new_mc_kl)) LOGGER.debug('Actual new KL: Laplace: %f MC: %f', np.sum(new_actual_laplace_kl), np.sum(new_mc_kl)) LOGGER.debug('Previous w KL: Laplace: %f MC: %f', prev_laplace_kl_sum, prev_mc_kl_sum) LOGGER.debug('Predicted w new KL: Laplace: %f MC: %f', new_pred_laplace_kl_sum, new_mc_kl_sum) LOGGER.debug('Actual w new KL: Laplace %f MC: %f', new_actual_laplace_kl_sum, new_mc_kl_sum) LOGGER.debug('Predicted/actual improvement: %f / %f', predicted_impr, actual_impr) # Compute actual KL step taken at last iteration. actual_step = self.cur[m].traj_info.last_kl_step / \ (self._hyperparams['kl_step'] * self.T) if actual_step < self.cur[m].step_mult: self.cur[m].step_mult = max(actual_step, self._hyperparams['min_step_mult']) self._set_new_mult(predicted_impr, actual_impr, m) def _policy_kl(self, m, prev=False): """ Monte-Carlo estimate of KL divergence between policy and trajectory. """ dU, T = self.dU, self.T if prev: traj, pol_info = self.prev[m].traj_distr, self.cur[m].pol_info samples = self.prev[m].sample_list else: traj, pol_info = self.cur[m].traj_distr, self.cur[m].pol_info samples = self.cur[m].sample_list N = len(samples) X, obs = samples.get_X(), samples.get_obs() kl, kl_m = np.zeros((N, T)), np.zeros(T) kl_l, kl_lm = np.zeros((N, T)), np.zeros(T) # Compute policy mean and covariance at each sample. pol_mu, _, pol_prec, pol_det_sigma = self.policy_opt.prob(obs) # Compute KL divergence. for t in range(T): # Compute trajectory action at sample. traj_mu = np.zeros((N, dU)) for i in range(N): traj_mu[i, :] = traj.K[t, :, :].dot(X[i, t, :]) + traj.k[t, :] diff = pol_mu[:, t, :] - traj_mu tr_pp_ct = pol_prec[:, t, :, :] * traj.pol_covar[t, :, :] k_ln_det_ct = 0.5 * dU + np.sum( np.log(np.diag(traj.chol_pol_covar[t, :, :])) ) ln_det_cp = np.log(pol_det_sigma[:, t]) # IMPORTANT: Note that this assumes that pol_prec does not # depend on state!!!! # (Only the last term makes this assumption.) d_pp_d = np.sum(diff * (diff.dot(pol_prec[1, t, :, :])), axis=1) kl[:, t] = 0.5 * np.sum(np.sum(tr_pp_ct, axis=1), axis=1) - \ k_ln_det_ct + 0.5 * ln_det_cp + 0.5 * d_pp_d tr_pp_ct_m = np.mean(tr_pp_ct, axis=0) kl_m[t] = 0.5 * np.sum(np.sum(tr_pp_ct_m, axis=0), axis=0) - \ k_ln_det_ct + 0.5 * np.mean(ln_det_cp) + \ 0.5 * np.mean(d_pp_d) # Compute trajectory action at sample with Lagrange # multiplier. traj_mu = np.zeros((N, dU)) for i in range(N): traj_mu[i, :] = \ (traj.K[t, :, :] - pol_info.lambda_K[t, :, :]).dot( X[i, t, :] ) + (traj.k[t, :] - pol_info.lambda_k[t, :]) # Compute KL divergence with Lagrange multiplier. diff_l = pol_mu[:, t, :] - traj_mu d_pp_d_l = np.sum(diff_l * (diff_l.dot(pol_prec[1, t, :, :])), axis=1) kl_l[:, t] = 0.5 * np.sum(np.sum(tr_pp_ct, axis=1), axis=1) - \ k_ln_det_ct + 0.5 * ln_det_cp + 0.5 * d_pp_d_l kl_lm[t] = 0.5 * np.sum(np.sum(tr_pp_ct_m, axis=0), axis=0) - \ k_ln_det_ct + 0.5 * np.mean(ln_det_cp) + \ 0.5 * np.mean(d_pp_d_l) return kl_m, kl, kl_lm, kl_l def _estimate_cost(self, traj_distr, traj_info, pol_info, m): """ Compute Laplace approximation to expected cost. Args: traj_distr: A linear Gaussian policy object. traj_info: A TrajectoryInfo object. pol_info: Policy linearization info. m: Condition number. """ # Constants. T, dU, dX = self.T, self.dU, self.dX # Perform forward pass (note that we repeat this here, because # traj_info may have different dynamics from the ones that were # used to compute the distribution already saved in traj). mu, sigma = self.traj_opt.forward(traj_distr, traj_info) # Compute cost. predicted_cost = np.zeros(T) for t in range(T): predicted_cost[t] = traj_info.cc[t] + 0.5 * \ (np.sum(sigma[t, :, :] * traj_info.Cm[t, :, :]) + mu[t, :].T.dot(traj_info.Cm[t, :, :]).dot(mu[t, :])) + \ mu[t, :].T.dot(traj_info.cv[t, :]) # Compute KL divergence. predicted_kl = np.zeros(T) for t in range(T): inv_pS = np.linalg.solve( pol_info.chol_pol_S[t, :, :], np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU)) ) Ufb = pol_info.pol_K[t, :, :].dot(mu[t, :dX].T) + \ pol_info.pol_k[t, :] diff = mu[t, dX:] - Ufb Kbar = traj_distr.K[t, :, :] - pol_info.pol_K[t, :, :] predicted_kl[t] = 0.5 * (diff).dot(inv_pS).dot(diff) + \ 0.5 * np.sum(traj_distr.pol_covar[t, :, :] * inv_pS) + \ 0.5 * np.sum( sigma[t, :dX, :dX] * Kbar.T.dot(inv_pS).dot(Kbar) ) + np.sum( np.log(np.diag(pol_info.chol_pol_S[t, :, :])) ) - np.sum( np.log(np.diag(traj_distr.chol_pol_covar[t, :, :])) ) + 0.5 * dU return predicted_cost, predicted_kl def compute_costs(self, m, eta): """ Compute cost estimates used in the LQR backward pass. """ traj_info, traj_distr = self.cur[m].traj_info, self.cur[m].traj_distr pol_info = self.cur[m].pol_info T, dU, dX = traj_distr.T, traj_distr.dU, traj_distr.dX Cm, cv = np.copy(traj_info.Cm), np.copy(traj_info.cv) # Modify policy action via Lagrange multiplier. cv[:, dX:] -= pol_info.lambda_k Cm[:, dX:, :dX] -= pol_info.lambda_K Cm[:, :dX, dX:] -= np.transpose(pol_info.lambda_K, [0, 2, 1]) #Pre-process the costs with KL-divergence terms. TKLm = np.zeros((T, dX+dU, dX+dU)) TKLv = np.zeros((T, dX+dU)) PKLm = np.zeros((T, dX+dU, dX+dU)) PKLv = np.zeros((T, dX+dU)) fCm, fcv = np.zeros(Cm.shape), np.zeros(cv.shape) for t in range(T): K, k = traj_distr.K[t, :, :], traj_distr.k[t, :] inv_pol_covar = traj_distr.inv_pol_covar[t, :, :] # Trajectory KL-divergence terms. TKLm[t, :, :] = np.vstack([ np.hstack([ K.T.dot(inv_pol_covar).dot(K), -K.T.dot(inv_pol_covar)]), np.hstack([-inv_pol_covar.dot(K), inv_pol_covar]) ]) TKLv[t, :] = np.concatenate([ K.T.dot(inv_pol_covar).dot(k), -inv_pol_covar.dot(k) ]) # Policy KL-divergence terms. inv_pol_S = np.linalg.solve( pol_info.chol_pol_S[t, :, :], np.linalg.solve(pol_info.chol_pol_S[t, :, :].T, np.eye(dU)) ) KB, kB = pol_info.pol_K[t, :, :], pol_info.pol_k[t, :] PKLm[t, :, :] = np.vstack([ np.hstack([KB.T.dot(inv_pol_S).dot(KB), -KB.T.dot(inv_pol_S)]), np.hstack([-inv_pol_S.dot(KB), inv_pol_S]) ]) PKLv[t, :] = np.concatenate([ KB.T.dot(inv_pol_S).dot(kB), -inv_pol_S.dot(kB) ]) wt = pol_info.pol_wt[t] fCm[t, :, :] = (Cm[t, :, :] + TKLm[t, :, :] * eta + PKLm[t, :, :] * wt) / (eta + wt) fcv[t, :] = (cv[t, :] + TKLv[t, :] * eta + PKLv[t, :] * wt) / (eta + wt) return fCm, fcv def _set_new_mult(self, predicted_impr, actual_impr, m): """ Adjust step size multiplier according to the predicted versus actual improvement. """ # Model improvement as I = predicted_dI * KL + penalty * KL^2, # where predicted_dI = pred/KL and penalty = (act-pred)/(KL^2). # Optimize I w.r.t. KL: 0 = predicted_dI + 2 * penalty * KL => # KL' = (-predicted_dI)/(2*penalty) = (pred/2*(pred-act)) * KL. # Therefore, the new multiplier is given by pred/2*(pred-act). new_mult = predicted_impr / (2.0 * max(1e-4, predicted_impr - actual_impr)) new_mult = max(0.1, min(5.0, new_mult)) new_step = max( min(new_mult * self.cur[m].step_mult, self._hyperparams['max_step_mult']), self._hyperparams['min_step_mult'] ) self.cur[m].step_mult = new_step if new_mult > 1: LOGGER.debug('Increasing step size multiplier to %f', new_step) else: LOGGER.debug('Decreasing step size multiplier to %f', new_step) def _measure_ent(self, m): """ Measure the entropy of the current trajectory. """ ent = 0 for t in range(self.T): ent = ent + np.sum( np.log(np.diag(self.cur[m].traj_distr.chol_pol_covar[t, :, :])) ) return ent def __getstate__(self): return {k: v for k, v in self.__dict__.iteritems() if (k != "_hyperparams" and k != "agent")} lto_code/python/gps/algorithm/dynamics/0000755000175000017500000000000013124341100016322 5ustar kekelto_code/python/gps/algorithm/dynamics/dynamics_lr_prior.py0000644000175000017500000000456613124341100022426 0ustar keke""" This file defines linear regression with an arbitrary prior. """ import numpy as np from gps.algorithm.algorithm_utils import gauss_fit_joint_prior class DynamicsLRPrior(object): """ Dynamics with linear regression, with arbitrary prior. """ def __init__(self, hyperparams): self._hyperparams = hyperparams # Fitted dynamics: x_t+1 = Fm * [x_t;u_t] + fv. self.Fm = np.array(np.nan) self.fv = np.array(np.nan) self.dyn_covar = np.array(np.nan) # Covariance. self.Fm = None self.fv = None self.dyn_covar = None self.prior = \ self._hyperparams['prior']['type'](self._hyperparams['prior']) def update_prior(self, samples): """ Update dynamics prior. """ X = samples.get_X() U = samples.get_U() self.prior.update(X, U) def get_prior(self): """ Return the dynamics prior. """ return self.prior def fit(self, X, U): """ Fit dynamics. """ N, T, dX = X.shape dU = U.shape[2] if N == 1: raise ValueError("Cannot fit dynamics on 1 sample") self.Fm = np.zeros([T, dX, dX+dU]) self.fv = np.zeros([T, dX]) self.dyn_covar = np.zeros([T, dX, dX]) it = slice(dX+dU) ip = slice(dX+dU, dX+dU+dX) # Fit dynamics with least squares regression. dwts = (1.0 / N) * np.ones(N) for t in range(T - 1): Ys = np.c_[X[:, t, :], U[:, t, :], X[:, t+1, :]] # Obtain Normal-inverse-Wishart prior. mu0, Phi, mm, n0 = self.prior.eval(dX, dU, Ys) sig_reg = np.zeros((dX+dU+dX, dX+dU+dX)) sig_reg[it, it] = self._hyperparams['regularization']*np.eye(dX+dU) Fm, fv, dyn_covar = gauss_fit_joint_prior(Ys, mu0, Phi, mm, n0, dwts, dX+dU, dX, sig_reg, self._hyperparams['clipping_thresh']) self.Fm[t, :, :] = Fm self.fv[t, :] = fv # Fm * [x; u] + fv gives the predicted state self.dyn_covar[t, :, :] = dyn_covar return self.Fm, self.fv, self.dyn_covar def copy(self): """ Return a copy of the dynamics estimate. """ dyn = type(self)(self._hyperparams) dyn.Fm = np.copy(self.Fm) dyn.fv = np.copy(self.fv) dyn.dyn_covar = np.copy(self.dyn_covar) return dyn lto_code/python/gps/algorithm/dynamics/config.py0000644000175000017500000000033513124341100020142 0ustar keke""" Default configuration and hyperparameter values for dynamics objects. """ # DynamicsPriorGMM DYN_PRIOR_GMM = { 'min_samples_per_cluster': 20, 'max_clusters': 50, 'max_samples': 20, 'strength': 1.0, } lto_code/python/gps/algorithm/dynamics/dynamics_prior_gmm.py0000644000175000017500000000715313124341100022564 0ustar keke""" This file defines the GMM prior for dynamics estimation. """ import copy import logging import numpy as np from gps.algorithm.dynamics.config import DYN_PRIOR_GMM from gps.utility.gmm import GMM LOGGER = logging.getLogger(__name__) class DynamicsPriorGMM(object): """ A dynamics prior encoded as a GMM over [x_t, u_t, x_t+1] points. See: S. Levine*, C. Finn*, T. Darrell, P. Abbeel, "End-to-end training of Deep Visuomotor Policies", arXiv:1504.00702, Appendix A.3. """ def __init__(self, hyperparams): """ Hyperparameters: min_samples_per_cluster: Minimum samples per cluster. max_clusters: Maximum number of clusters to fit. max_samples: Maximum number of trajectories to use for fitting the GMM at any given time. strength: Adjusts the strength of the prior. """ config = copy.deepcopy(DYN_PRIOR_GMM) config.update(hyperparams) self._hyperparams = config self.X = None self.U = None self.gmm = GMM() self._min_samp = self._hyperparams['min_samples_per_cluster'] self._max_samples = self._hyperparams['max_samples'] self._max_clusters = self._hyperparams['max_clusters'] self._strength = self._hyperparams['strength'] def initial_state(self): """ Return dynamics prior for initial time step. """ # Compute mean and covariance. mu0 = np.mean(self.X[:, 0, :], axis=0) Phi = np.diag(np.var(self.X[:, 0, :], axis=0)) # Factor in multiplier. n0 = self.X.shape[2] * self._strength m = self.X.shape[2] * self._strength # Multiply Phi by m (since it was normalized before). Phi = Phi * m return mu0, Phi, m, n0 def update(self, X, U): """ Update prior with additional data. Args: X: A N x T x dX matrix of sequential state data. U: A N x T x dU matrix of sequential control data. """ # Constants. T = X.shape[1] - 1 # Append data to dataset. if self.X is None: self.X = X else: self.X = np.concatenate([self.X, X], axis=0) if self.U is None: self.U = U else: self.U = np.concatenate([self.U, U], axis=0) # Remove excess samples from dataset. start = max(0, self.X.shape[0] - self._max_samples + 1) self.X = self.X[start:, :] self.U = self.U[start:, :] # Compute cluster dimensionality. Do = X.shape[2] + U.shape[2] + X.shape[2] # Create dataset. N = self.X.shape[0] xux = np.reshape( np.c_[self.X[:, :T, :], self.U[:, :T, :], self.X[:, 1:(T+1), :]], [T * N, Do] ) # Choose number of clusters. K = int(max(2, min(self._max_clusters, np.floor(float(N * T) / self._min_samp)))) LOGGER.debug('Generating %d clusters for dynamics GMM.', K) # Update GMM. self.gmm.update(xux, K) def eval(self, Dx, Du, pts): """ Evaluate prior. Args: pts: A N x Dx+Du+Dx matrix. """ # Construct query data point by rearranging entries and adding # in reference. assert pts.shape[1] == Dx + Du + Dx # Perform query and fix mean. mu0, Phi, m, n0 = self.gmm.inference(pts) # Factor in multiplier. n0 = n0 * self._strength m = m * self._strength # Multiply Phi by m (since it was normalized before). Phi *= m return mu0, Phi, m, n0 lto_code/python/gps/algorithm/dynamics/__init__.py0000644000175000017500000000000013124341100020421 0ustar kekelto_code/python/gps/algorithm/algorithm_utils.py0000644000175000017500000001216613124341100020301 0ustar keke""" This file defines utility classes and functions for algorithms. """ import numpy as np from gps.utility.general_utils import BundleType from gps.algorithm.policy.lin_gauss_policy import LinearGaussianPolicy class IterationData(BundleType): """ Collection of iteration variables. """ def __init__(self): variables = { 'sample_list': None, # List of samples for the current iteration. 'traj_info': None, # Current TrajectoryInfo object. 'pol_info': None, # Current PolicyInfo object. 'traj_distr': None, # Initial trajectory distribution. 'new_traj_distr': None, # Updated trajectory distribution. 'cs': None, # Sample costs of the current iteration. 'step_mult': 1.0, # KL step multiplier for the current iteration. 'eta': 1.0, # Dual variable used in LQR backward pass. } BundleType.__init__(self, variables) class TrajectoryInfo(BundleType): """ Collection of trajectory-related variables. """ def __init__(self): variables = { 'dynamics': None, # Dynamics object for the current iteration. 'x0mu': None, # Mean for the initial state, used by the dynamics. 'x0sigma': None, # Covariance for the initial state distribution. 'cc': None, # Cost estimate constant term. 'cv': None, # Cost estimate vector term. 'Cm': None, # Cost estimate matrix term. 'last_kl_step': float('inf'), # KL step of the previous iteration. } BundleType.__init__(self, variables) class PolicyInfo(BundleType): """ Collection of policy-related variables. """ def __init__(self, hyperparams): T, dU, dX = hyperparams['T'], hyperparams['dU'], hyperparams['dX'] variables = { 'lambda_k': np.zeros((T, dU)), # Dual variables. 'lambda_K': np.zeros((T, dU, dX)), # Dual variables. 'pol_wt': hyperparams['init_pol_wt'] * np.ones(T), # Policy weight. #'pol_mu': None, # Mean of the current policy output. #'pol_sig': None, # Covariance of the current policy output. 'pol_K': np.zeros((T, dU, dX)), # Policy linearization. 'pol_k': np.zeros((T, dU)), # Policy linearization. 'pol_S': np.zeros((T, dU, dU)), # Policy linearization covariance. 'chol_pol_S': np.zeros((T, dU, dU)), # Cholesky decomp of covar. 'prev_kl': None, # Previous KL divergence. 'init_kl': None, # The initial KL divergence, before the iteration. 'policy_samples': [], # List of current policy samples. 'policy_prior': None, # Current prior for policy linearization. } BundleType.__init__(self, variables) def traj_distr(self): """ Create a trajectory distribution object from policy info. """ T, dU, dX = self.pol_K.shape # Compute inverse policy covariances. inv_pol_S = np.empty_like(self.chol_pol_S) for t in range(T): inv_pol_S[t, :, :] = np.linalg.solve( self.chol_pol_S[t, :, :], np.linalg.solve(self.chol_pol_S[t, :, :].T, np.eye(dU)) ) return LinearGaussianPolicy(self.pol_K, self.pol_k, self.pol_S, self.chol_pol_S, inv_pol_S) def estimate_moments(X, mu, covar): """ Estimate the moments for a given linearized policy. """ N, T, dX = X.shape dU = mu.shape[-1] if len(covar.shape) == 3: covar = np.tile(covar, [N, 1, 1, 1]) Xmu = np.concatenate([X, mu], axis=2) ev = np.mean(Xmu, axis=0) em = np.zeros((N, T, dX+dU, dX+dU)) pad1 = np.zeros((dX, dX+dU)) pad2 = np.zeros((dU, dX)) for n in range(N): for t in range(T): covar_pad = np.vstack([pad1, np.hstack([pad2, covar[n, t, :, :]])]) em[n, t, :, :] = np.outer(Xmu[n, t, :], Xmu[n, t, :]) + covar_pad return ev, em def gauss_fit_joint_prior(pts, mu0, Phi, m, n0, dwts, dX, dU, sig_reg, clipping_thresh = None): """ Perform Gaussian fit to data with a prior. """ # Build weights matrix. #D = np.diag(dwts) # Compute empirical mean and covariance. mun = np.sum((pts.T * dwts).T, axis=0) diff = pts - mun #empsig = diff.T.dot(D).dot(diff) empsig = (diff.T * dwts).dot(diff) empsig = 0.5 * (empsig + empsig.T) # MAP estimate of joint distribution. N = dwts.shape[0] mu = mun sigma = (N * empsig + Phi + (N * m) / (N + m) * np.outer(mun - mu0, mun - mu0)) / (N + n0) sigma = 0.5 * (sigma + sigma.T) # Add sigma regularization. sigma += sig_reg # Conditioning to get dynamics. fd = np.linalg.solve(sigma[:dX, :dX], sigma[:dX, dX:dX+dU]).T ori_fd = fd if clipping_thresh is not None: fd = np.maximum(np.minimum(fd, clipping_thresh), -clipping_thresh) fc = mu[dX:dX+dU] - fd.dot(mu[:dX]) #dynsig = sigma[dX:dX+dU, dX:dX+dU] - ori_fd.dot(sigma[:dX, :dX]).dot(ori_fd.T) dynsig = sigma[dX:dX+dU, dX:dX+dU] - ori_fd.dot(sigma[:dX, dX:dX+dU]) # Mathematically equivalent to the above dynsig = 0.5 * (dynsig + dynsig.T) return fd, fc, dynsig lto_code/python/gps/algorithm/traj_opt/0000755000175000017500000000000013124341100016335 5ustar kekelto_code/python/gps/algorithm/traj_opt/traj_opt_utils.py0000644000175000017500000000567313124341100021764 0ustar keke""" This file defines utilities for trajectory optimization. """ import numpy as np import scipy as sp # Constants used in TrajOptLQR. DGD_MAX_ITER = 50 def traj_distr_kl(new_mu, new_sigma, new_traj_distr, prev_traj_distr): """ Compute KL divergence between new and previous trajectory distributions. Args: new_mu: T x dX, mean of new trajectory distribution. new_sigma: T x dX x dX, variance of new trajectory distribution. new_traj_distr: A linear Gaussian policy object, new distribution. prev_traj_distr: A linear Gaussian policy object, previous distribution. Returns: kl_div: The KL divergence between the new and previous trajectories. """ # Constants. T = new_mu.shape[0] dU = new_traj_distr.dU # Initialize vector of divergences for each time step. kl_div = np.zeros(T) # Step through trajectory. for t in range(T): # Fetch matrices and vectors from trajectory distributions. mu_t = new_mu[t, :] sigma_t = new_sigma[t, :, :] K_prev = prev_traj_distr.K[t, :, :] K_new = new_traj_distr.K[t, :, :] k_prev = prev_traj_distr.k[t, :] k_new = new_traj_distr.k[t, :] chol_prev = prev_traj_distr.chol_pol_covar[t, :, :] chol_new = new_traj_distr.chol_pol_covar[t, :, :] # Compute log determinants and precision matrices. logdet_prev = 2 * sum(np.log(np.diag(chol_prev))) logdet_new = 2 * sum(np.log(np.diag(chol_new))) prc_prev = sp.linalg.solve_triangular( chol_prev, sp.linalg.solve_triangular(chol_prev.T, np.eye(dU), lower=True) ) prc_new = sp.linalg.solve_triangular( chol_new, sp.linalg.solve_triangular(chol_new.T, np.eye(dU), lower=True) ) # Construct matrix, vector, and constants. M_prev = np.r_[ np.c_[K_prev.T.dot(prc_prev).dot(K_prev), -K_prev.T.dot(prc_prev)], np.c_[-prc_prev.dot(K_prev), prc_prev] ] M_new = np.r_[ np.c_[K_new.T.dot(prc_new).dot(K_new), -K_new.T.dot(prc_new)], np.c_[-prc_new.dot(K_new), prc_new] ] v_prev = np.r_[K_prev.T.dot(prc_prev).dot(k_prev), -prc_prev.dot(k_prev)] v_new = np.r_[K_new.T.dot(prc_new).dot(k_new), -prc_new.dot(k_new)] c_prev = 0.5 * k_prev.T.dot(prc_prev).dot(k_prev) c_new = 0.5 * k_new.T.dot(prc_new).dot(k_new) # Compute KL divergence at timestep t. kl_div[t] = max( 0, -0.5 * mu_t.T.dot(M_new - M_prev).dot(mu_t) - mu_t.T.dot(v_new - v_prev) - c_new + c_prev - 0.5 * np.sum(sigma_t * (M_new-M_prev)) - 0.5 * logdet_new + 0.5 * logdet_prev ) # Add up divergences across time to get total divergence. return np.sum(kl_div) lto_code/python/gps/algorithm/traj_opt/config.py0000644000175000017500000000027213124341100020155 0ustar keke""" Default configuration for trajectory optimization. """ TRAJ_OPT = { # Dual variable updates for non-PD Q-function. 'del0': 1e-4, 'min_eta': 1e-4, 'max_eta': 1e16, } lto_code/python/gps/algorithm/traj_opt/traj_opt.py0000644000175000017500000002461013124341100020534 0ustar keke""" This file defines code for iLQG-based trajectory optimization. """ import logging import copy import numpy as np from numpy.linalg import LinAlgError import scipy as sp from gps.algorithm.traj_opt.config import TRAJ_OPT from gps.algorithm.traj_opt.traj_opt_utils import traj_distr_kl, DGD_MAX_ITER LOGGER = logging.getLogger(__name__) class TrajOpt(object): """ LQR trajectory optimization """ def __init__(self, hyperparams): config = copy.deepcopy(TRAJ_OPT) config.update(hyperparams) self._hyperparams = config def update(self, m, algorithm): """ Run dual gradient decent to optimize trajectories. """ T = algorithm.T eta = algorithm.cur[m].eta step_mult = algorithm.cur[m].step_mult traj_info = algorithm.cur[m].traj_info prev_traj_distr = algorithm.cur[m].traj_distr # Set KL-divergence step size (epsilon). kl_step = T * algorithm.base_kl_step * step_mult # We assume at min_eta, kl_div > kl_step, opposite for max_eta. min_eta = self._hyperparams['min_eta'] max_eta = self._hyperparams['max_eta'] LOGGER.debug("Running DGD for trajectory %d, eta: %f", m, eta) for itr in range(DGD_MAX_ITER): LOGGER.debug("Iteration %i, bracket: (%.2e , %.2e , %.2e)", itr, min_eta, eta, max_eta) # Run fwd/bwd pass, note that eta may be updated. # NOTE: we can just ignore case when the new eta is larger. traj_distr, eta = self.backward(prev_traj_distr, traj_info, eta, algorithm, m) new_mu, new_sigma = self.forward(traj_distr, traj_info) # Compute KL divergence constraint violation. kl_div = traj_distr_kl(new_mu, new_sigma, traj_distr, prev_traj_distr) con = kl_div - kl_step # Convergence check - constraint satisfaction. if (abs(con) < 0.1*kl_step): LOGGER.debug("KL: %f / %f, converged iteration %i", kl_div, kl_step, itr) break # Choose new eta (bisect bracket or multiply by constant) if con < 0: # Eta was too big. max_eta = eta geom = np.sqrt(min_eta*max_eta) # Geometric mean. new_eta = max(geom, 0.1*max_eta) LOGGER.debug("KL: %f / %f, eta too big, new eta: %f", kl_div, kl_step, new_eta) else: # Eta was too small. min_eta = eta geom = np.sqrt(min_eta*max_eta) # Geometric mean. new_eta = min(geom, 10.0*min_eta) LOGGER.debug("KL: %f / %f, eta too small, new eta: %f", kl_div, kl_step, new_eta) # Logarithmic mean: log_mean(x,y) = (y - x)/(log(y) - log(x)) eta = new_eta if kl_div > kl_step and abs(kl_div - kl_step) > 0.1*kl_step: LOGGER.warning( "Final KL divergence after DGD convergence is too high." ) return traj_distr, eta def estimate_cost(self, traj_distr, traj_info): """ Compute Laplace approximation to expected cost. """ # Constants. T = traj_distr.T # Perform forward pass (note that we repeat this here, because # traj_info may have different dynamics from the ones that were # used to compute the distribution already saved in traj). mu, sigma = self.forward(traj_distr, traj_info) # Compute cost. predicted_cost = np.zeros(T) for t in range(T): predicted_cost[t] = traj_info.cc[t] + 0.5 * \ np.sum(sigma[t, :, :] * traj_info.Cm[t, :, :]) + 0.5 * \ mu[t, :].T.dot(traj_info.Cm[t, :, :]).dot(mu[t, :]) + \ mu[t, :].T.dot(traj_info.cv[t, :]) return predicted_cost def forward(self, traj_distr, traj_info): """ Perform LQR forward pass. Computes state-action marginals from dynamics and policy. Args: traj_distr: A linear Gaussian policy object. traj_info: A TrajectoryInfo object. Returns: mu: A T x dX mean action vector. sigma: A T x dX x dX covariance matrix. """ # Compute state-action marginals from specified conditional # parameters and current traj_info. T = traj_distr.T dU = traj_distr.dU dX = traj_distr.dX # Constants. idx_x = slice(dX) # Allocate space. sigma = np.zeros((T, dX+dU, dX+dU)) mu = np.zeros((T, dX+dU)) # Pull out dynamics. Fm = traj_info.dynamics.Fm fv = traj_info.dynamics.fv dyn_covar = traj_info.dynamics.dyn_covar # Set initial covariance (initial mu is always zero). sigma[0, idx_x, idx_x] = traj_info.x0sigma mu[0, idx_x] = traj_info.x0mu for t in range(T): sigma[t, :, :] = np.vstack([ np.hstack([ sigma[t, idx_x, idx_x], sigma[t, idx_x, idx_x].dot(traj_distr.K[t, :, :].T) ]), np.hstack([ traj_distr.K[t, :, :].dot(sigma[t, idx_x, idx_x]), traj_distr.K[t, :, :].dot(sigma[t, idx_x, idx_x]).dot( traj_distr.K[t, :, :].T ) + traj_distr.pol_covar[t, :, :] ]) ]) mu[t, :] = np.hstack([ mu[t, idx_x], traj_distr.K[t, :, :].dot(mu[t, idx_x]) + traj_distr.k[t, :] ]) if t < T - 1: sigma[t+1, idx_x, idx_x] = \ Fm[t, :, :].dot(sigma[t, :, :]).dot(Fm[t, :, :].T) + \ dyn_covar[t, :, :] mu[t+1, idx_x] = Fm[t, :, :].dot(mu[t, :]) + fv[t, :] return mu, sigma def backward(self, prev_traj_distr, traj_info, eta, algorithm, m): """ Perform LQR backward pass. This computes a new linear Gaussian policy object. Args: prev_traj_distr: A linear Gaussian policy object from previous iteration. traj_info: A TrajectoryInfo object. eta: Dual variable. algorithm: Algorithm object needed to compute costs. m: Condition number. Returns: traj_distr: A new linear Gaussian policy. new_eta: The updated dual variable. Updates happen if the Q-function is not PD. """ # Constants. T = prev_traj_distr.T dU = prev_traj_distr.dU dX = prev_traj_distr.dX traj_distr = prev_traj_distr.nans_like() pol_wt = algorithm.cur[m].pol_info.pol_wt idx_x = slice(dX) idx_u = slice(dX, dX+dU) # Pull out dynamics. Fm = traj_info.dynamics.Fm fv = traj_info.dynamics.fv # Non-SPD correction terms. del_ = self._hyperparams['del0'] eta0 = eta # Run dynamic programming. fail = True while fail: fail = False # Flip to true on non-symmetric PD. # Allocate. Vxx = np.zeros((T, dX, dX)) Vx = np.zeros((T, dX)) fCm, fcv = algorithm.compute_costs(m, eta) # Compute state-action-state function at each time step. for t in range(T - 1, -1, -1): # Add in the cost. Qtt = fCm[t, :, :] # (X+U) x (X+U) Qt = fcv[t, :] # (X+U) x 1 # Add in the value function from the next time step. if t < T - 1: multiplier = (pol_wt[t+1] + eta)/(pol_wt[t] + eta) Qtt = Qtt + multiplier * \ Fm[t, :, :].T.dot(Vxx[t+1, :, :]).dot(Fm[t, :, :]) Qt = Qt + multiplier * \ Fm[t, :, :].T.dot(Vx[t+1, :] + Vxx[t+1, :, :].dot(fv[t, :])) # Symmetrize quadratic component. Qtt = 0.5 * (Qtt + Qtt.T) # Compute Cholesky decomposition of Q function action # component. try: U = sp.linalg.cholesky(Qtt[idx_u, idx_u]) L = U.T except LinAlgError as e: # Error thrown when Qtt[idx_u, idx_u] is not # symmetric positive definite. LOGGER.debug('LinAlgError: %s', e) fail = True break # Store conditional covariance, inverse, and Cholesky. traj_distr.inv_pol_covar[t, :, :] = Qtt[idx_u, idx_u] traj_distr.pol_covar[t, :, :] = sp.linalg.solve_triangular( U, sp.linalg.solve_triangular(L, np.eye(dU), lower=True) ) traj_distr.chol_pol_covar[t, :, :] = sp.linalg.cholesky( traj_distr.pol_covar[t, :, :] ) # Compute mean terms. traj_distr.k[t, :] = -sp.linalg.solve_triangular( U, sp.linalg.solve_triangular(L, Qt[idx_u], lower=True) ) traj_distr.K[t, :, :] = -sp.linalg.solve_triangular( U, sp.linalg.solve_triangular(L, Qtt[idx_u, idx_x], lower=True) ) # Compute value function. Vxx[t, :, :] = Qtt[idx_x, idx_x] + \ Qtt[idx_x, idx_u].dot(traj_distr.K[t, :, :]) Vx[t, :] = Qt[idx_x] + Qtt[idx_x, idx_u].dot(traj_distr.k[t, :]) Vxx[t, :, :] = 0.5 * (Vxx[t, :, :] + Vxx[t, :, :].T) # Increment eta on non-SPD Q-function. if fail: old_eta = eta eta = eta0 + del_ LOGGER.debug('Increasing eta: %f -> %f', old_eta, eta) del_ *= 2 # Increase del_ exponentially on failure. if eta >= 1e16: if np.any(np.isnan(Fm)) or np.any(np.isnan(fv)): raise ValueError('NaNs encountered in dynamics!') raise ValueError('Failed to find PD solution even for very \ large eta (check that dynamics and cost are \ reasonably well conditioned)!') return traj_distr, eta lto_code/python/gps/algorithm/traj_opt/__init__.py0000644000175000017500000000000013124341100020434 0ustar kekelto_code/python/gps/algorithm/policy_opt/0000755000175000017500000000000013124341100016674 5ustar kekelto_code/python/gps/algorithm/policy_opt/policy_opt.py0000644000175000017500000001764313124341100021442 0ustar keke""" This file defines policy optimization for a tensorflow policy. """ import copy import logging import numpy as np # NOTE: Order of these imports matters for some reason. # Changing it can lead to segmentation faults on some machines. from gps.algorithm.policy_opt.config import POLICY_OPT import tensorflow as tf from gps.algorithm.policy.tf_policy import TfPolicy from gps.algorithm.policy_opt.tf_utils import TfSolver LOGGER = logging.getLogger(__name__) class PolicyOpt(object): """ Policy optimization using tensor flow for DAG computations/nonlinear function approximation. """ def __init__(self, hyperparams, dO, dU): config = copy.deepcopy(POLICY_OPT) config.update(hyperparams) self._hyperparams = config self._dO = dO self._dU = dU tf.set_random_seed(self._hyperparams['random_seed']) self.tf_iter = 0 self.batch_size = self._hyperparams['batch_size'] self.device_string = "/cpu:0" if self._hyperparams['use_gpu'] == 1: self.gpu_device = self._hyperparams['gpu_id'] self.device_string = "/gpu:" + str(self.gpu_device) self.act_op = None # mu_hat self.loss_scalar = None self.obs_tensor = None self.precision_tensor = None self.action_tensor = None # mu true self.solver = None self.init_network() self.init_solver() self.var = self._hyperparams['init_var'] * np.ones(dU) self.sess = tf.Session() self.policy = TfPolicy(dU, self.obs_tensor, self.act_op, np.zeros(dU), self.sess, self.device_string) init_op = tf.initialize_all_variables() self.sess.run(init_op) def init_network(self): """ Helper method to initialize the tf networks used """ tf_map_generator = self._hyperparams['network_model'] tf_map = tf_map_generator(dim_input=self._dO, dim_output=self._dU, batch_size=self.batch_size, network_config=self._hyperparams['network_params']) self.obs_tensor = tf_map.get_input_tensor() self.action_tensor = tf_map.get_target_output_tensor() self.precision_tensor = tf_map.get_precision_tensor() self.act_op = tf_map.get_output_op() self.loss_scalar = tf_map.get_loss_op() def init_solver(self): """ Helper method to initialize the solver. """ self.solver = TfSolver(loss_scalar=self.loss_scalar, solver_name=self._hyperparams['solver_type'], base_lr=self._hyperparams['lr'], lr_policy=self._hyperparams['lr_policy'], momentum=self._hyperparams['momentum'], momentum2=self._hyperparams['momentum2'], epsilon=self._hyperparams['epsilon'], weight_decay=self._hyperparams['weight_decay']) def update(self, obs, tgt_mu, tgt_prc, tgt_wt): """ Update policy. Args: obs: Numpy array of observations, N x T x dO. tgt_mu: Numpy array of mean controller outputs, N x T x dU. tgt_prc: Numpy array of precision matrices, N x T x dU x dU. tgt_wt: Numpy array of weights, N x T. Returns: A tensorflow object with updated weights. """ N, T = obs.shape[:2] dU, dO = self._dU, self._dO # Renormalize weights. tgt_wt *= (float(N * T) / np.sum(tgt_wt)) # Allow weights to be at most twice the robust median. mn = np.median(tgt_wt[(tgt_wt > 1e-2).nonzero()]) for n in range(N): for t in range(T): tgt_wt[n, t] = min(tgt_wt[n, t], 2 * mn) # Robust median should be around one. tgt_wt /= mn # Reshape inputs. obs = np.reshape(obs, (N*T, dO)) tgt_mu = np.reshape(tgt_mu, (N*T, dU)) tgt_prc = np.reshape(tgt_prc, (N*T, dU, dU)) tgt_wt = np.reshape(tgt_wt, (N*T, 1, 1)) # Fold weights into tgt_prc. tgt_prc = tgt_wt * tgt_prc # Normalize obs, but only compute normalzation at the beginning. if self.policy.scale is None or self.policy.bias is None: # 1e-3 to avoid infs if some state dimensions don't change in the # first batch of samples self.policy.scale = np.diag( 1.0 / np.maximum(np.std(obs, axis=0), 1e-3)) self.policy.bias = - np.mean( obs.dot(self.policy.scale), axis=0) obs = obs.dot(self.policy.scale) + self.policy.bias # Assuming that N*T >= self.batch_size. batches_per_epoch = np.floor(N*T / self.batch_size) idx = range(N*T) average_loss = 0 np.random.shuffle(idx) # actual training. for i in range(self._hyperparams['iterations']): # Load in data for this batch. start_idx = int(i * self.batch_size % (batches_per_epoch * self.batch_size)) idx_i = idx[start_idx:start_idx+self.batch_size] feed_dict = {self.obs_tensor: obs[idx_i], self.action_tensor: tgt_mu[idx_i], self.precision_tensor: tgt_prc[idx_i]} train_loss = self.solver(feed_dict, self.sess) average_loss += train_loss if (i+1) % 500 == 0: LOGGER.debug('tensorflow iteration %d, average loss %f', i+1, average_loss / 500) print ('supervised tf loss is ' + str(average_loss)) average_loss = 0 # Keep track of tensorflow iterations for loading solver states. self.tf_iter += self._hyperparams['iterations'] # Optimize variance. self.var = (np.sum(tgt_wt,axis=0)[:,0] - 2*N*T*self._hyperparams['ent_reg']) / np.sum(np.diagonal(tgt_prc, axis1=1, axis2=2),axis=0) self.policy.chol_pol_covar = np.diag(np.sqrt(self.var)) return self.policy def prob(self, obs, diag_var = False): """ Run policy forward. Args: obs: Numpy array of observations that is N x T x dO. """ dU = self._dU N, T = obs.shape[:2] output = np.zeros((N, T, dU)) for i in range(N): for t in range(T): # Feed in data. if self.policy.scale is not None: feed_dict = {self.obs_tensor: np.expand_dims(obs[i, t], axis=0).dot(self.policy.scale) + self.policy.bias} else: feed_dict = {self.obs_tensor: np.expand_dims(obs[i, t], axis=0)} with tf.device(self.device_string): output[i, t, :] = self.sess.run(self.act_op, feed_dict=feed_dict) if diag_var: pol_sigma = np.tile(self.var, [N, T, 1]) pol_prec = np.tile(1.0 / self.var, [N, T, 1]) pol_det_sigma = np.tile(np.prod(self.var), [N, T]) else: pol_sigma = np.tile(np.diag(self.var), [N, T, 1, 1]) pol_prec = np.tile(np.diag(1.0 / self.var), [N, T, 1, 1]) pol_det_sigma = np.tile(np.prod(self.var), [N, T]) return output, pol_sigma, pol_prec, pol_det_sigma def set_ent_reg(self, ent_reg): """ Set the entropy regularization. """ self._hyperparams['ent_reg'] = ent_reg # For pickling. def __getstate__(self): return { 'hyperparams': self._hyperparams, 'dO': self._dO, 'dU': self._dU, 'scale': self.policy.scale, 'bias': self.policy.bias, 'tf_iter': self.tf_iter, } # For unpickling. def __setstate__(self, state): self.__init__(state['hyperparams'], state['dO'], state['dU']) self.policy.scale = state['scale'] self.policy.bias = state['bias'] self.tf_iter = state['tf_iter'] lto_code/python/gps/algorithm/policy_opt/lto_model.py0000644000175000017500000000540213124341100021225 0ustar kekeimport tensorflow as tf from gps.algorithm.policy_opt.tf_utils import TfMap import numpy as np def init_weights(shape, name=None): return tf.Variable(tf.random_normal(shape, stddev=0.01), name=name) def init_bias(shape, name=None): return tf.Variable(tf.zeros(shape, dtype='float'), name=name) def batched_matrix_vector_multiply(vector, matrix): """ computes x^T A in mini-batches. """ vector_batch_as_matricies = tf.expand_dims(vector, [1]) mult_result = tf.batch_matmul(vector_batch_as_matricies, matrix) squeezed_result = tf.squeeze(mult_result, [1]) return squeezed_result def get_input_layer(): """produce the placeholder inputs that are used to run ops forward and backwards. net_input: usually an observation. action: mu, the ground truth actions we're trying to learn. precision: precision matrix used to compute loss.""" net_input = tf.placeholder("float", [None, None], name='nn_input') # (N*T) x dO action = tf.placeholder('float', [None, None], name='action') # (N*T) x dU precision = tf.placeholder('float', [None, None, None], name='precision') # (N*T) x dU x dU return net_input, action, precision def get_loss_layer(mlp_out, action, precision, batch_size): """The loss layer used for the MLP network is obtained through this class.""" scale_factor = tf.constant(2*batch_size, dtype='float') uP = batched_matrix_vector_multiply(action - mlp_out, precision) uPu = tf.reduce_sum(uP*(action - mlp_out)) # this last dot product is then summed, so we just the sum all at once. return uPu/scale_factor def fully_connected_tf_network(dim_input, dim_output, batch_size=25, network_config=None): dim_hidden = network_config['dim_hidden'] + [dim_output] n_layers = len(dim_hidden) nn_input, action, precision = get_input_layer() weights = [] biases = [] in_shape = dim_input for layer_step in range(0, n_layers): cur_weight = init_weights([in_shape, dim_hidden[layer_step]], name='w_' + str(layer_step)) cur_bias = init_bias([dim_hidden[layer_step]], name='b_' + str(layer_step)) in_shape = dim_hidden[layer_step] weights.append(cur_weight) biases.append(cur_bias) cur_top = nn_input for layer_step in range(0, n_layers): if layer_step != n_layers-1: # final layer has no RELU cur_top = tf.nn.relu(tf.matmul(cur_top, weights[layer_step]) + biases[layer_step]) else: cur_top = tf.matmul(cur_top, weights[layer_step]) + biases[layer_step] mlp_applied = cur_top loss_out = get_loss_layer(mlp_out=mlp_applied, action=action, precision=precision, batch_size=batch_size) return TfMap.init_from_lists([nn_input, action, precision], [mlp_applied], [loss_out]) lto_code/python/gps/algorithm/policy_opt/config.py0000644000175000017500000000134313124341100020514 0ustar keke""" Default configuration for policy optimization. """ import os POLICY_OPT = { # Initialization. 'init_var': 0.1, # Initial policy variance. 'ent_reg': 0.0, # Entropy regularizer. # Solver hyperparameters. 'iterations': 5000, # Number of iterations per inner iteration. 'batch_size': 25, 'lr': 0.001, # Base learning rate (by default it's fixed). 'lr_policy': 'fixed', # Learning rate policy. 'momentum': 0.9, # Momentum. 'momentum2': 0.999, 'epsilon': 1e-8, 'weight_decay': 0.005, # Weight decay. 'solver_type': 'Adam', # Solver type (e.g. 'SGD', 'Adam', etc.). # set gpu usage. 'use_gpu': 1, # Whether or not to use the GPU. 'gpu_id': 0, 'random_seed': 1 } lto_code/python/gps/algorithm/policy_opt/tf_utils.py0000644000175000017500000001043613124341100021103 0ustar kekeimport tensorflow as tf def check_list_and_convert(the_object): if isinstance(the_object, list): return the_object return [the_object] class TfMap: """ a container for inputs, outputs, and loss in a tf graph. This object exists only to make well-defined the tf inputs, outputs, and losses used in the policy_opt_tf class.""" def __init__(self, input_tensor, target_output_tensor, precision_tensor, output_op, loss_op): self.input_tensor = input_tensor self.target_output_tensor = target_output_tensor self.precision_tensor = precision_tensor self.output_op = output_op self.loss_op = loss_op @classmethod def init_from_lists(cls, inputs, outputs, loss): inputs = check_list_and_convert(inputs) outputs = check_list_and_convert(outputs) loss = check_list_and_convert(loss) if len(inputs) < 3: # pad for the constructor if needed. inputs += [None]*(3 - len(inputs)) return cls(inputs[0], inputs[1], inputs[2], outputs[0], loss[0]) def get_input_tensor(self): return self.input_tensor def set_input_tensor(self, input_tensor): self.input_tensor = input_tensor def get_target_output_tensor(self): return self.target_output_tensor def set_target_output_tensor(self, target_output_tensor): self.target_output_tensor = target_output_tensor def get_precision_tensor(self): return self.precision_tensor def set_precision_tensor(self, precision_tensor): self.precision_tensor = precision_tensor def get_output_op(self): return self.output_op def set_output_op(self, output_op): self.output_op = output_op def get_loss_op(self): return self.loss_op def set_loss_op(self, loss_op): self.loss_op = loss_op class TfSolver: """ A container for holding solver hyperparams in tensorflow. Used to execute backwards pass. """ def __init__(self, loss_scalar, solver_name='adam', base_lr=None, lr_policy=None, momentum=None, momentum2=None, epsilon=None, weight_decay=None): self.base_lr = base_lr self.lr_policy = lr_policy self.momentum = momentum self.momentum2 = momentum2 self.epsilon = epsilon self.solver_name = solver_name self.loss_scalar = loss_scalar if self.lr_policy != 'fixed': raise NotImplementedError('learning rate policies other than fixed are not implemented') self.weight_decay = weight_decay if weight_decay is not None: trainable_vars = tf.trainable_variables() loss_with_reg = self.loss_scalar for var in trainable_vars: loss_with_reg += self.weight_decay*tf.nn.l2_loss(var) self.loss_scalar = loss_with_reg self.solver_op = self.get_solver_op() def get_solver_op(self): solver_string = self.solver_name.lower() if solver_string == 'adam': return tf.train.AdamOptimizer(learning_rate=self.base_lr,beta1=self.momentum,beta2=self.momentum2,epsilon=self.epsilon).minimize(self.loss_scalar) elif solver_string == 'rmsprop': return tf.train.RMSPropOptimizer(learning_rate=self.base_lr,decay=self.momentum).minimize(self.loss_scalar) elif solver_string == 'momentum': return tf.train.MomentumOptimizer(learning_rate=self.base_lr,momentum=self.momentum).minimize(self.loss_scalar) elif solver_string == 'adagrad': return tf.train.AdagradOptimizer(learning_rate=self.base_lr,initial_accumulator_value=self.momentum).minimize(self.loss_scalar) elif solver_string == 'sgd': return tf.train.GradientDescentOptimizer(learning_rate=self.base_lr).minimize(self.loss_scalar) else: raise NotImplementedError("Please select a valid optimizer.") def __call__(self, feed_dict, sess, device_string="/cpu:0", additional_tensors = None): if additional_tensors is None: with tf.device(device_string): loss = sess.run([self.loss_scalar, self.solver_op], feed_dict) return loss[0] else: with tf.device(device_string): loss = sess.run([self.loss_scalar] + additional_tensors + [self.solver_op], feed_dict) return loss[:-1] lto_code/python/gps/algorithm/policy_opt/__init__.py0000644000175000017500000000000013124341100020773 0ustar kekelto_code/python/gps/algorithm/config.py0000644000175000017500000000176613124341100016344 0ustar keke""" Default configuration and hyperparameter values for algorithms. """ # Algorithm ALG = { 'inner_iterations': 4, 'min_eta': 1e-5, # Minimum initial lagrange multiplier in DGD for # trajectory optimization. 'kl_step':0.2, 'min_step_mult':0.01, 'max_step_mult':10.0, # Trajectory settings. 'initial_state_var':1e-6, 'init_traj_distr': None, # A function that takes in two arguments, agent and cond, and returns a policy # Trajectory optimization. 'traj_opt': None, # Dynamics hyperaparams. 'dynamics': None, # Costs. 'cost': None, # A list of Cost objects for each condition. 'sample_on_policy': False, 'policy_dual_rate': 0.1, 'policy_dual_rate_covar': 0.0, 'fixed_lg_step': 0, 'lg_step_schedule': 10.0, 'ent_reg_schedule': 0.0, 'init_pol_wt': 0.01, 'policy_sample_mode': 'add', 'exp_step_increase': 2.0, 'exp_step_decrease': 0.5, 'exp_step_upper': 0.5, 'exp_step_lower': 1.0 } lto_code/python/gps/algorithm/cost/0000755000175000017500000000000013124341100015463 5ustar kekelto_code/python/gps/algorithm/cost/cost.py0000644000175000017500000000504413124341100017010 0ustar kekeimport copy import numpy as np from gps.algorithm.cost.config import COST from gps.algorithm.cost.cost_utils import get_ramp_multiplier from gps.proto.gps_pb2 import CUR_LOC class Cost(object): def __init__(self, hyperparams): config = copy.deepcopy(COST) config.update(hyperparams) self._hyperparams = config # Used by _eval_cost in algorithm.py self.weight = self._hyperparams['weight'] self.cur_cond_idx = self._hyperparams['cur_cond_idx'] def eval(self, sample, obj_val_only = False): """ Evaluate cost function and derivatives on a sample. Args: sample: A single sample """ T = sample.T Du = sample.dU Dx = sample.dX cur_fcn = sample.agent.fcns[self.cur_cond_idx]['fcn_obj'] final_l = np.zeros(T) if not obj_val_only: final_lu = np.zeros((T, Du)) final_lx = np.zeros((T, Dx)) final_luu = np.zeros((T, Du, Du)) final_lxx = np.zeros((T, Dx, Dx)) final_lux = np.zeros((T, Du, Dx)) x = sample.get(CUR_LOC) _, dim = x.shape # Time step-specific weights wpm = get_ramp_multiplier( self._hyperparams['ramp_option'], T, wp_final_multiplier=self._hyperparams['wp_final_multiplier'], wp_custom=self._hyperparams['wp_custom'] if 'wp_custom' in self._hyperparams else None ) if not obj_val_only: ls = np.empty((T, dim)) lss = np.empty((T, dim, dim)) cur_fcn.new_sample(batch_size="all") # Get noiseless gradient for t in range(T): final_l[t] = cur_fcn.evaluate(x[t,:][:,None]) if not obj_val_only: ls[t,:] = cur_fcn.grad(x[t,:][:,None])[:,0] lss[t,:,:] = cur_fcn.hess(x[t,:][:,None]) final_l = final_l * wpm if not obj_val_only: ls = ls * wpm[:,None] lss = lss * wpm[:,None,None] # Equivalent to final_lx[:,sensor_start_idx:sensor_end_idx] = ls sample.agent.pack_data_x(final_lx, ls, data_types=[CUR_LOC]) # Equivalent to final_lxx[:,sensor_start_idx:sensor_end_idx,sensor_start_idx:sensor_end_idx] = lss sample.agent.pack_data_x(final_lxx, lss, data_types=[CUR_LOC, CUR_LOC]) if obj_val_only: return (final_l,) else: return final_l, final_lx, final_lu, final_lxx, final_luu, final_lux lto_code/python/gps/algorithm/cost/config.py0000644000175000017500000000047413124341100017307 0ustar keke""" Default configuration and hyperparameter values for costs. """ import numpy as np from gps.algorithm.cost.cost_utils import RAMP_CONSTANT COST = { 'ramp_option': RAMP_CONSTANT, # How target cost ramps over time. 'wp_final_multiplier': 1.0, # Weight multiplier on final time step. 'weight': 1.0 } lto_code/python/gps/algorithm/cost/cost_utils.py0000644000175000017500000000167413124341100020235 0ustar keke""" This file defines utility classes and functions for costs. """ import numpy as np RAMP_CONSTANT = 1 RAMP_LINEAR = 2 RAMP_QUADRATIC = 3 RAMP_FINAL_ONLY = 4 RAMP_CUSTOM = 5 def get_ramp_multiplier(ramp_option, T, wp_final_multiplier=1.0, wp_custom=None): """ Return a time-varying multiplier. Returns: A (T,) float vector containing weights for each time step. """ if ramp_option == RAMP_CONSTANT: wpm = np.ones(T) elif ramp_option == RAMP_LINEAR: wpm = (np.arange(T, dtype=np.float32) + 1) / T elif ramp_option == RAMP_QUADRATIC: wpm = ((np.arange(T, dtype=np.float32) + 1) / T) ** 2 elif ramp_option == RAMP_FINAL_ONLY: wpm = np.zeros(T) wpm[T-1] = 1.0 elif ramp_option == RAMP_CUSTOM: assert(wp_custom is not None) wpm = wp_custom else: raise ValueError('Unknown cost ramp requested!') wpm[-1] *= wp_final_multiplier return wpm lto_code/python/gps/algorithm/cost/__init__.py0000644000175000017500000000000013124341100017562 0ustar kekelto_code/python/gps/algorithm/policy/0000755000175000017500000000000013124341100016012 5ustar kekelto_code/python/gps/algorithm/policy/lin_gauss_policy.py0000644000175000017500000000454313124341100021735 0ustar keke""" This file defines the linear Gaussian policy class. """ import numpy as np from gps.algorithm.policy.policy import Policy from gps.utility.general_utils import check_shape class LinearGaussianPolicy(Policy): """ Time-varying linear Gaussian policy. U = K*x + k + noise, where noise ~ N(0, chol_pol_covar) """ def __init__(self, K, k, pol_covar, chol_pol_covar, inv_pol_covar): Policy.__init__(self) # Assume K has the correct shape, and make sure others match. self.T = K.shape[0] self.dU = K.shape[1] self.dX = K.shape[2] check_shape(k, (self.T, self.dU)) check_shape(pol_covar, (self.T, self.dU, self.dU)) check_shape(chol_pol_covar, (self.T, self.dU, self.dU)) check_shape(inv_pol_covar, (self.T, self.dU, self.dU)) self.K = K self.k = k self.pol_covar = pol_covar self.chol_pol_covar = chol_pol_covar self.inv_pol_covar = inv_pol_covar def act(self, x, obs, t, noise=None): """ Return an action for a state. Args: x: State vector. obs: Observation vector. t: Time step. noise: Action noise. This will be scaled by the variance. """ u = self.K[t].dot(x) + self.k[t] u += self.chol_pol_covar[t].T.dot(noise) return u def fold_k(self, noise): """ Fold noise into k. Args: noise: A T x Du noise vector with mean 0 and variance 1. Returns: k: A T x dU bias vector. """ k = np.zeros_like(self.k) for i in range(self.T): scaled_noise = self.chol_pol_covar[i].T.dot(noise[i]) k[i] = scaled_noise + self.k[i] return k def nans_like(self): """ Returns: A new linear Gaussian policy object with the same dimensions but all values filled with NaNs. """ policy = LinearGaussianPolicy( np.zeros_like(self.K), np.zeros_like(self.k), np.zeros_like(self.pol_covar), np.zeros_like(self.chol_pol_covar), np.zeros_like(self.inv_pol_covar) ) policy.K.fill(np.nan) policy.k.fill(np.nan) policy.pol_covar.fill(np.nan) policy.chol_pol_covar.fill(np.nan) policy.inv_pol_covar.fill(np.nan) return policy lto_code/python/gps/algorithm/policy/lto/0000755000175000017500000000000013124341100016610 5ustar kekelto_code/python/gps/algorithm/policy/lto/lbfgs_policy.py0000644000175000017500000000464313124341100021645 0ustar kekeimport numpy as np from gps.algorithm.policy.policy import Policy from gps.proto.gps_pb2 import CUR_LOC class LBFGSPolicy(Policy): def __init__(self, agent, learning_rate, mem_len, cond, noise_var = None): Policy.__init__(self) self.agent = agent self.learning_rate = learning_rate self.mem_len = mem_len if noise_var is not None: self.sqrt_noise_var = np.sqrt(noise_var) self.cond = cond # cond, not m self.reset() def act(self, x, obs, t, noise=None): assert(t == self.prev_t + 1) self.prev_t = t cur_loc = self.agent.unpack_data_x(x, data_types=[CUR_LOC]) grad = self.agent.fcns[self.cond]['fcn_obj'].grad(cur_loc[:,None])[:,0] if self.s_k is None: self.s_k = np.empty((grad.shape[0],self.mem_len-1)) self.s_k.fill(np.nan) self.y_k = np.empty((grad.shape[0],self.mem_len-1)) self.y_k.fill(np.nan) self.r_k = np.empty((self.mem_len-1,)) self.r_k.fill(np.nan) else: self.s_k[:,1:] = self.s_k[:,:-1] self.s_k[:,0] = cur_loc - self.prev_loc self.y_k[:,1:] = self.y_k[:,:-1] self.y_k[:,0] = grad - self.prev_grad self.r_k[1:] = self.r_k[:-1] self.r_k[0] = 1. / (np.dot(self.y_k[:,0], self.s_k[:,0]) + 1e-8) a_k = np.empty((min(t,self.mem_len-1),)) a_k.fill(np.nan) q = grad for i in range(min(t,self.mem_len-1)): a_k[i] = self.r_k[i] * np.dot(self.s_k[:,i],q) q = q - a_k[i] * self.y_k[:,i] if t == 0: z = q else: z = np.dot(self.s_k[:,0], self.y_k[:,0]) / np.dot(self.y_k[:,0], self.y_k[:,0]) * q for i in range(min(t,self.mem_len-1)-1,-1,-1): b = self.r_k[i] * np.dot(self.y_k[:,i],z) z = z + self.s_k[:,i]*(a_k[i] - b) cur_dir = -z assert(not np.any(np.isnan(cur_dir))) u = self.learning_rate*cur_dir self.prev_loc = cur_loc self.prev_grad = grad if noise is not None: u += self.sqrt_noise_var * noise return u def reset(self): self.s_k = None self.y_k = None self.r_k = None self.prev_loc = None self.prev_grad = None self.prev_t = -1 lto_code/python/gps/algorithm/policy/lto/cg_policy.py0000644000175000017500000000243713124341100021140 0ustar kekeimport numpy as np from gps.algorithm.policy.policy import Policy from gps.proto.gps_pb2 import CUR_LOC class ConjugateGradientPolicy(Policy): def __init__(self, agent, learning_rate, cond, noise_var = None): Policy.__init__(self) self.agent = agent self.learning_rate = learning_rate if noise_var is not None: self.sqrt_noise_var = np.sqrt(noise_var) self.cond = cond # cond, not m self.reset() def act(self, x, obs, t, noise=None): assert(t == self.prev_t + 1) self.prev_t = t cur_loc = self.agent.unpack_data_x(x, data_types=[CUR_LOC]) grad = self.agent.fcns[self.cond]['fcn_obj'].grad(cur_loc[:,None])[:,0] if self.prev_dir is None: cur_dir = -grad else: beta = np.dot(grad, grad) / float(np.dot(self.prev_grad, self.prev_grad)) cur_dir = -grad + beta*self.prev_dir u = self.learning_rate*cur_dir self.prev_dir = cur_dir self.prev_grad = grad u = self.add_noise(u, noise, t) if noise is not None: u += self.sqrt_noise_var * noise return u def reset(self): self.prev_dir = None self.prev_grad = None self.prev_t = -1 lto_code/python/gps/algorithm/policy/lto/momentum_policy.py0000644000175000017500000000216513124341100022406 0ustar kekeimport numpy as np from gps.algorithm.policy.policy import Policy from gps.proto.gps_pb2 import CUR_LOC class MomentumPolicy(Policy): def __init__(self, agent, learning_rate, momentum, cond, noise_var = None): Policy.__init__(self) self.agent = agent self.learning_rate = learning_rate self.momentum = momentum if noise_var is not None: self.sqrt_noise_var = np.sqrt(noise_var) self.cond = cond # cond, not m self.reset() def act(self, x, obs, t, noise=None): assert(t == self.prev_t + 1) self.prev_t = t cur_loc = self.agent.unpack_data_x(x, data_types=[CUR_LOC]) grad = self.agent.fcns[self.cond]['fcn_obj'].grad(cur_loc[:,None])[:,0] if self.prev_update is None: self.prev_update = np.zeros((grad.shape[0],)) u = self.momentum*self.prev_update - self.learning_rate*grad self.prev_update = u if noise is not None: u += self.sqrt_noise_var * noise return u def reset(self): self.prev_update = None self.prev_t = -1 lto_code/python/gps/algorithm/policy/lto/gd_policy.py0000644000175000017500000000145613124341100021141 0ustar kekeimport numpy as np from gps.algorithm.policy.policy import Policy from gps.proto.gps_pb2 import CUR_LOC class GradientDescentPolicy(Policy): def __init__(self, agent, learning_rate, cond, noise_var = None): Policy.__init__(self) self.agent = agent self.learning_rate = learning_rate if noise_var is not None: self.sqrt_noise_var = np.sqrt(noise_var) self.cond = cond # cond, not m def act(self, x, obs, t, noise=None): cur_loc = self.agent.unpack_data_x(x, data_types=[CUR_LOC], condition=self.cond) grad = self.agent.fcns[self.cond]['fcn_obj'].grad(cur_loc[:,None])[:,0] u = -self.learning_rate*grad if noise is not None: u += self.sqrt_noise_var * noise return u lto_code/python/gps/algorithm/policy/lto/__init__.py0000644000175000017500000000000013124341100020707 0ustar kekelto_code/python/gps/algorithm/policy/policy.py0000644000175000017500000000165413124341100017671 0ustar keke""" This file defines the base class for the policy. """ import abc class Policy(object): """ Computes actions from states/observations. """ __metaclass__ = abc.ABCMeta @abc.abstractmethod def act(self, x, obs, t, noise): """ Args: x: State vector. obs: Observation vector. t: Time step. noise: A dU-dimensional noise vector. Returns: A dU dimensional action vector. """ raise NotImplementedError("Must be implemented in subclass.") def reset(self): return # Called when done using the object - must call reset() before starting to use it again def finalize(self): return def set_meta_data(self, meta): """ Set meta data for policy (e.g., domain image, multi modal observation sizes) Args: meta: meta data. """ return lto_code/python/gps/algorithm/policy/policy_prior_gmm.py0000644000175000017500000001210513124341100021735 0ustar keke""" This file defines a GMM prior for policy linearization. """ import copy import logging import numpy as np from gps.algorithm.policy.config import POLICY_PRIOR_GMM from gps.utility.gmm import GMM from gps.algorithm.algorithm_utils import gauss_fit_joint_prior LOGGER = logging.getLogger(__name__) class PolicyPriorGMM(object): """ A policy prior encoded as a GMM over [x_t, u_t] points, where u_t is the output of the policy for the given state x_t. This prior is used when computing the linearization of the policy. See the method AlgorithmBADMM._update_policy_fit, in python/gps/algorithm.algorithm_badmm.py. Also see the GMM dynamics prior, in python/gps/algorithm/dynamics/dynamics_prior_gmm.py. This is a similar GMM prior that is used for the dynamics estimate. """ def __init__(self, hyperparams): """ Hyperparameters: min_samples_per_cluster: Minimum number of samples. max_clusters: Maximum number of clusters to fit. max_samples: Maximum number of trajectories to use for fitting the GMM at any given time. strength: Adjusts the strength of the prior. """ config = copy.deepcopy(POLICY_PRIOR_GMM) config.update(hyperparams) self._hyperparams = config self.X = None self.obs = None self.gmm = GMM() self._min_samp = self._hyperparams['min_samples_per_cluster'] self._max_samples = self._hyperparams['max_samples'] self._max_clusters = self._hyperparams['max_clusters'] self._strength = self._hyperparams['strength'] self._init_sig_reg = self._hyperparams['init_regularization'] self._subsequent_sig_reg = self._hyperparams['subsequent_regularization'] def update(self, samples, policy_opt, mode='add'): """ Update GMM using new samples or policy_opt. By default does not replace old samples. Args: samples: SampleList containing new samples policy_opt: PolicyOpt containing current policy """ X, obs = samples.get_X(), samples.get_obs() if self.X is None or mode == 'replace': self.X = X self.obs = obs elif mode == 'add' and X.size > 0: self.X = np.concatenate([self.X, X], axis=0) self.obs = np.concatenate([self.obs, obs], axis=0) # Trim extra samples N = self.X.shape[0] if N > self._max_samples: start = N - self._max_samples self.X = self.X[start:, :, :] self.obs = self.obs[start:, :, :] # Evaluate policy at samples to get mean policy action. U = policy_opt.prob(self.obs,diag_var=True)[0] # Create the dataset N, T = self.X.shape[:2] dO = self.X.shape[2] + U.shape[2] XU = np.reshape(np.concatenate([self.X, U], axis=2), [T * N, dO]) # Choose number of clusters. K = int(max(2, min(self._max_clusters, np.floor(float(N * T) / self._min_samp)))) LOGGER.debug('Generating %d clusters for policy prior GMM.', K) self.gmm.update(XU, K) def eval(self, Ts, Ps): """ Evaluate prior. """ # Construct query data point. pts = np.concatenate((Ts, Ps), axis=1) # Perform query. mu0, Phi, m, n0 = self.gmm.inference(pts) # Factor in multiplier. n0 *= self._strength m *= self._strength # Multiply Phi by m (since it was normalized before). Phi *= m return mu0, Phi, m, n0 def fit(self, X, pol_mu, pol_sig): """ Fit policy linearization. Args: X: Samples (N, T, dX) pol_mu: Policy means (N, T, dU) pol_sig: Policy covariance (N, T, dU) """ N, T, dX = X.shape dU = pol_mu.shape[2] if N == 1: raise ValueError("Cannot fit dynamics on 1 sample") # Collapse policy covariances. (This is only correct because # the policy doesn't depend on state). pol_sig = np.mean(pol_sig, axis=0) # Allocate. pol_K = np.zeros([T, dU, dX]) pol_k = np.zeros([T, dU]) pol_S = np.zeros([T, dU, dU]) # Fit policy linearization with least squares regression. dwts = (1.0 / N) * np.ones(N) for t in range(T): Ts = X[:, t, :] Ps = pol_mu[:, t, :] Ys = np.concatenate([Ts, Ps], axis=1) # Obtain Normal-inverse-Wishart prior. mu0, Phi, mm, n0 = self.eval(Ts, Ps) sig_reg = np.zeros((dX+dU, dX+dU)) # Slightly regularize on first timestep. if t == 0: sig_reg[:dX, :dX] = self._init_sig_reg*np.eye(dX) else: sig_reg[:dX, :dX] = self._subsequent_sig_reg*np.eye(dX) pol_K[t, :, :], pol_k[t, :], pol_S[t, :, :] = \ gauss_fit_joint_prior(Ys, mu0, Phi, mm, n0, dwts, dX, dU, sig_reg) pol_S += pol_sig return pol_K, pol_k, pol_S lto_code/python/gps/algorithm/policy/tf_policy.py0000644000175000017500000001044313124341100020356 0ustar kekeimport pickle import os import uuid import numpy as np import tensorflow as tf from gps.algorithm.policy.policy import Policy class TfPolicy(Policy): """ A neural network policy implemented in TensorFlow. The network output is taken to be the mean, and Gaussian noise is added on top of it. U = net.forward(obs) + noise, where noise ~ N(0, diag(var)) Args: obs_tensor: tensor representing tf observation. Used in feed dict for forward pass. act_op: tf op to execute the forward pass. Use sess.run on this op. var: Du-dimensional noise variance vector. sess: tf session. device_string: tf device string for running on either gpu or cpu. """ def __init__(self, dU, obs_tensor, act_op, var, sess, device_string): Policy.__init__(self) self.dU = dU self.obs_tensor = obs_tensor self.act_op = act_op self.sess = sess self.device_string = device_string self.chol_pol_covar = np.diag(np.sqrt(var)) self.scale = None # must be set from elsewhere based on observations self.bias = None def act(self, x, obs, t, noise): """ Return an action for a state. Args: x: State vector. obs: Observation vector. t: Time step. noise: Action noise. This will be scaled by the variance. """ # Normalize obs. if len(obs.shape) == 1: obs = np.expand_dims(obs, axis=0) obs = obs.dot(self.scale) + self.bias with tf.device(self.device_string): action_mean = self.sess.run(self.act_op, feed_dict={self.obs_tensor: obs}) if noise is None: u = action_mean else: u = action_mean + self.chol_pol_covar.T.dot(noise) return u[0] # the DAG computations are batched by default, but we use batch size 1. def pickle_policy(self, deg_obs, deg_action, checkpoint_path, goal_state=None, should_hash=False): """ We can save just the policy if we are only interested in running forward at a later point without needing a policy optimization class. Useful for debugging and deploying. """ if should_hash is True: hash_str = str(uuid.uuid4()) checkpoint_path += hash_str pickled_pol = {'deg_obs': deg_obs, 'deg_action': deg_action, 'chol_pol_covar': self.chol_pol_covar, 'checkpoint_path_tf': checkpoint_path + '_tf_data.ckpt', 'scale': self.scale, 'bias': self.bias, 'device_string': self.device_string, 'goal_state': goal_state} pickle.dump(pickled_pol, open(checkpoint_path + '.pkl', "wb")) saver = tf.train.Saver() saver.save(self.sess, checkpoint_path + '_tf_data.ckpt') @classmethod def load_policy(cls, policy_dict_path, tf_generator, network_config=None): """ For when we only need to load a policy for the forward pass. For instance, to run on the robot from a checkpointed policy. """ from tensorflow.python.framework import ops ops.reset_default_graph() # we need to destroy the default graph before re_init or checkpoint won't restore. pol_dict = pickle.load(open(policy_dict_path, "rb")) if 'deg_obs' in network_config: pol_dict['deg_obs'] = network_config['deg_obs'] if 'deg_action' in network_config: pol_dict['deg_action'] = network_config['deg_action'] tf_map = tf_generator(dim_input=pol_dict['deg_obs'], dim_output=pol_dict['deg_action'], batch_size=1, network_config=network_config) sess = tf.Session() init_op = tf.initialize_all_variables() sess.run(init_op) saver = tf.train.Saver() check_file = '/'.join(str.split(policy_dict_path, '/')[:-1]) + '/' + str.split(pol_dict['checkpoint_path_tf'], '/')[-1] saver.restore(sess, check_file) device_string = pol_dict['device_string'] cls_init = cls(pol_dict['deg_action'], tf_map.get_input_tensor(), tf_map.get_output_op(), np.zeros((1,)), sess, device_string) cls_init.chol_pol_covar = pol_dict['chol_pol_covar'] cls_init.scale = pol_dict['scale'] cls_init.bias = pol_dict['bias'] return cls_init lto_code/python/gps/algorithm/policy/config.py0000644000175000017500000000041613124341100017632 0ustar keke""" Default configuration and hyperparameter values for policies. """ INIT_LG = { 'init_var': 1.0, 'verbose': False } # PolicyPriorGMM POLICY_PRIOR_GMM = { 'min_samples_per_cluster': 20, 'max_clusters': 50, 'max_samples': 20, 'strength': 1.0, } lto_code/python/gps/algorithm/policy/lin_gauss_init.py0000644000175000017500000000776413124341100021411 0ustar keke""" Initializations for linear Gaussian controllers. """ import copy import numpy as np import scipy as sp from gps.algorithm.policy.lin_gauss_policy import LinearGaussianPolicy from gps.algorithm.policy.config import INIT_LG from gps.agent.lto.lto_world import LTOWorld from gps.proto.gps_pb2 import PAST_GRADS, CUR_GRAD def init_lto_controller(hyperparams, agent): config = copy.deepcopy(INIT_LG) config.update(hyperparams) dX, dU = config['dX'], config['dU'] T = config['T'] cur_cond_idx = config['cur_cond_idx'] history_len = agent.history_len fcn = agent.fcns[cur_cond_idx] # Create new world to avoiding changing the state of the original world world = LTOWorld(fcn['fcn_obj'], fcn['dim'], fcn['init_loc'], history_len) # Compute initial state. world.reset_world() world.run() x0 = agent.get_vectorized_state(world.get_state(), cur_cond_idx) best_momentum = None best_learning_rate = None min_obj_val = float('Inf') if config['verbose']: print("Finding Initial Linear Gaussian Controller") for i in range(config['all_possible_momentum_params'].shape[0]): cur_momentum = config['all_possible_momentum_params'][i] for j in range(config['all_possible_learning_rates'].shape[0]): cur_learning_rate = config['all_possible_learning_rates'][j] cur_Kt = np.zeros((dU, dX)) # K matrix for a single time step. # Equivalent to Kt[:,sensor_start_idx:sensor_end_idx] = np.eye(dU) agent.pack_data_x(cur_Kt, np.eye(dU), data_types=[CUR_GRAD]) # Oldest gradients come first agent.pack_data_x(cur_Kt, np.tile(np.eye(dU),(1,history_len)) * (cur_momentum ** np.ravel(np.tile(np.arange(history_len,0,-1)[:,None],(1,dU))))[None,:], data_types=[PAST_GRADS]) cur_Kt = -cur_learning_rate*cur_Kt cur_kt = np.dot(cur_Kt, x0) cur_policy = LinearGaussianPolicy(cur_Kt[None,:,:], cur_kt[None,:], np.zeros((1,dU,dU)), np.zeros((1,dU,dU)), np.zeros((1,dU,dU))) world.reset_world() world.run() for t in range(T): X_t = agent.get_vectorized_state(world.get_state(), cur_cond_idx) U_t = cur_policy.act(X_t, None, 0, np.zeros((dU,))) world.run_next(U_t) fcn['fcn_obj'].new_sample(batch_size="all") cur_obj_val = fcn['fcn_obj'].evaluate(world.cur_loc) if config['verbose']: print("Learning Rate: %.4f, Momentum: %.4f, Final Objective Value: %.4f" % (cur_learning_rate,cur_momentum,cur_obj_val)) if cur_obj_val < min_obj_val: min_obj_val = cur_obj_val best_momentum = cur_momentum best_learning_rate = cur_learning_rate if config['verbose']: print("") print("Best Final Objective Value: %.4f" % (min_obj_val)) print("Best Momentum: %.4f" % (best_momentum)) print("Best Learning Rate: %.4f" % (best_learning_rate)) print("------------------------------------------------------") Kt = np.zeros((dU, dX)) # K matrix for a single time step. # Equivalent to Kt[:,sensor_start_idx:sensor_end_idx] = np.eye(dU) agent.pack_data_x(Kt, np.eye(dU), data_types=[CUR_GRAD]) # Oldest gradients come first agent.pack_data_x(Kt, np.tile(np.eye(dU),(1,history_len)) * (best_momentum ** np.ravel(np.tile(np.arange(history_len,0,-1)[:,None],(1,dU))))[None,:], data_types=[PAST_GRADS]) Kt = -best_learning_rate*Kt kt = np.dot(Kt, x0) K = np.tile(Kt[None,:,:], (T, 1, 1)) # Controller gains matrix. k = np.tile(kt[None,:], (T, 1)) PSig = np.tile((config['init_var']*np.eye(dU))[None,:,:], (T, 1, 1)) cholPSig = np.tile((np.sqrt(config['init_var'])*np.eye(dU))[None,:,:], (T, 1, 1)) invPSig = np.tile(((1./config['init_var'])*np.eye(dU))[None,:,:], (T, 1, 1)) return LinearGaussianPolicy(K, k, PSig, cholPSig, invPSig) lto_code/python/gps/algorithm/policy/__init__.py0000644000175000017500000000000013124341100020111 0ustar kekelto_code/python/gps/algorithm/__init__.py0000644000175000017500000000000013124341100016612 0ustar kekelto_code/python/gps/agent/0000755000175000017500000000000013124341100013623 5ustar kekelto_code/python/gps/agent/lto/0000755000175000017500000000000013124341100014421 5ustar kekelto_code/python/gps/agent/lto/fcn.py0000644000175000017500000005464513124341100015557 0ustar kekeimport sys import os import numpy as np import tensorflow as tf import cPickle as pickle from time import time def printWithoutNewline(s): sys.stdout.write(s) sys.stdout.flush() # A FcnFamily is a function template with unrealized placeholders (e.g. coefficients) # A Fcn is a member of a FcnFamily with actual values substituted in for the placeholders # For input to the functions "evaluate", "grad", "hess", x can be a list of variables, but each variable must be an N x 1 vector # fcn must be a function that takes two arguments, x and params. x is a list of variables, and params is a dict, with the keys corresponding to names of placeholders and values being the substituted values class FcnFamily(object): # params is a dict whose entries are (name, type) # hyperparams is a dict and must be the SAME as the parameters passed into the constructor of the child class - it is used for pickling # Options can be passed in as extra keyword arguments. Available options: disabled_hess, session, start_session_manually, gpu_id, tensor_prefix # Options that are for internal use only: graph_def and tensor_names - these are used when unpickling def __init__(self, fcn, num_dims, params, hyperparams, **kwargs): self.num_dims = num_dims self.fcn_defns = fcn self.param_defns = params self.hyperparams = hyperparams self.options = kwargs self.session = None if "session" in self.options: session = self.options["session"] del self.options["session"] if "start_session_manually" in self.options and self.options["start_session_manually"]: print("Warning: start_session_manually is set to True even though session is passed in. Starting session anyway. ") self.start_session(session) else: if "start_session_manually" not in self.options or not self.options["start_session_manually"]: self.start_session() if "start_session_manually" in self.options: del self.options["start_session_manually"] def start_session(self, session = None): def construct_graph(): if "graph_def" in self.options: graph_def = tf.GraphDef() graph_def.ParseFromString(self.options["graph_def"]) del self.options["graph_def"] self.session.graph.as_default() tf.import_graph_def(graph_def, name="") if "tensor_names" in self.options: tensor_names = self.options["tensor_names"] else: prefix = "%s_" % (self.options["tensor_prefix"]) if "tensor_prefix" in self.options else "" tensor_names = dict() tensor_names["params"] = {param_name: "%sparam_%s:0" % (prefix,param_name) for param_name in self.param_defns} tensor_names["x"] = ["%sx_%d:0" % (prefix,i) for i in range(len(self.num_dims))] tensor_names["fcn"] = "%sfcn:0" % (prefix) tensor_names["grad"] = ["%sgrad_%d:0" % (prefix,i) for i in range(len(self.num_dims))] if ("disable_hess" not in self.options) or (not self.options["disable_hess"]): tensor_names["hess"] = [["%shess_%d_%d:0" % (prefix,i,j) for j in range(i+1)] for i in range(len(self.num_dims))] for param_name in self.param_defns: self.params[param_name] = self.session.graph.get_tensor_by_name(tensor_names["params"][param_name]) self.is_param_subsampled[param_name] = "subsampled" in self.param_defns[param_name] and self.param_defns[param_name]["subsampled"] self.x_ = [self.session.graph.get_tensor_by_name(tensor_names["x"][i]) for i in range(len(self.num_dims))] # A list of variable groups self.fcn_ = self.session.graph.get_tensor_by_name(tensor_names["fcn"]) self.grad_ = [self.session.graph.get_tensor_by_name(tensor_names["grad"][i]) for i in range(len(self.num_dims))] if ("disable_hess" not in self.options) or (not self.options["disable_hess"]): self.hess_ = [] for i in range(len(self.num_dims)): # Iterate over each variable group block_cols_of_block_cells = [self.session.graph.get_tensor_by_name(tensor_names["hess"][i][j]) for j in range(i+1)] self.hess_.append(block_cols_of_block_cells) else: prefix = "%s_" % (self.options["tensor_prefix"]) if "tensor_prefix" in self.options else "" for param_name in self.param_defns: self.params[param_name] = tf.placeholder(self.param_defns[param_name]["type"], name="%sparam_%s" % (prefix,param_name)) self.is_param_subsampled[param_name] = "subsampled" in self.param_defns[param_name] and self.param_defns[param_name]["subsampled"] self.x_ = [tf.placeholder(tf.float64, name="%sx_%d" % (prefix,i)) for i in range(len(self.num_dims))] # A list of variable groups fcn = self.fcn_defns(self.x_, self.params) # May return a tuple of functions - assume the first one is the main function which we will be differentiating self.fcn_ = tf.identity(fcn, name="%sfcn" % (prefix)) self.grad_ = [tf.identity(cur_grad, name="%sgrad_%d" % (prefix,i)) for i,cur_grad in enumerate(tf.gradients(self.fcn_, self.x_))] # A list of gradient expressions wrt each variable group, each of which is a vector if ("disable_hess" not in self.options) or (not self.options["disable_hess"]): self.hess_ = [] for i in range(len(self.num_dims)): # Iterate over each variable group # Each element is an individual row rows_of_block_cols = [tf.gradients(self.grad_[i][k,:], self.x_[:i+1]) for k in range(self.num_dims[i])] # Each element is a block column block_cols_of_block_cells = [tf.transpose(tf.concat(1, [row[j] for row in rows_of_block_cols]), name="%shess_%d_%d" % (prefix,i,j)) for j in range(i+1)] self.hess_.append(block_cols_of_block_cells) if self.session is not None: if session is not None and self.session != session: print("Warning: start_session is called with a different session than the one in use. Will keep using existing session. ") else: if session is None: self.session = tf.Session() else: self.session = session self.params = {} self.is_param_subsampled = {} self.device_string = "/cpu:0" if "gpu_id" in self.options: self.device_string = "/gpu:%d" % (self.options["gpu_id"]) if self.device_string == "/cpu:0": with tf.device(self.device_string): construct_graph() else: construct_graph() def assign_param_vals_(self, param_vals): placeholder_vals = {} for key in self.params: placeholder_vals[self.params[key]] = param_vals[key] return placeholder_vals def evaluate(self, x, param_vals): assert self.session is not None, "start_session() must be called first. " placeholder_vals = {self.x_[i]: x[i] for i in range(len(self.x_))} placeholder_vals.update(self.assign_param_vals_(param_vals)) with tf.device(self.device_string): val = self.session.run(self.fcn_, placeholder_vals) return val def grad(self, x, param_vals): assert self.session is not None, "start_session() must be called first. " placeholder_vals = {self.x_[i]: x[i] for i in range(len(self.x_))} placeholder_vals.update(self.assign_param_vals_(param_vals)) with tf.device(self.device_string): vals = self.session.run(self.grad_, placeholder_vals) return vals # Returns a list of lists, with vals[i][j] containing the second derivative wrt self.x_[i] and self.x_[j] def hess(self, x, param_vals): assert ("disable_hess" not in self.options) or (not self.options["disable_hess"]), "Hessian is disabled. " assert self.session is not None, "start_session() must be called first. " placeholder_vals = {self.x_[i]: x[i] for i in range(len(self.x_))} placeholder_vals.update(self.assign_param_vals_(param_vals)) with tf.device(self.device_string): flattened_vals = self.session.run([hess_elem for hess_list in self.hess_ for hess_elem in hess_list], placeholder_vals) vals = [] j = 0 for i in range(len(self.x_)): vals.append(flattened_vals[j:j+(i+1)]) vals[-1].extend([None] * (len(self.x_)-i-1)) j += (i+1) # Fill in the upper triangle of the Hessian by taking advantage of the symmetry of the Hessian for i in range(1,len(self.x_)): for j in range(i): vals[j][i] = vals[i][j].T return vals def get_total_num_dim(self): total_num_dim = 0 for num_dim in self.num_dims: total_num_dim += num_dim return total_num_dim def destroy(self): if self.session is not None: self.session.close() self.session = None # For pickling def __getstate__(self): if self.session is None: print("Warning: Session automatically started for the purposes of pickling. ") self.start_session() tf.train.write_graph(self.session.graph_def, "/tmp", "tf_graph.pb", False) #proto with open("/tmp/tf_graph.pb", "rb") as f: graph_def_str = f.read() os.remove("/tmp/tf_graph.pb") tensor_names = dict() tensor_names["params"] = {param_name: self.params[param_name].name for param_name in self.params} tensor_names["x"] = [cur_x.name for cur_x in self.x_] tensor_names["fcn"] = self.fcn_.name tensor_names["grad"] = [cur_grad.name for cur_grad in self.grad_] if ("disable_hess" not in self.options) or (not self.options["disable_hess"]): tensor_names["hess"] = [[cur_hess_block.name for cur_hess_block in cur_hess_block_row] for cur_hess_block_row in self.hess_] return {"hyperparams": self.hyperparams, "options": {option_name: self.options[option_name] for option_name in self.options if option_name not in ["session", "graph_def", "start_session_manually"]}, "graph_def": graph_def_str, "tensor_names": tensor_names} # For unpickling def __setstate__(self, state): kwargs = state["hyperparams"].copy() kwargs.update(state["options"]) kwargs["graph_def"] = state["graph_def"] kwargs["tensor_names"] = state["tensor_names"] kwargs["start_session_manually"] = True self.__init__(**kwargs) class Fcn(object): # If disable_subsampling is set to True, will never subsample regardless of what batch_size is set to be, either in the constructor or in Fcn.new_sample() def __init__(self, family, param_vals, batch_size = "all", disable_subsampling = False): self.family = family self.param_vals = param_vals self.batch_size = batch_size self.disable_subsampling = (disable_subsampling or all(not self.family.is_param_subsampled[key] for key in self.family.params)) # If batch size is "all" or no params are subsampled, don't require calling Fcn.new_sample() before calling Fcn.evaluate/grad/hess. if self.disable_subsampling or batch_size == "all": self.subsampled_param_vals = self.param_vals else: self.subsampled_param_vals = None # If self.disable_subsampling is True, this is a no-op. # If batch_size is set, temporarily overrides self.batch_size # By setting batch_size to "all", can temporarily disable subsampling def new_sample(self, batch_size = None): if not self.disable_subsampling: if batch_size is None: batch_size = self.batch_size if batch_size != "all": subsampled_idx = None # Same sampled indices are used for all params to preserve correspondence between individual entries (i.e. one row of data corresponds to one element of label) self.subsampled_param_vals = {} for key in self.family.params: if not self.family.is_param_subsampled[key] or batch_size >= self.param_vals[key].shape[0]: self.subsampled_param_vals[key] = self.param_vals[key] else: if subsampled_idx is None: subsampled_idx = np.random.permutation(self.param_vals[key].shape[0])[:batch_size] self.subsampled_param_vals[key] = self.param_vals[key][subsampled_idx] else: self.subsampled_param_vals = self.param_vals def evaluate(self, x): assert self.subsampled_param_vals is not None, "Fcn.new_sample() must be called first. " return self.family.evaluate(x, self.subsampled_param_vals) def grad(self, x): assert self.subsampled_param_vals is not None, "Fcn.new_sample() must be called first. " return self.family.grad(x, self.subsampled_param_vals) def hess(self, x): assert self.subsampled_param_vals is not None, "Fcn.new_sample() must be called first. " return self.family.hess(x, self.subsampled_param_vals) class QuadFormFcnFamily(FcnFamily): def __init__(self, num_dim, **kwargs): def fcn(x, params): return tf.matmul(x[0], tf.matmul(params["A"], x[0]), transpose_a=True) FcnFamily.__init__(self, fcn, [num_dim], {"A": {"type": tf.float64}}, {"num_dim": num_dim}, **kwargs) class QuadFormFcn(Fcn): def __init__(self, family, A, *args, **kwargs): Fcn.__init__(self, family, {"A": A}, *args, **kwargs) def evaluate(self, x): return Fcn.evaluate(self, [x]) def grad(self, x): return Fcn.grad(self, [x])[0] def hess(self, x): return Fcn.hess(self, [x])[0][0] class LogisticRegressionFcnFamily(FcnFamily): def __init__(self, dim, **kwargs): # params["data"] is N x dim, params["labels"] is N x 1 # params["sigma_sq"] is 1 x 1 and represents the squared of the sigma parameter in the GM estimator # The larger sigma is, the larger the non-saturating range def fcn(x, params): weights = tf.slice(x[0], [0,0], [dim,-1]) # dim x 1 matrix bias = tf.slice(x[0], [dim,0], [1,-1]) # 1 x 1 matrix preds = tf.matmul(params["data"], weights) + bias # N x 1 matrix, where N is the number of data points loss = tf.reduce_mean(tf.nn.sigmoid_cross_entropy_with_logits(preds, params["labels"])) # L2 regularization for the fully connected parameters. regularizers = tf.nn.l2_loss(weights) # Add the regularization term to the loss. loss += 5e-4 * regularizers return loss FcnFamily.__init__(self, fcn, [dim+1], {"data": {"type": tf.float64, "subsampled": True}, "labels": {"type": tf.float64, "subsampled": True}}, {"dim": dim}, **kwargs) class LogisticRegressionFcn(Fcn): def __init__(self, family, data, labels, *args, **kwargs): Fcn.__init__(self, family, {"data": data, "labels": labels}, *args, **kwargs) def evaluate(self, x): return Fcn.evaluate(self, [x]) def grad(self, x): return Fcn.grad(self, [x])[0] def hess(self, x): return Fcn.hess(self, [x])[0][0] class LogisticRegressionWithoutBiasFcnFamily(FcnFamily): def __init__(self, dim, **kwargs): # params["data"] is N x dim, params["labels"] is N x 1 def fcn(x, params): weights = x[0] preds = tf.matmul(params["data"], weights) # N x 1 matrix, where N is the number of data points loss = tf.reduce_mean(tf.nn.sigmoid_cross_entropy_with_logits(preds, params["labels"])) # L2 regularization for the fully connected parameters. regularizers = tf.nn.l2_loss(weights) # Add the regularization term to the loss. loss += 5e-4 * regularizers return loss FcnFamily.__init__(self, fcn, [dim], {"data": {"type": tf.float64, "subsampled": True}, "labels": {"type": tf.float64, "subsampled": True}}, {"dim": dim}, **kwargs) class LogisticRegressionWithoutBiasFcn(Fcn): def __init__(self, family, data, labels, *args, **kwargs): Fcn.__init__(self, family, {"data": data, "labels": labels}, *args, **kwargs) def evaluate(self, x): return Fcn.evaluate(self, [x]) def grad(self, x): return Fcn.grad(self, [x])[0] def hess(self, x): return Fcn.hess(self, [x])[0][0] # Robust linear regresison using Geman-McLure (GM) estimator class RobustRegressionFcnFamily(FcnFamily): def __init__(self, dim, **kwargs): # params["data"] is N x dim, params["labels"] is N x 1 # params["sigma_sq"] is 1 x 1 and represents the squared of the sigma parameter in the GM estimator # The larger sigma is, the larger the non-saturating range def fcn(x, params): weights = tf.slice(x[0], [0,0], [dim,-1]) # dim x 1 matrix bias = tf.slice(x[0], [dim,0], [1,-1]) # 1 x 1 matrix preds = tf.matmul(params["data"], weights) + bias # N x 1 matrix, where N is the number of data points err = params["labels"] - preds err_sq = tf.square(err) loss = tf.reduce_mean(tf.truediv(err_sq, tf.add(err_sq, params["sigma_sq"]))) return loss FcnFamily.__init__(self, fcn, [dim+1], {"data": {"type": tf.float64, "subsampled": True}, "labels": {"type": tf.float64, "subsampled": True}, "sigma_sq": {"type": tf.float64}}, {"dim": dim}, **kwargs) class RobustRegressionFcn(Fcn): def __init__(self, family, data, labels, sigma_sq, *args, **kwargs): Fcn.__init__(self, family, {"data": data, "labels": labels, "sigma_sq": sigma_sq}, *args, **kwargs) def evaluate(self, x): return Fcn.evaluate(self, [x]) def grad(self, x): return Fcn.grad(self, [x])[0] def hess(self, x): return Fcn.hess(self, [x])[0][0] class NeuralNetFcnFamily(FcnFamily): def __init__(self, input_dim, hidden_dim, output_dim, **kwargs): if not isinstance(hidden_dim, list): hidden_dim = [hidden_dim] dims = [input_dim] + hidden_dim + [output_dim] def fcn(x, params): weights = [] biases = [] for i in range(len(dims)-1): weights.append(tf.reshape(x[2*i], [dims[i], dims[i+1]])) biases.append(tf.reshape(x[2*i+1], [1, dims[i+1]])) cur_layer = params["data"] for i in range(len(dims)-1): if i == len(dims)-2: cur_layer = tf.matmul(cur_layer, weights[i]) + biases[i] else: cur_layer = tf.nn.relu(tf.matmul(cur_layer, weights[i]) + biases[i]) output = cur_layer loss = tf.reduce_mean(tf.nn.sparse_softmax_cross_entropy_with_logits( output, params["labels"])) # L2 regularization for the fully connected parameters. regularizers = tf.nn.l2_loss(weights[0]) for i in range(1,len(dims)-1): regularizers += tf.nn.l2_loss(weights[i]) # Add the regularization term to the loss. loss += params["l2_weight"] * regularizers return loss param_sizes = [] for i in range(len(dims)-1): param_sizes.append(dims[i]*dims[i+1]) param_sizes.append(dims[i+1]) FcnFamily.__init__(self, fcn, param_sizes, {"data": {"type": tf.float64, "subsampled": True}, "labels": {"type": tf.int64, "subsampled": True}, "l2_weight": {"type": tf.float64}}, {"input_dim": input_dim, "hidden_dim": hidden_dim, "output_dim": output_dim}, **kwargs) class NeuralNetFcn(Fcn): # labels is an N x 1 array, where N is the batch size def __init__(self, family, data, labels, l2_weight = 5e-4, *args, **kwargs): assert(labels.shape[1] == 1) Fcn.__init__(self, family, {"data": data, "labels": labels[:,0], "l2_weight": l2_weight}, *args, **kwargs) def unpack_x(self, x): unpacked_x = [] prev_dim = 0 for num_dim in self.family.num_dims: unpacked_x.append(x[prev_dim:prev_dim+num_dim,:]) prev_dim += num_dim return unpacked_x def evaluate(self, x): return Fcn.evaluate(self, self.unpack_x(x)) def grad(self, x): return np.vstack(Fcn.grad(self, self.unpack_x(x))) def hess(self, x): return np.vstack([np.hstack(block_row) for block_row in Fcn.hess(self, self.unpack_x(x))]) def main(*args): family = QuadFormFcnFamily(2) fcn = QuadFormFcn(family, np.array([[2., 1.], [1., 2.]])) print(fcn.evaluate(np.array([[-1.],[2.]]))) print(fcn.grad(np.array([[-1.],[2.]]))) print(fcn.hess(np.array([[-1.],[2.]]))) family.destroy() input_dim = 5 hidden_dim = [5] output_dim = 5 num_examples = 10 family = NeuralNetFcnFamily(input_dim,hidden_dim,output_dim) data = np.random.randn(num_examples,input_dim) labels = np.random.randint(output_dim,size=(num_examples,1)) fcn = NeuralNetFcn(family, data, labels) weights1 = np.random.randn(input_dim*hidden_dim[0],1) biases1 = np.random.randn(hidden_dim[0],1) weights2 = np.random.randn(hidden_dim[0]*output_dim,1) biases2 = np.random.randn(output_dim,1) x = np.vstack((weights1,biases1,weights2,biases2)) print("Dimensionality: %d" % (x.shape[0])) print(fcn.evaluate(x)) print(fcn.grad(x)) family.destroy() if __name__ == '__main__': main(*sys.argv[1:]) lto_code/python/gps/agent/lto/agent_lto.py0000644000175000017500000000561613124341100016757 0ustar kekefrom copy import deepcopy import numpy as np from gps.agent.agent import Agent from gps.proto.gps_pb2 import ACTION from gps.sample.sample import Sample from gps.agent.lto.lto_world import LTOWorld class AgentLTO(Agent): def __init__(self, hyperparams): Agent.__init__(self, hyperparams) self._setup_conditions() self._setup_worlds() def _setup_conditions(self): self.conds = self._hyperparams['conditions'] self.fcns = self._hyperparams['fcns'] self.history_len = self._hyperparams['history_len'] def _setup_worlds(self): self._worlds = [LTOWorld(self.fcns[i]['fcn_obj'], self.fcns[i]['dim'], self.fcns[i]['init_loc'], self.history_len) for i in range(self.conds)] self.x0 = [] for i in range(self.conds): self._worlds[i].reset_world() self._worlds[i].run(batch_size="all") # Get noiseless initial state x0 = self.get_vectorized_state(self._worlds[i].get_state()) self.x0.append(x0) def sample(self, policy, condition, verbose=False, save=True, noisy=True): """ Runs a trial and constructs a new sample containing information about the trial. Args: policy: Policy to to used in the trial. condition (int): Which condition setup to run. verbose (boolean): Whether or not to plot the trial (not used here). save (boolean): Whether or not to store the trial into the samples. noisy (boolean): Whether or not to use noise during sampling. """ self._worlds[condition].reset_world() self._worlds[condition].run() new_sample = self._init_sample(self._worlds[condition].get_state()) U = np.zeros([self.T, self.dU]) if noisy: noise = np.random.randn(self.T, self.dU) else: noise = np.zeros((self.T, self.dU)) policy.reset() # To support non-Markovian policies for t in range(self.T): X_t = new_sample.get_X(t=t) obs_t = new_sample.get_obs(t=t) U[t, :] = policy.act(X_t, obs_t, t, noise[t, :]) if (t+1) < self.T: for _ in range(self._hyperparams['substeps']): self._worlds[condition].run_next(U[t, :]) self._set_sample(new_sample, self._worlds[condition].get_state(), t) new_sample.set(ACTION, U) policy.finalize() if save: self._samples[condition].append(new_sample) return new_sample def _init_sample(self, init_X): """ Construct a new sample and fill in the first time step. """ sample = Sample(self) self._set_sample(sample, init_X, -1) return sample def _set_sample(self, sample, X, t): for sensor in X.keys(): sample.set(sensor, np.array(X[sensor]), t=t+1) lto_code/python/gps/agent/lto/lto_world.py0000644000175000017500000000627513124341100017012 0ustar keke""" This file defines an environment for the Box2D PointMass simulator. """ import numpy as np from collections import deque from gps.proto.gps_pb2 import CUR_LOC, PAST_OBJ_VAL_DELTAS, PAST_GRADS, CUR_GRAD, PAST_LOC_DELTAS class LTOWorld(object): def __init__(self, fcn, dim, init_loc, history_len): self.fcn = fcn self.dim = dim self.init_loc = init_loc self.history_len = history_len self.past_locs = deque(maxlen=history_len) self.past_obj_vals = deque(maxlen=history_len) self.past_grads = deque(maxlen=history_len) def run(self, batch_size = None): """Initiates the first time step""" self.fcn.new_sample(batch_size=batch_size) self.cur_loc = self.init_loc self.cur_obj_val = self.fcn.evaluate(self.cur_loc) self.cur_grad = self.fcn.grad(self.cur_loc) # action is of shape (dU,) def run_next(self, action, batch_size = None): """Moves forward in time one step""" self.fcn.new_sample(batch_size=batch_size) self.past_locs.append(self.cur_loc) self.past_obj_vals.append(self.cur_obj_val) self.past_grads.append(self.cur_grad) self.cur_loc = self.cur_loc + action[:,None] self.cur_obj_val = self.fcn.evaluate(self.cur_loc) self.cur_grad = self.fcn.grad(self.cur_loc) def reset_world(self): self.past_locs.clear() self.past_obj_vals.clear() self.past_grads.clear() def get_state(self): past_obj_val_deltas = [] for i in range(1,len(self.past_obj_vals)): past_obj_val_deltas.append((self.past_obj_vals[i] - self.past_obj_vals[i-1]) / float(self.past_obj_vals[i-1])) if len(self.past_obj_vals) > 0: past_obj_val_deltas.append((self.cur_obj_val - self.past_obj_vals[-1]) / float(self.past_obj_vals[-1])) past_obj_val_deltas = np.array(past_obj_val_deltas) past_loc_deltas = [] for i in range(1,len(self.past_locs)): past_loc_deltas.append(self.past_locs[i] - self.past_locs[i-1]) if len(self.past_locs) > 0: past_loc_deltas.append(self.cur_loc - self.past_locs[-1]) past_loc_deltas = np.vstack(past_loc_deltas)[:,0] else: past_loc_deltas = np.zeros((0,)) if len(self.past_grads) > 0: past_grads = np.vstack(self.past_grads)[:,0] else: past_grads = np.zeros((0,)) past_obj_val_deltas = np.hstack((np.zeros((self.history_len-past_obj_val_deltas.shape[0],)),past_obj_val_deltas)) past_grads = np.hstack((np.zeros((self.history_len*self.dim-past_grads.shape[0],)),past_grads)) past_loc_deltas = np.hstack((np.zeros((self.history_len*self.dim-past_loc_deltas.shape[0],)),past_loc_deltas)) cur_loc = self.cur_loc[:,0] cur_grad = self.cur_grad[:,0] state = {CUR_LOC: cur_loc, PAST_OBJ_VAL_DELTAS: past_obj_val_deltas, PAST_GRADS: past_grads, CUR_GRAD: cur_grad, PAST_LOC_DELTAS: past_loc_deltas } return state lto_code/python/gps/agent/lto/__init__.py0000644000175000017500000000000013124341100016520 0ustar kekelto_code/python/gps/agent/agent.py0000644000175000017500000002667213124341100015310 0ustar keke""" This file defines the base agent class. """ import abc import copy import numpy as np from gps.agent.config import AGENT from gps.proto.gps_pb2 import ACTION from gps.sample.sample_list import SampleList class Agent(object): """ Agent superclass. The agent interacts with the environment to collect samples. """ __metaclass__ = abc.ABCMeta def __init__(self, hyperparams): config = copy.deepcopy(AGENT) config.update(hyperparams) self._hyperparams = config # Store samples, along with size/index information for samples. self._samples = [[] for _ in range(self._hyperparams['conditions'])] self.T = self._hyperparams['T'] self.dU = self._hyperparams['sensor_dims'][ACTION] self.x_data_types = self._hyperparams['state_include'] self.obs_data_types = self._hyperparams['obs_include'] if 'meta_include' in self._hyperparams: self.meta_data_types = self._hyperparams['meta_include'] else: self.meta_data_types = [] # List of indices for each data type in state X. self._state_idx, i = [], 0 for sensor in self.x_data_types: dim = self._hyperparams['sensor_dims'][sensor] self._state_idx.append(list(range(i, i+dim))) i += dim self.dX = i # List of indices for each data type in observation. self._obs_idx, i = [], 0 for sensor in self.obs_data_types: dim = self._hyperparams['sensor_dims'][sensor] self._obs_idx.append(list(range(i, i+dim))) i += dim self.dO = i # List of indices for each data type in meta data. self._meta_idx, i = [], 0 for sensor in self.meta_data_types: dim = self._hyperparams['sensor_dims'][sensor] self._meta_idx.append(list(range(i, i+dim))) i += dim self.dM = i self._x_data_idx = {d: i for d, i in zip(self.x_data_types, self._state_idx)} self._obs_data_idx = {d: i for d, i in zip(self.obs_data_types, self._obs_idx)} self._meta_data_idx = {d: i for d, i in zip(self.meta_data_types, self._meta_idx)} @abc.abstractmethod def sample(self, policy, condition, verbose=True, save=True, noisy=True): """ Draw a sample from the environment, using the specified policy and under the specified condition, with or without noise. """ raise NotImplementedError("Must be implemented in subclass.") def reset(self, condition): """ Reset environment to the specified condition. """ pass # May be overridden in subclass. def get_samples(self, condition, start=0, end=None): """ Return the requested samples based on the start and end indices. Args: start: Starting index of samples to return. end: End index of samples to return. """ return (SampleList(self._samples[condition][start:]) if end is None else SampleList(self._samples[condition][start:end])) def clear_samples(self, condition=None): """ Reset the samples for a given condition, defaulting to all conditions. Args: condition: Condition for which to reset samples. """ if condition is None: self._samples = [[] for _ in range(self._hyperparams['conditions'])] else: self._samples[condition] = [] def delete_last_sample(self, condition): """ Delete the last sample from the specified condition. """ self._samples[condition].pop() def get_idx_x(self, sensor_name): """ Return the indices corresponding to a certain state sensor name. Args: sensor_name: The name of the sensor. """ return self._x_data_idx[sensor_name] def get_idx_obs(self, sensor_name): """ Return the indices corresponding to a certain observation sensor name. Args: sensor_name: The name of the sensor. """ return self._obs_data_idx[sensor_name] def pack_data_obs(self, existing_mat, data_to_insert, data_types, axes=None): """ Update the observation matrix with new data. Args: existing_mat: Current observation matrix. data_to_insert: New data to insert into the existing matrix. data_types: Name of the sensors to insert data for. axes: Which axes to insert data. Defaults to the last axes. """ num_sensor = len(data_types) if axes is None: # If axes not specified, assume indexing on last dimensions. axes = list(range(-1, -num_sensor - 1, -1)) else: # Make sure number of sensors and axes are consistent. if num_sensor != len(axes): raise ValueError( 'Length of sensors (%d) must equal length of axes (%d)', num_sensor, len(axes) ) # Shape checks. insert_shape = list(existing_mat.shape) for i in range(num_sensor): # Make sure to slice along X. if existing_mat.shape[axes[i]] != self.dO: raise ValueError('Axes must be along an dX=%d dimensional axis', self.dO) insert_shape[axes[i]] = len(self._obs_data_idx[data_types[i]]) if tuple(insert_shape) != data_to_insert.shape: raise ValueError('Data has shape %s. Expected %s', data_to_insert.shape, tuple(insert_shape)) # Actually perform the slice. index = [slice(None) for _ in range(len(existing_mat.shape))] for i in range(num_sensor): index[axes[i]] = slice(self._obs_data_idx[data_types[i]][0], self._obs_data_idx[data_types[i]][-1] + 1) existing_mat[index] = data_to_insert def pack_data_meta(self, existing_mat, data_to_insert, data_types, axes=None): """ Update the meta data matrix with new data. Args: existing_mat: Current meta data matrix. data_to_insert: New data to insert into the existing matrix. data_types: Name of the sensors to insert data for. axes: Which axes to insert data. Defaults to the last axes. """ num_sensor = len(data_types) if axes is None: # If axes not specified, assume indexing on last dimensions. axes = list(range(-1, -num_sensor - 1, -1)) else: # Make sure number of sensors and axes are consistent. if num_sensor != len(axes): raise ValueError( 'Length of sensors (%d) must equal length of axes (%d)', num_sensor, len(axes) ) # Shape checks. insert_shape = list(existing_mat.shape) for i in range(num_sensor): # Make sure to slice along X. if existing_mat.shape[axes[i]] != self.dM: raise ValueError('Axes must be along an dX=%d dimensional axis', self.dM) insert_shape[axes[i]] = len(self._meta_data_idx[data_types[i]]) if tuple(insert_shape) != data_to_insert.shape: raise ValueError('Data has shape %s. Expected %s', data_to_insert.shape, tuple(insert_shape)) # Actually perform the slice. index = [slice(None) for _ in range(len(existing_mat.shape))] for i in range(num_sensor): index[axes[i]] = slice(self._meta_data_idx[data_types[i]][0], self._meta_data_idx[data_types[i]][-1] + 1) existing_mat[index] = data_to_insert def pack_data_x(self, existing_mat, data_to_insert, data_types, axes=None): """ Update the state matrix with new data. Args: existing_mat: Current state matrix. data_to_insert: New data to insert into the existing matrix. data_types: Name of the sensors to insert data for. axes: Which axes to insert data. Defaults to the last axes. """ num_sensor = len(data_types) if axes is None: # If axes not specified, assume indexing on last dimensions. axes = list(range(-1, -num_sensor - 1, -1)) else: # Make sure number of sensors and axes are consistent. if num_sensor != len(axes): raise ValueError( 'Length of sensors (%d) must equal length of axes (%d)', num_sensor, len(axes) ) # Shape checks. insert_shape = list(existing_mat.shape) for i in range(num_sensor): # Make sure to slice along X. if existing_mat.shape[axes[i]] != self.dX: raise ValueError('Axes must be along an dX=%d dimensional axis', self.dX) insert_shape[axes[i]] = len(self._x_data_idx[data_types[i]]) if tuple(insert_shape) != data_to_insert.shape: raise ValueError('Data has shape %s. Expected %s', data_to_insert.shape, tuple(insert_shape)) # Actually perform the slice. index = [slice(None) for _ in range(len(existing_mat.shape))] for i in range(num_sensor): index[axes[i]] = slice(self._x_data_idx[data_types[i]][0], self._x_data_idx[data_types[i]][-1] + 1) existing_mat[index] = data_to_insert def unpack_data_x(self, existing_mat, data_types, axes=None): """ Returns the requested data from the state matrix. Args: existing_mat: State matrix to unpack from. data_types: Names of the sensor to unpack. axes: Which axes to unpack along. Defaults to the last axes. """ num_sensor = len(data_types) if axes is None: # If axes not specified, assume indexing on last dimensions. axes = list(range(-1, -num_sensor - 1, -1)) else: # Make sure number of sensors and axes are consistent. if num_sensor != len(axes): raise ValueError( 'Length of sensors (%d) must equal length of axes (%d)', num_sensor, len(axes) ) # Shape checks. for i in range(num_sensor): # Make sure to slice along X. if existing_mat.shape[axes[i]] != self.dX: raise ValueError('Axes must be along an dX=%d dimensional axis', self.dX) # Actually perform the slice. index = [slice(None) for _ in range(len(existing_mat.shape))] for i in range(num_sensor): index[axes[i]] = slice(self._x_data_idx[data_types[i]][0], self._x_data_idx[data_types[i]][-1] + 1) return existing_mat[index] # state is a dictionary def get_vectorized_state(self, state, condition = None): state_vector = np.empty((self.dX,)) state_vector.fill(np.nan) for data_type in self.x_data_types: self.pack_data_x(state_vector, state[data_type], data_types=[data_type]) assert(not np.any(np.isnan(state_vector))) return state_vector lto_code/python/gps/agent/config.py0000644000175000017500000000020013124341100015432 0ustar keke""" Default configuration and hyperparameters for agent objects. """ import numpy as np # Agent AGENT = { 'substeps': 1, } lto_code/python/gps/agent/__init__.py0000644000175000017500000000000013124341100015722 0ustar kekelto_code/python/gps/gps_main.py0000644000175000017500000001106313124341100014675 0ustar keke""" This file defines the main object that runs experiments. """ # Difference from gps_main.py: Uses a workaround to save Tensorflow policy. import logging import imp import os import os.path import sys import argparse import time import numpy as np import random # Add gps/python to path so that imports work. sys.path.append('/'.join(str.split(__file__, '/')[:-2])) import gps as gps_globals from gps.utility.display import Display from gps.sample.sample_list import SampleList class GPSMain(object): """ Main class to run algorithms and experiments. """ def __init__(self, config): """ Initialize GPSMain Args: config: Hyperparameters for experiment """ self._hyperparams = config self._conditions = config['common']['conditions'] if 'train_conditions' in config['common']: self._train_idx = config['common']['train_conditions'] self._test_idx = config['common']['test_conditions'] else: self._train_idx = range(self._conditions) config['common']['train_conditions'] = config['common']['conditions'] self._hyperparams=config self._test_idx = self._train_idx self._data_files_dir = config['common']['data_files_dir'] self.agent = config['agent']['type'](config['agent']) self.disp = Display(config['common']) # For logging config['algorithm']['agent'] = self.agent self.algorithm = config['algorithm']['type'](config['algorithm']) def run(self): itr_start = 0 for itr in range(itr_start, self._hyperparams['iterations']): for m, cond in enumerate(self._train_idx): for i in range(self._hyperparams['num_samples']): self._take_sample(itr, cond, m, i) traj_sample_lists = [self.agent.get_samples(cond, -self._hyperparams['num_samples']) for cond in self._train_idx] # Clear agent samples. self.agent.clear_samples() self.algorithm.iteration(traj_sample_lists) pol_sample_lists = self._take_policy_samples(self._train_idx) self._prev_traj_costs, self._prev_pol_costs = self.disp.update(itr, self.algorithm, self.agent, traj_sample_lists, pol_sample_lists) self.algorithm.policy_opt.policy.pickle_policy(self.algorithm.policy_opt._dO, self.algorithm.policy_opt._dU, self._data_files_dir + ('policy_itr_%02d' % itr)) if 'on_exit' in self._hyperparams: self._hyperparams['on_exit'](self._hyperparams) def _take_sample(self, itr, cond, m, i): if self.algorithm.iteration_count == 0: pol = self.algorithm.cur[m].traj_distr else: if self.algorithm._hyperparams['sample_on_policy']: pol = self.algorithm.policy_opt.policy else: pol = self.algorithm.cur[m].traj_distr self.agent.sample(pol, cond) def _take_policy_samples(self, cond_list): pol_samples = [[] for _ in range(len(cond_list))] for cond in range(len(cond_list)): for i in range(self._hyperparams['num_samples']): pol_samples[cond].append(self.agent.sample(self.algorithm.policy_opt.policy, cond_list[cond], save=False)) return [SampleList(samples) for samples in pol_samples] def main(): parser = argparse.ArgumentParser(description='Run the Guided Policy Search algorithm.') parser.add_argument('experiment', type=str, help='experiment name') args = parser.parse_args() exp_name = args.experiment from gps import __file__ as gps_filepath gps_filepath = os.path.abspath(gps_filepath) gps_dir = '/'.join(str.split(gps_filepath, '/')[:-3]) + '/' exp_dir = gps_dir + 'experiments/' + exp_name + '/' hyperparams_file = exp_dir + 'hyperparams.py' logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.INFO) if not os.path.exists(hyperparams_file): sys.exit("Experiment '%s' does not exist.\nDid you create '%s'?" % (exp_name, hyperparams_file)) # May be used by hyperparams.py to load different conditions gps_globals.phase = "TRAIN" hyperparams = imp.load_source('hyperparams', hyperparams_file) seed = hyperparams.config.get('random_seed', 0) random.seed(seed) np.random.seed(seed) gps = GPSMain(hyperparams.config) gps.run() if 'on_exit' in hyperparams.config: hyperparams.config['on_exit'](hyperparams.config) if __name__ == "__main__": main() lto_code/python/gps/sample/0000755000175000017500000000000013124341100014006 5ustar kekelto_code/python/gps/sample/sample_list.py0000644000175000017500000000551013124341100016675 0ustar keke""" This file defines the sample list wrapper and sample writers. """ import cPickle import logging import numpy as np LOGGER = logging.getLogger(__name__) class SampleList(object): """ Class that handles writes and reads to sample data. """ def __init__(self, samples): self._samples = samples def get(self, sensor_name, idx=None): """ Returns N x T x dX numpy array of states. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get(sensor_name) for i in idx]) def get_X(self, idx=None): """ Returns N x T x dX numpy array of states. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get_X() for i in idx]) def get_coordwise_X(self, coord=None, idx=None): """ Returns N x num_coords x T x coordwsie_dX numpy array of features. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get_coordwise_X(coord) for i in idx]) def get_U(self, idx=None): """ Returns N x T x dU numpy array of actions. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get_U() for i in idx]) def get_obs(self, idx=None): """ Returns N x T x dO numpy array of features. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get_obs() for i in idx]) def get_coordwise_obs(self, coord=None, idx=None): """ Returns N x num_coords x T x coordwsie_dO numpy array of features. """ if idx is None: idx = range(len(self._samples)) return np.asarray([self._samples[i].get_coordwise_obs(coord) for i in idx]) def get_samples(self, idx=None): """ Returns N sample objects. """ if idx is None: idx = range(len(self._samples)) return [self._samples[i] for i in idx] def num_samples(self): """ Returns number of samples. """ return len(self._samples) # Convenience methods. def __len__(self): return self.num_samples() def __getitem__(self, idx): return self.get_samples([idx])[0] class PickleSampleWriter(object): """ Pickles samples into data_file. """ def __init__(self, data_file): self._data_file = data_file def write(self, samples): """ Write samples to data file. """ with open(self._data_file, 'wb') as data_file: cPickle.dump(data_file, samples) class SysOutWriter(object): """ Writes notifications to sysout on sample writes. """ def __init__(self): pass def write(self, samples): """ Write number of samples to sysout. """ LOGGER.debug('Collected %d samples', len(samples)) lto_code/python/gps/sample/sample.py0000644000175000017500000001024713124341100015645 0ustar keke""" This file defines the sample class. """ import numpy as np from gps.proto.gps_pb2 import ACTION class Sample(object): """ Class that handles the representation of a trajectory and stores a single trajectory. Note: must be serializable for easy saving, no C++ references! """ def __init__(self, agent): self.agent = agent self.T = agent.T self.dX = agent.dX self.dU = agent.dU self.dO = agent.dO self.dM = agent.dM # Dictionary containing the sample data from various sensors. self._data = {} self._X = np.empty((self.T, self.dX)) self._X.fill(np.nan) self._obs = np.empty((self.T, self.dO)) self._obs.fill(np.nan) self._meta = np.empty(self.dM) self._meta.fill(np.nan) def set(self, sensor_name, sensor_data, t=None): """ Set trajectory data for a particular sensor. """ if t is None: self._data[sensor_name] = sensor_data self._X.fill(np.nan) # Invalidate existing X. self._obs.fill(np.nan) # Invalidate existing obs. self._meta.fill(np.nan) # Invalidate existing meta data. else: if sensor_name not in self._data: self._data[sensor_name] = \ np.empty((self.T,) + sensor_data.shape) self._data[sensor_name].fill(np.nan) self._data[sensor_name][t, :] = sensor_data self._X[t, :].fill(np.nan) self._obs[t, :].fill(np.nan) def get(self, sensor_name, t=None): """ Get trajectory data for a particular sensor. """ return (self._data[sensor_name] if t is None else self._data[sensor_name][t, :]) def get_X(self, t=None): """ Get the state. Put it together if not precomputed. """ X = self._X if t is None else self._X[t, :] if np.any(np.isnan(X)): for data_type in self._data: if data_type not in self.agent.x_data_types: continue data = (self._data[data_type] if t is None else self._data[data_type][t, :]) self.agent.pack_data_x(X, data, data_types=[data_type]) return X def get_U(self, t=None): """ Get the action. """ return self._data[ACTION] if t is None else self._data[ACTION][t, :] def get_obs(self, t=None): """ Get the observation. Put it together if not precomputed. """ obs = self._obs if t is None else self._obs[t, :] if np.any(np.isnan(obs)): for data_type in self._data: if data_type not in self.agent.obs_data_types: continue if data_type in self.agent.meta_data_types: continue data = (self._data[data_type] if t is None else self._data[data_type][t, :]) self.agent.pack_data_obs(obs, data, data_types=[data_type]) return obs def get_meta(self): """ Get the meta data. Put it together if not precomputed. """ meta = self._meta if np.any(np.isnan(meta)): for data_type in self._data: if data_type not in self.agent.meta_data_types: continue data = self._data[data_type] self.agent.pack_data_meta(meta, data, data_types=[data_type]) return meta def __copy__(self): cls = self.__class__ result = cls.__new__(cls, self.agent) result.__dict__.update(self.__dict__) return result def __deepcopy__(self, memo): cls = self.__class__ result = cls.__new__(cls, self.agent) memo[id(self)] = result for name in self.__dict__: if name != "agent": # Do not deepcopy self.agent setattr(result, name, copy.deepcopy(self.__dict__[name], memo)) return result # For pickling. def __getstate__(self): state = self.__dict__.copy() state.pop('agent') return state # For unpickling. def __setstate__(self, state): self.__dict__ = state self.__dict__['agent'] = None lto_code/python/gps/sample/__init__.py0000644000175000017500000000000013124341100016105 0ustar kekelto_code/python/gps/utility/0000755000175000017500000000000013124341100014230 5ustar kekelto_code/python/gps/utility/gmm.py0000644000175000017500000001752713124341100015376 0ustar keke""" This file defines a Gaussian mixture model class. """ import logging import numpy as np import scipy.linalg LOGGER = logging.getLogger(__name__) def logsum(vec, axis=0, keepdims=True): maxv = np.max(vec, axis=axis, keepdims=keepdims) maxv[maxv == -float('inf')] = 0 return np.log(np.sum(np.exp(vec-maxv), axis=axis, keepdims=keepdims)) + maxv class GMM(object): """ Gaussian Mixture Model. """ def __init__(self, init_sequential=False, eigreg=False, warmstart=True): self.init_sequential = init_sequential self.eigreg = eigreg self.warmstart = warmstart self.sigma = None def inference(self, pts): """ Evaluate dynamics prior. Args: pts: A N x D array of points. """ # Compute posterior cluster weights. logwts = self.clusterwts(pts) # Compute posterior mean and covariance. mu0, Phi = self.moments(logwts) # Set hyperparameters. m = self.N n0 = m - 2 - mu0.shape[0] # Normalize. m = float(m) / self.N n0 = float(n0) / self.N return mu0, Phi, m, n0 def estep(self, data): """ Compute log observation probabilities under GMM. Args: data: A N x D array of points. Returns: logobs: A N x K array of log probabilities (for each point on each cluster). """ # Constants. K = self.sigma.shape[0] Di = data.shape[1] N = data.shape[0] # Compute probabilities. data = data.T mu = self.mu[:, 0:Di].T mu_expand = np.expand_dims(np.expand_dims(mu, axis=1), axis=1) assert mu_expand.shape == (Di, 1, 1, K) # Calculate for each point distance to each cluster. data_expand = np.tile(data, [K, 1, 1, 1]).transpose([2, 3, 1, 0]) diff = data_expand - np.tile(mu_expand, [1, N, 1, 1]) assert diff.shape == (Di, N, 1, K) Pdiff = np.zeros_like(diff) cconst = np.zeros((1, 1, 1, K)) for i in range(K): U = scipy.linalg.cholesky(self.sigma[i, :Di, :Di], check_finite=False) Pdiff[:, :, 0, i] = scipy.linalg.solve_triangular( U, scipy.linalg.solve_triangular( U.T, diff[:, :, 0, i], lower=True, check_finite=False ), check_finite=False ) cconst[0, 0, 0, i] = -np.sum(np.log(np.diag(U))) - 0.5 * Di * \ np.log(2 * np.pi) logobs = -0.5 * np.sum(diff * Pdiff, axis=0, keepdims=True) + cconst assert logobs.shape == (1, N, 1, K) logobs = logobs[0, :, 0, :] + self.logmass.T return logobs def moments(self, logwts): """ Compute the moments of the cluster mixture with logwts. Args: logwts: A K x 1 array of log cluster probabilities. Returns: mu: A (D,) mean vector. sigma: A D x D covariance matrix. """ # Exponentiate. wts = np.exp(logwts) # Compute overall mean. mu = np.sum(self.mu * wts, axis=0) # Compute overall covariance. # For some reason this version works way better than the "right" # one... could we be computing xxt wrong? diff = self.mu - np.expand_dims(mu, axis=0) diff_expand = np.expand_dims(diff, axis=1) * \ np.expand_dims(diff, axis=2) wts_expand = np.expand_dims(wts, axis=2) sigma = np.sum((self.sigma + diff_expand) * wts_expand, axis=0) return mu, sigma def clusterwts(self, data): """ Compute cluster weights for specified points under GMM. Args: data: An N x D array of points Returns: A K x 1 array of average cluster log probabilities. """ # Compute probability of each point under each cluster. logobs = self.estep(data) # Renormalize to get cluster weights. logwts = logobs - logsum(logobs, axis=1) # Average the cluster probabilities. logwts = logsum(logwts, axis=0) - np.log(data.shape[0]) return logwts.T def update(self, data, K, max_iterations=100): """ Run EM to update clusters. Args: data: An N x D data matrix, where N = number of data points. K: Number of clusters to use. """ # Constants. N = data.shape[0] Do = data.shape[1] LOGGER.debug('Fitting GMM with %d clusters on %d points', K, N) if (not self.warmstart or self.sigma is None or K != self.sigma.shape[0]): # Initialization. LOGGER.debug('Initializing GMM.') self.sigma = np.zeros((K, Do, Do)) self.mu = np.zeros((K, Do)) self.logmass = np.log(1.0 / K) * np.ones((K, 1)) self.mass = (1.0 / K) * np.ones((K, 1)) self.N = data.shape[0] N = self.N # Set initial cluster indices. if not self.init_sequential: cidx = np.random.randint(0, K, size=(1, N)) else: raise NotImplementedError() # Initialize. for i in range(K): cluster_idx = (cidx == i)[0] mu = np.mean(data[cluster_idx, :], axis=0) diff = (data[cluster_idx, :] - mu).T sigma = (1.0 / cluster_idx.shape[0]) * (diff.dot(diff.T)) self.mu[i, :] = mu self.sigma[i, :, :] = sigma + np.eye(Do) * 2e-6 prevll = -float('inf') for itr in range(max_iterations): # E-step: compute cluster probabilities. logobs = self.estep(data) # Compute log-likelihood. ll = np.sum(logsum(logobs, axis=1)) LOGGER.debug('GMM itr %d/%d. Log likelihood: %f', itr, max_iterations, ll) if ll < prevll: LOGGER.debug('Log-likelihood decreased! Ending on itr=%d/%d', itr, max_iterations) break if np.abs(ll-prevll) < 1e-5*prevll: LOGGER.debug('GMM converged on itr=%d/%d', itr, max_iterations) break prevll = ll # Renormalize to get cluster weights. logw = logobs - logsum(logobs, axis=1) assert logw.shape == (N, K) # Renormalize again to get weights for refitting clusters. logwn = logw - logsum(logw, axis=0) assert logwn.shape == (N, K) w = np.exp(logwn) # M-step: update clusters. # Fit cluster mass. self.logmass = logsum(logw, axis=0).T self.logmass = self.logmass - logsum(self.logmass, axis=0) assert self.logmass.shape == (K, 1) self.mass = np.exp(self.logmass) # Reboot small clusters. w[:, (self.mass < (1.0 / K) * 1e-4)[:, 0]] = 1.0 / N # Fit cluster means. w_expand = np.expand_dims(w, axis=2) data_expand = np.expand_dims(data, axis=1) self.mu = np.sum(w_expand * data_expand, axis=0) # Fit covariances. wdata = data_expand * np.sqrt(w_expand) assert wdata.shape == (N, K, Do) for i in range(K): # Compute weighted outer product. XX = wdata[:, i, :].T.dot(wdata[:, i, :]) mu = self.mu[i, :] self.sigma[i, :, :] = XX - np.outer(mu, mu) if self.eigreg: # Use eigenvalue regularization. raise NotImplementedError() else: # Use quick and dirty regularization. sigma = self.sigma[i, :, :] self.sigma[i, :, :] = 0.5 * (sigma + sigma.T) + \ 1e-6 * np.eye(Do) lto_code/python/gps/utility/general_utils.py0000644000175000017500000000546313124341100017447 0ustar keke""" This file defines general utility functions and classes. """ import numpy as np class BundleType(object): """ This class bundles many fields, similar to a record or a mutable namedtuple. """ def __init__(self, variables): for var, val in variables.items(): object.__setattr__(self, var, val) # Freeze fields so new ones cannot be set. def __setattr__(self, key, value): if not hasattr(self, key): raise AttributeError("%r has no attribute %s" % (self, key)) object.__setattr__(self, key, value) def check_shape(value, expected_shape, name=''): """ Throws a ValueError if value.shape != expected_shape. Args: value: Matrix to shape check. expected_shape: A tuple or list of integers. name: An optional name to add to the exception message. """ if value.shape != tuple(expected_shape): raise ValueError('Shape mismatch %s: Expected %s, got %s' % (name, str(expected_shape), str(value.shape))) def finite_differences(func, inputs, func_output_shape=(), epsilon=1e-5): """ Computes gradients via finite differences. derivative = (func(x+epsilon) - func(x-epsilon)) / (2*epsilon) Args: func: Function to compute gradient of. Inputs and outputs can be arbitrary dimension. inputs: Vector value to compute gradient at. func_output_shape: Shape of the output of func. Default is empty-tuple, which works for scalar-valued functions. epsilon: Difference to use for computing gradient. Returns: Gradient vector of each dimension of func with respect to each dimension of input. """ gradient = np.zeros(inputs.shape+func_output_shape) for idx, _ in np.ndenumerate(inputs): test_input = np.copy(inputs) test_input[idx] += epsilon obj_d1 = func(test_input) assert obj_d1.shape == func_output_shape test_input = np.copy(inputs) test_input[idx] -= epsilon obj_d2 = func(test_input) assert obj_d2.shape == func_output_shape diff = (obj_d1 - obj_d2) / (2 * epsilon) gradient[idx] += diff return gradient def approx_equal(a, b, threshold=1e-5): """ Return whether two numbers are equal within an absolute threshold. Returns: True if a and b are equal within threshold. """ return np.all(np.abs(a - b) < threshold) def extract_condition(hyperparams, m): """ Pull the relevant hyperparameters corresponding to the specified condition, and return a new hyperparameter dictionary. """ return {var: val[m] if isinstance(val, list) else val for var, val in hyperparams.items()} def replicate_var(val, num_conds): return val if isinstance(val, list) else [val] * num_conds lto_code/python/gps/utility/display.py0000644000175000017500000000605213124341100016252 0ustar kekeimport numpy as np class Display(object): def __init__(self, hyperparams): self._hyperparams = hyperparams self._log_filename = self._hyperparams['log_filename'] self._first_update = True def _output_column_titles(self, algorithm, policy_titles=False): """ Setup iteration data column titles: iteration, average cost, and for each condition the mean cost over samples, step size, linear Guassian controller entropies, and initial/final KL divergences for BADMM. """ condition_titles = '%3s | %8s %12s' % ('', '', '') itr_data_fields = '%3s | %8s %12s' % ('itr', 'avg_cost', 'avg_pol_cost') for m in range(algorithm.M): condition_titles += ' | %8s %9s %-7d' % ('', 'condition', m) itr_data_fields += ' | %8s %8s %8s' % (' cost ', ' step ', 'entropy ') condition_titles += ' %8s %8s %8s' % ('', '', '') itr_data_fields += ' %8s %8s %8s' % ('pol_cost', 'kl_div_i', 'kl_div_f') self.append_output_text(condition_titles) self.append_output_text(itr_data_fields) def _update_iteration_data(self, itr, algorithm, costs, pol_sample_lists): """ Update iteration data information: iteration, average cost, and for each condition the mean cost over samples, step size, linear Guassian controller entropies, and initial/final KL divergences for BADMM. """ avg_cost = np.mean(costs) if pol_sample_lists is not None: pol_costs = [np.mean([np.sum(algorithm.cost[m].eval(pol_sample_lists[m][i],True)[0]) for i in range(len(pol_sample_lists[m]))]) for m in range(algorithm.M)] itr_data = '%3d | %8.2f %12.2f' % (itr, avg_cost, np.mean(pol_costs)) else: pol_costs = None itr_data = '%3d | %8.2f' % (itr, avg_cost) for m in range(algorithm.M): cost = costs[m] step = algorithm.prev[m].step_mult * algorithm.base_kl_step entropy = 2*np.sum(np.log(np.diagonal(algorithm.prev[m].traj_distr.chol_pol_covar, axis1=1, axis2=2))) itr_data += ' | %8.2f %8.4f %8.2f' % (cost, step, entropy) kl_div_i = algorithm.cur[m].pol_info.init_kl.mean() kl_div_f = algorithm.cur[m].pol_info.prev_kl.mean() itr_data += ' %8.2f %8.2f %8.2f' % (pol_costs[m], kl_div_i, kl_div_f) self.append_output_text(itr_data) return pol_costs def update(self, itr, algorithm, agent, traj_sample_lists, pol_sample_lists): if self._first_update: self._output_column_titles(algorithm) self._first_update = False costs = [np.mean(np.sum(algorithm.prev[m].cs, axis=1)) for m in range(algorithm.M)] pol_costs = self._update_iteration_data(itr, algorithm, costs, pol_sample_lists) return costs, pol_costs def append_output_text(self, text): with open(self._log_filename, 'a') as f: f.write(text + '\n') print(text) lto_code/python/gps/utility/__init__.py0000644000175000017500000000000013124341100016327 0ustar kekelto_code/python/gps/__init__.py0000644000175000017500000000010513124341100014632 0ustar keke""" This Python module houses the guided policy search codebase. """ lto_code/src/0000755000175000017500000000000013124341100011202 5ustar kekelto_code/src/proto/0000755000175000017500000000000013124341100012345 5ustar kekelto_code/src/proto/gps.proto0000644000175000017500000000125213124341100014223 0ustar kekesyntax = "proto2"; package gps; // Enum for sample types enum SampleType { ACTION = 0; CUR_LOC = 1; PAST_OBJ_VAL_DELTAS = 2; PAST_GRADS = 3; CUR_GRAD = 4; PAST_LOC_DELTAS = 5; } // Message containing the data for a single sample. message Sample { optional uint32 T = 1; // sample length optional uint32 dX = 2; // dimension of state X optional uint32 dU = 3; // dimension of action U optional uint32 dO = 4; // dimension of observation // Data arrays holding X, U, obs, and meta data. repeated float X = 5 [packed = true]; repeated float U = 6 [packed = true]; repeated float obs = 7 [packed = true]; repeated float meta = 8 [packed = true]; }