├── GOAP.pdf
├── .gitignore
├── src
    ├── json test
    ├── worldstate.cpp
    ├── goap_json_test.cpp
    ├── proposition.cpp
    ├── task_manager.cpp
    ├── goap_test.cpp
    ├── action.cpp
    ├── goap.cpp
    └── action_planner.cpp
├── package.xml
├── README.md
├── License.txt
├── CMakeLists.txt
└── include
    └── task_planning
        ├── worldstate.h
        ├── task_manager.h
        ├── task_manager.tpp
        └── action_planner.h


/GOAP.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/UTNuclearRoboticsPublic/task_planning/HEAD/GOAP.pdf


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | *~
2 | 
3 | # Emacs
4 | .#*
5 | 
6 | # kdevelop config files
7 | .kdev_include_paths
8 | 


--------------------------------------------------------------------------------
/src/json test:
--------------------------------------------------------------------------------
 1 | 	"load": {
 2 | 		"pre": [
 3 | 			{"atom": "loaded", "value": "false"}
 4 | 	    	]
 5 | 		"pst": [
 6 | 			{"atom": "loaded", "value": "true"}
 7 | 		]
 8 | 	}
 9 | 	
10 | 	"shoot": {
11 | 		"pre": [
12 | 			{"atom": "loaded", "value": "true"}
13 | 			{"atom": "enemyalive", "value": "true"}
14 | 		]
15 | 		"pst": [
16 | 			{"atom": "enemyalive", "value": "false"}
17 | 			{"atom": "loaded", "value": "false"}
18 | 		]
19 | 	}
20 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>task_planning</name>
 4 |   <version>0.0.0</version>
 5 |   <description>Provides Goal Oriented Action Planning (GOAP) library</description>
 6 | 
 7 |   <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
 8 |   <license>LANS</license>
 9 |   <author email="blakeanderson@utexas.edu">Blake Anderson</author>
10 | 
11 |   <buildtool_depend>catkin</buildtool_depend>
12 | 
13 |   <depend>roscpp</depend>
14 |   <depend>roslib</depend>
15 | 
16 | </package>
17 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # task_planning
 2 | Goal Oriented Action Planning (GOAP) and task execution.
 3 | 
 4 | The Task Planning package was initially written at Los Alamos National Laboratory (LANL). Later revisions were done at The University of Texas in the Nuclear Robotics Group. The software is open source, see License.txt.
 5 | 
 6 | TASK_PLANNING ROS PACKAGE GUIDE
 7 | ----------------------------------------------------------------
 8 | XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
 9 | ----------------------------------------------------------------
10 | 
11 | TABLE OF CONTENTS
12 |   1 Introduction
13 | 
14 |   2 Overview
15 | 
16 |   3 Usage
17 | 
18 |   4 Contact
19 | 
20 |   5 License
21 | 
22 | 1 INTRODUCTION
23 | 
24 |   This package provides a library of classes implementing Goal Oriented Action Planning (GOAP). Although this is organized as a ROS package, the library itself does not utilize any ROS features.
25 | 
26 | 2 OVERVIEW
27 | 
28 |   The following classes implement the GOAP algorithm:
29 | 
30 |   Worldstate - Stores a vector of boolean variables (atoms) which represent the state of the world and robot.
31 | 
32 |   Action - Modifies the world atoms in some way. An action is defined by its preconditions, postconditions, and cost function.
33 | 
34 |   ActionPlanner - Stores the set of actions and produces action plans using the createPlan() function.
35 | 
36 |   TaskManager - Top level class providing both task planning and execution. It executes task plans by associating callback functions with each action.
37 | 
38 | 3 USAGE
39 | 
40 |   See GOAP.pdf for explanation of action planning principles and example code.
41 |   
42 |   rosrun task_planning goap_test - Performs a test of the ActionPlanner class.
43 | 
44 | 4 CONTACT
45 |   
46 |   The lab members responsible for this package are listed below with their contact information.
47 | 
48 |   Blake Anderson 
49 |   blakeanderson@utexas.edu
50 | 
51 | 5 LICENSE
52 | 
53 |   See License.txt for terms of use for this package.
54 | 
55 | 


--------------------------------------------------------------------------------
/License.txt:
--------------------------------------------------------------------------------
 1 | Copyright (c) 2016, Los Alamos National Security, LLC
 2 | All rights reserved.
 3 | Copyright 2016. Los Alamos National Security, LLC. This software was produced under U.S. Government contract DE-AC52-06NA25396 for Los Alamos National Laboratory (LANL), which is operated by Los Alamos National Security, LLC for the U.S. Department of Energy. The U.S. Government has rights to use, reproduce, and distribute this software.  NEITHER THE GOVERNMENT NOR LOS ALAMOS NATIONAL SECURITY, LLC MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR ASSUMES ANY LIABILITY FOR THE USE OF THIS SOFTWARE.  If software is modified to produce derivative works, such modified software should be clearly marked, so as not to confuse it with the version available from LANL.
 4 | 
 5 | Additionally, redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 
 8 | 3. Neither the name of Los Alamos National Security, LLC, Los Alamos National Laboratory, LANL, the U.S. Government, nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 
 9 | 
10 | THIS SOFTWARE IS PROVIDED BY LOS ALAMOS NATIONAL SECURITY, LLC AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL LOS ALAMOS NATIONAL SECURITY, LLC OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
11 | 


--------------------------------------------------------------------------------
/src/worldstate.cpp:
--------------------------------------------------------------------------------
 1 | ///////////////////////////////////////////////////////////////////////////////
 2 | //      Title     : worldstate.cpp
 3 | //      Project   : task_planning
 4 | //      Created   : 5/25/2016
 5 | //      Author    : Blake Anderson
 6 | //      Platforms : Ubuntu 64-bit
 7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
 8 | //                 
 9 | //          All files within this directory are subject to the following, unless an alternative
10 | //          license is explicitly included within the text of each file.
11 | //
12 | //          This software and documentation constitute an unpublished work
13 | //          and contain valuable trade secrets and proprietary information
14 | //          belonging to the University. None of the foregoing material may be
15 | //          copied or duplicated or disclosed without the express, written
16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
23 | //          University be liable for incidental, special, indirect, direct or
24 | //          consequential damages or loss of profits, interruption of business,
25 | //          or related expenses which may arise from use of software or documentation,
26 | //          including but not limited to those resulting from defects in software
27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
28 | //
29 | ///////////////////////////////////////////////////////////////////////////////
30 | 
31 | #include <task_planning/worldstate.h>
32 | 
33 | namespace goap {
34 | 
35 | void Worldstate::initialize(std::size_t new_size)
36 | {
37 |   values.resize(new_size, false);
38 |   care.resize(new_size, false);
39 | }
40 | 
41 | void Worldstate::clear()
42 | {
43 |   values.clear();
44 |   care.clear();
45 | }
46 | 
47 | } // namespace goap


--------------------------------------------------------------------------------
/src/goap_json_test.cpp:
--------------------------------------------------------------------------------
 1 | /*-------------------------------------------------------------------------------
 2 |  goap_test.cpp
 3 |  Version: In Development
 4 |  Commit Date: Never
 5 | 
 6 |  Authors: Blake Anderson
 7 |  The University of Texas at Austin
 8 |  Department of Mechanical Engineering
 9 |  Nuclear and Applied Robotics Group
10 |  
11 |  Modified from code by Abraham Stolk.
12 | 
13 |  Description: Test script for Goal Oriented Action Planning
14 | 
15 |  Command Line Arguments: None
16 | -------------------------------------------------------------------------------*/
17 | 
18 | /*
19 | Copyright 2012 Abraham T. Stolk
20 | 
21 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
22 | 
23 | http://www.apache.org/licenses/LICENSE-2.0
24 | 
25 | Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
26 | */
27 | 
28 | #include "task_planning/goap.h"	// for planner interface.
29 | 
30 | #include <string.h>
31 | #include <cstdio>
32 | #include <sys/stat.h>
33 | 
34 | #include <ros/ros.h>
35 | #include <ros/package.h>
36 | 
37 | int main( int argc, char* argv[] )
38 | {
39 |   goap::GOAP goap;
40 |   goap::actionplanner_t ap;
41 |   
42 |   std::string file_path = ros::package::getPath("task_planning") + "/json_test";
43 |   std::cout << file_path << std::endl;
44 |   
45 |   if (!goap.load_json(file_path, &ap)) {
46 |     return 1;
47 |   }
48 |   
49 |   goap::worldstate_t fr;
50 |   goap.goap_worldstate_set( &ap, &fr, "loaded", false );
51 |   goap.goap_worldstate_set( &ap, &fr, "enemy_alive", true );
52 | 
53 |   goap.goap_set_cost( &ap, "shoot", 1 );
54 |   goap.goap_set_cost( &ap, "load", 1 );
55 |   
56 |   std::string desc;
57 |   goap.goap_description(&ap, &desc);
58 |   printf("Planner:\n%s", desc.c_str());
59 | 
60 |   goap::worldstate_t goal;
61 |   goap.goap_worldstate_set(&ap, &goal, "enemy_alive", false);
62 | 
63 |   goap.goap_worldstate_description(&ap, &goal, &desc);
64 |   printf("GOAL: %s", desc.c_str());
65 |   //goap_worldstate_set( &ap, &goal, "alive", true ); // add this to avoid suicide actions in plan.
66 | 
67 |   std::vector<goap::worldstate_t> states;
68 |   std::vector<std::string> plan;
69 | 
70 |   const int plancost = goap.astar_plan(&ap, fr, goal, &plan, &states);
71 |   printf("plancost = %d\n", plancost);
72 | 
73 |   goap.goap_worldstate_description(&ap, &fr, &desc);
74 |   printf("Start: %s\n", desc.c_str());
75 |   for (int i = 0; i < plan.size(); ++i)
76 |   {
77 |     goap.goap_worldstate_description(&ap, &states.at(i), &desc);
78 |     printf("%d: %-20s%s\n", i, plan.at(i).c_str(), desc.c_str());
79 |   }
80 | 
81 |   return plancost;
82 | }


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(task_planning)
 3 | 
 4 | set(CMAKE_CXX_FLAGS "-std=c++11")
 5 | 
 6 | ## Find catkin macros and libraries
 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 8 | ## is used, also find other catkin packages
 9 | find_package(catkin REQUIRED
10 |   roscpp
11 |   roslib
12 | )
13 | 
14 | ## System dependencies are found with CMake's conventions
15 | find_package(Boost REQUIRED system chrono)
16 | 
17 | catkin_package(
18 |  INCLUDE_DIRS include
19 |  LIBRARIES task_planner
20 |  CATKIN_DEPENDS roscpp roslib
21 |  DEPENDS Boost
22 | )
23 | 
24 | include_directories(include)
25 | 
26 | add_library(task_planner
27 |   src/task_manager.cpp
28 |   src/action_planner.cpp
29 |   src/action.cpp
30 |   src/worldstate.cpp
31 |   src/proposition.cpp
32 | )
33 | 
34 | ## Declare a cpp executable
35 | add_executable(goap_test src/goap_test.cpp)
36 | 
37 | ## Add cmake target dependencies of the executable/library
38 | ## as an example, message headers may need to be generated before nodes
39 | #add_dependencies(task_planning_node task_planning_generate_messages_cpp)
40 | 
41 | ## Specify libraries to link a library or executable target against
42 | target_link_libraries(task_planner
43 |   ${Boost_LIBRARIES}
44 | )
45 | 
46 | target_link_libraries(goap_test task_planner ${Boost_LIBRARIES})
47 | 
48 | #############
49 | ## Install ##
50 | #############
51 | 
52 | # all install targets should use catkin DESTINATION variables
53 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
54 | 
55 | ## Mark executable scripts (Python etc.) for installation
56 | ## in contrast to setup.py, you can choose the destination
57 | # install(PROGRAMS
58 | #   scripts/my_python_script
59 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
60 | # )
61 | 
62 | ## Mark executables and/or libraries for installation
63 | # install(TARGETS task_planning task_planning_node
64 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
65 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
66 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
67 | # )
68 | 
69 | ## Mark cpp header files for installation
70 | #install(DIRECTORY include/${PROJECT_NAME}/
71 | #  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
72 | #  FILES_MATCHING PATTERN "*.h"
73 | #  PATTERN ".svn" EXCLUDE
74 | #)
75 | 
76 | ## Mark other files for installation (e.g. launch and bag files, etc.)
77 | # install(FILES
78 | #   # myfile1
79 | #   # myfile2
80 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
81 | # )
82 | 
83 | #############
84 | ## Testing ##
85 | #############
86 | 
87 | ## Add gtest based cpp test target and link libraries
88 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_task_planning.cpp)
89 | # if(TARGET ${PROJECT_NAME}-test)
90 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
91 | # endif()
92 | 
93 | ## Add folders to be run by python nosetests
94 | # catkin_add_nosetests(test)
95 | 


--------------------------------------------------------------------------------
/include/task_planning/worldstate.h:
--------------------------------------------------------------------------------
 1 | ///////////////////////////////////////////////////////////////////////////////
 2 | //      Title     : worldstate.h
 3 | //      Project   : task_planning
 4 | //      Created   : 5/25/2016
 5 | //      Author    : Blake Anderson
 6 | //      Platforms : Ubuntu 64-bit
 7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
 8 | //                 
 9 | //          All files within this directory are subject to the following, unless an alternative
10 | //          license is explicitly included within the text of each file.
11 | //
12 | //          This software and documentation constitute an unpublished work
13 | //          and contain valuable trade secrets and proprietary information
14 | //          belonging to the University. None of the foregoing material may be
15 | //          copied or duplicated or disclosed without the express, written
16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
23 | //          University be liable for incidental, special, indirect, direct or
24 | //          consequential damages or loss of profits, interruption of business,
25 | //          or related expenses which may arise from use of software or documentation,
26 | //          including but not limited to those resulting from defects in software
27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
28 | //
29 | ///////////////////////////////////////////////////////////////////////////////
30 | 
31 | /*
32 |  * Defines a world state using a set of boolean variables.
33 |  */
34 | 
35 | #ifndef GOAP_WORLDSTATE_H
36 | #define GOAP_WORLDSTATE_H
37 | 
38 | #define BOOST_DYNAMIC_BITSET_DONT_USE_FRIENDS // needed for hash_value to access m_bits
39 | #include <boost/dynamic_bitset.hpp>
40 | #include <boost/functional/hash/hash.hpp>
41 | 
42 | namespace goap {
43 | 
44 | // Describes the world state by listing values (t/f) for all known atoms.
45 | class Worldstate
46 | {
47 | public:
48 |   boost::dynamic_bitset<> values;  // Values for atoms.
49 |   boost::dynamic_bitset<> care;	   // Mask for atoms that do not matter.
50 |   
51 |   /**
52 |    * @breif Resize values and care
53 |    * @param new_size Set values and care to this size.
54 |    */
55 |   void initialize(std::size_t new_size);
56 |   
57 |   /**
58 |    * @breif Clears values and care.
59 |    */
60 |   void clear();
61 |   
62 |   bool operator==(const Worldstate &rhs) const { return values == rhs.values; }
63 |   
64 |   bool operator<(const Worldstate &rhs) const { 
65 |     hash hasher;
66 |     return hasher(*this) < hasher(rhs);
67 |   }
68 |   
69 |   struct hash
70 |   {
71 |     size_t operator()(const Worldstate& ws) const {
72 |       return boost::hash_value(ws.values.m_bits);
73 |     }
74 |   };
75 | };
76 | 
77 | } // namespace goap
78 | 
79 | #endif


--------------------------------------------------------------------------------
/src/proposition.cpp:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : proposition.cpp
  3 | //      Project   : task_planning
  4 | //      Created   : 6/3/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | #include <task_planning/action_planner.h>
 32 | #include <boost/algorithm/string.hpp>
 33 | #include <boost/tokenizer.hpp>
 34 | #include <sstream>
 35 | 
 36 | namespace goap {
 37 | 
 38 | bool ActionPlanner::Proposition::parse(std::string definition)
 39 | {
 40 |   boost::trim(definition);
 41 |   
 42 |   // Find the start of the parameter list
 43 |   std::size_t param_start = definition.find('(');
 44 |   if (param_start == std::string::npos) {
 45 |     printf("Malformed Proposition, '(' expected: %s\n", definition.c_str());
 46 |     return false;
 47 |   }
 48 |   
 49 |   if (param_start == 0) {
 50 |     printf("Malformed Proposition, missing root: %s\n", definition.c_str());
 51 |     return false;
 52 |   } 
 53 |   
 54 |   std::size_t param_end = definition.find(')', param_start+1);
 55 |   if (param_end == std::string::npos) {
 56 |     printf("Malformed Proposition, ')' expected: %s\n", definition.c_str());
 57 |     return false;
 58 |   }
 59 |   
 60 |   if (param_end < definition.size()-1) {
 61 |     printf("Malformed Proposition, characters found after param: %s\n", definition.c_str());
 62 |     return false;
 63 |   }
 64 |   
 65 |   // Tokenize the parameters
 66 |   std::string param_list = definition.substr(param_start+1);
 67 |   param_list.pop_back(); // Remove ')'
 68 |   
 69 |   typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
 70 | 
 71 |   boost::char_separator<char> sep(",");
 72 |   tokenizer tokens(param_list, sep);
 73 | 
 74 |    for (tokenizer::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter) {
 75 |     std::string param = *tok_iter;
 76 |     
 77 |     parameter new_param;
 78 |     if (!parseParam(param, &new_param)) {
 79 |       printf("Malformed Proposition, failed to parse param: %s\n", param.c_str());
 80 |       return false;
 81 |     }
 82 |     
 83 |     params.push_back(new_param);
 84 |   }
 85 |   
 86 |   root = definition.substr(0, param_start);
 87 |   name = definition;
 88 |   
 89 |   return true;
 90 | }
 91 | 
 92 | void ActionPlanner::Proposition::buildName()
 93 | {
 94 |   std::stringstream output;
 95 |   
 96 |   output << root << "(";
 97 |   
 98 |   for (size_t i = 0, size = params.size(); i < size; ++i) {
 99 |     if (i != 0) {
100 |       output << ", ";
101 |     }
102 |     
103 |     output << params.at(i).label;
104 |   }
105 |   
106 |   output << ")";
107 |   name = output.str();
108 | }
109 | 
110 | bool ActionPlanner::Proposition::parseParam(std::string text, parameter* parameter)
111 | {
112 |   boost::trim(text);
113 |   
114 |   parameter->negated = false;
115 |   parameter->all = false;
116 |   parameter->literal = false;
117 |   parameter->set_members = NULL;
118 |   
119 |   // Check if empty
120 |   if (text.empty()) {
121 |     return true;
122 |   }
123 |   
124 |   // Look for ':' separator and parse the two parts
125 |   std::size_t separator = text.find(':');
126 |   bool separated = (separator != std::string::npos);
127 |   
128 |   if (separated) {
129 |     parameter->entity_set_name = text.substr(separator+1);
130 |     boost::trim(parameter->entity_set_name);
131 |     
132 |     if (parameter->entity_set_name.empty()) {
133 |       printf("Malformed Proposition, empty string following ':'. Expected parameter type.\n.");
134 |       return false;
135 |     }
136 |   }
137 |   
138 |   parameter->label = text.substr(0, separator); // is entire string if no separator
139 |   boost::trim(parameter->label);
140 |   if (parameter->label.empty()) {
141 |     printf("Malformed Proposition, empty parameter label.\n.");
142 |     return false;
143 |   }
144 |   
145 |   // Parse for negated parameter
146 |   if (parameter->label.front() == '~') {
147 |     
148 |     parameter->negated = true;
149 |     parameter->label.erase(0,1); // Remove the '~'
150 |     
151 |     if (parameter->label.empty()) {
152 |       printf("Malformed Proposition, negated parameter must reference an entity set.\n.");
153 |       return false;
154 |     }
155 |   } // end negated parsing
156 |   
157 |   // Parse for literal
158 |   if (parameter->label.front() == '$') {
159 |     parameter->literal = true;
160 |     parameter->label.erase(0,1); // remove the '$'
161 | 
162 |     // Must have entity set
163 |     if (!separated) {
164 |       printf("Malformed Proposition, literal parameter must reference an entity set.\n");
165 |       return false;
166 |     }
167 |   } // end literal parsing
168 |   
169 |   
170 |   // Parse for all parameter
171 |   if (parameter->label == "ALL") {
172 |     
173 |     // Cannot combine all with negated
174 |     if (parameter->negated) {
175 |       printf("Malformed Proposition, can't combine ""ALL"" with ""~""\n");
176 |       return false;
177 |     }
178 |     
179 |     // Cannot combine all with literal
180 |     if (parameter->literal) {
181 |       printf("Malformed Proposition, can't combine ""ALL"" with ""$"" in parameter.\n");
182 |       return false;
183 |     }
184 |     
185 |     // Must have entity set
186 |     if (!separated) {
187 |       printf("Malformed Proposition, ""ALL"" parameter must reference an entity set.\n");
188 |       return false;
189 |     }
190 |     
191 |     parameter->all = true;
192 |   } // end ALL parsing
193 |   
194 |   
195 |   return true;
196 | }
197 | 
198 | ActionPlanner::Proposition::parameter* ActionPlanner::Proposition::getParameter(std::string value)
199 | {
200 |   parameter* param = NULL;
201 |   std::vector<parameter>::iterator it = std::find(params.begin(), params.end(), value);
202 |   
203 |   if (it != params.end()) {
204 |     param = &(*it);
205 |   }
206 |   
207 |   return param;
208 | }
209 | 
210 | } // namespace goap


--------------------------------------------------------------------------------
/src/task_manager.cpp:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : task_manager.cpp
  3 | //      Project   : task_planning
  4 | //      Created   : 5/25/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | #include "task_planning/task_manager.h"
 32 | 
 33 | #include <iostream>
 34 | 
 35 | namespace goap {
 36 | 
 37 | TaskManager::TaskManager(ActionPlanner planner) :
 38 |   planner_(planner)
 39 | {
 40 |   action_counter_ = plan_.end();
 41 |   
 42 |   // Initialize the worldstates
 43 |   planner_.updateWorldstateSize(&start_state_);
 44 |   planner_.updateWorldstateSize(&current_state_);
 45 |   planner_.updateWorldstateSize(&goal_state_);
 46 | }
 47 | 
 48 | bool TaskManager::buildPlan(int* cost, std::vector<Worldstate>* worldstates)
 49 | {
 50 |   int plan_cost = planner_.createPlan(start_state_, goal_state_, &plan_, worldstates);
 51 |   
 52 |   if (plan_cost != -1) {
 53 |     action_counter_ = plan_.begin();
 54 |   }
 55 |   
 56 |   *cost = plan_cost;
 57 |   
 58 |   return plan_cost != -1;
 59 | }
 60 | 
 61 | 
 62 | bool TaskManager::runPlan()
 63 | {
 64 |   action_counter_ = plan_.begin();
 65 |   current_state_ = start_state_;
 66 |   
 67 |   // Iterate until end of action stack
 68 |   while (action_counter_ != plan_.end()) {
 69 |     
 70 |     // Verify that preconditions are met. If not, skip this action
 71 |     bool is_possible;
 72 |     planner_.isActionPossible(&current_state_, *action_counter_, &is_possible);
 73 |     if (!is_possible) {
 74 |       std::cout << "Error in TaskManager::runPlan. Preconditions not met for action " << *action_counter_ << std::endl;
 75 |       action_counter_++;
 76 |     }
 77 |     else {
 78 |       preActionFunction();
 79 |       RETURN_TYPE ret = executeAction(*action_counter_);
 80 |       
 81 |       // Respond to the RETURN_TYPE of the action.
 82 |       switch (ret) {
 83 | 	case CONTINUE:
 84 | 	  // Update worldstate
 85 | 	  planner_.doAction(*action_counter_, &current_state_, &current_state_);
 86 | 	  
 87 | 	  // Move to next action
 88 | 	  action_counter_++;
 89 | 	  break;
 90 | 	case CONTINUE_FAILED:
 91 | 	  action_counter_++;
 92 | 	  break;
 93 | 	case REPEAT:
 94 | 	  // Do not increment the action stack counter
 95 | 	  break;
 96 | 	case BAD_CALL:
 97 | 	  std::cout << "In TaskManager::runPlan. Action " << *action_counter_ << " returned BAD_CALL. Exiting plan.\n";
 98 | 	  // We do not break, so we continue to case FAIL_TERMINATE.
 99 | 	case TERMINATE:
100 | 	  action_counter_ = plan_.end();
101 | 	  return false;
102 | 	case REPLAN:
103 | 	{
104 | 	  int cost;
105 | 	  std::vector<Worldstate> worldstates;
106 | 	  start_state_ = current_state_;
107 | 	  
108 | 	  // Try to build new plan
109 | 	  if (buildPlan(&cost, &worldstates)) {
110 | 	    printf("New Action Plan:\n");
111 | 	    for (int i = 0; i < plan_.size(); ++i) {
112 | 	      printf("%d: %s\n", i, plan_.at(i).c_str());
113 | 	    }
114 | 	    
115 | 	    // Start at beginning of new plan
116 | 	    action_counter_ = plan_.begin();
117 | 	    break;
118 | 	  }
119 | 	  else {
120 | 	    // No valid plan
121 | 	    std::cout << "Error in TaskManager::runPlan. Failed to replan. Valid plan not found. Exiting plan.\n";
122 | 	    action_counter_ = plan_.end();
123 | 	    return false;
124 | 	  }
125 | 	}
126 | 	default:
127 | 	  // Invalid return type
128 | 	  std::cout << "Error in TaskManager::runPlan. Unhandled return type " << ret << ". Exiting plan.\n";
129 | 	  action_counter_ = plan_.end();
130 | 	  return false;
131 |       }
132 |     }
133 |   }
134 |   
135 |   // We reached the end of the action stack
136 |   return true;
137 | }
138 | 
139 | 
140 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback func)
141 | {
142 |   // Check that action exists in the planner
143 |   if (planner_.findAction(action_name) == NULL) {
144 |     std::cout << "Error in TaskManager::setActionCallback. Action " << action_name << " not found.\n";
145 |     return false;
146 |   }
147 |   
148 |   callback_funcs_[action_name] = func;
149 |   return true;
150 | }
151 | 
152 | 
153 | bool TaskManager::setStartStateProposition(std::string atom, bool value)
154 | {
155 |   return planner_.setProposition(&start_state_, atom, value);
156 | }
157 | 
158 | bool TaskManager::setCurrentStateProposition(std::string atom, bool value)
159 | {
160 |   return planner_.setProposition(&current_state_, atom, value);
161 | }
162 | 
163 | bool TaskManager::setGoalStateProposition(std::string atom, bool value)
164 | {
165 |   return planner_.setProposition(&goal_state_, atom, value);
166 | }
167 | 
168 | void TaskManager::clearStartState()
169 | {
170 |   start_state_.clear();
171 | }
172 | 
173 | void TaskManager::clearCurrentState()
174 | {
175 |   current_state_.clear();
176 | }
177 | 
178 | void TaskManager::clearGoalState()
179 | {
180 |   goal_state_.clear();
181 | }
182 | 
183 | action_plan_t TaskManager::getPlan() const
184 | {
185 |   return plan_;
186 | }
187 | 
188 | const goap::ActionPlanner& TaskManager::getPlanner() const
189 | {
190 |   return planner_;
191 | }
192 | 
193 | bool TaskManager::getCurrentStateProposition(std::string atom, bool* result) const
194 | {
195 |   return planner_.getPropositionValue(&current_state_, atom, result);
196 | }
197 | 
198 | void TaskManager::setStartStatetoCurrent()
199 | {
200 |   start_state_ = current_state_;
201 | }
202 | 
203 | std::string TaskManager::getCurrentAction() const
204 | {
205 |   if (action_counter_ != plan_.end()) {
206 |     return *action_counter_;
207 |   }
208 |   
209 |   return "";
210 | }
211 | 
212 | TaskManager::RETURN_TYPE TaskManager::executeAction(std::string action) const
213 | {
214 |   ActionCallback function;
215 |   
216 |   // Check that the function exists
217 |   try {
218 |     function = callback_funcs_.at(action);
219 |   } catch(std::out_of_range) {
220 |     return BAD_CALL;
221 |   }
222 |   
223 |   // Execute callback function and return the result
224 |   return function();
225 | }
226 | 
227 | void TaskManager::preActionFunction() const
228 | {
229 |   std::cout << "Running action " << *action_counter_ << ".\n";
230 | }
231 | 
232 | } // namespace goap


--------------------------------------------------------------------------------
/src/goap_test.cpp:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : goap_test.cpp
  3 | //      Project   : task_planning
  4 | //      Created   : 6/6/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | /*
 32 |  * Tests the goap::ActionPlanner class and its subsidiary classes.
 33 |  * The test simulates a waypoint survey task for a mobile robot.
 34 |  * Uses the GOAP 2.0 algorithm utilizing Action Description Language (ADL)
 35 |  * https://en.wikipedia.org/wiki/Action_description_language
 36 |  * 
 37 |  * Usage: rosrun task_planning goap_test [num_waypoints]
 38 |  */
 39 | 
 40 | #include "task_planning/action_planner.h"
 41 | 
 42 | #include <string.h>
 43 | #include <cstdio>
 44 | #include <sys/stat.h>
 45 | 
 46 | #include <boost/chrono/thread_clock.hpp>
 47 | 
 48 | #include <stdlib.h>
 49 | #include <iostream>
 50 | 
 51 | 
 52 | int main( int argc, char* argv[] )
 53 | {
 54 |   // Get number of waypoints from command line
 55 |   int num_waypoints = 5; // Default
 56 |   if( argc == 2 ) {
 57 |     num_waypoints = atoi(argv[1]);
 58 |   }
 59 |   
 60 |   // Declare ActionPlanner
 61 |   goap::ActionPlanner ap;
 62 |   
 63 |   boost::chrono::thread_clock clock;
 64 |   
 65 |   /*
 66 |    * Define actions using ADL: https://en.wikipedia.org/wiki/Action_description_language
 67 |    * 
 68 |    * Action parameters are defined by (name:type).
 69 |    * This defines the action MoveRobot which takes three parameters.
 70 |    * Preconditions are propositions which must be met in order to perform the action.
 71 |    * Effects are propositions that will be set by the action.
 72 |    * Propositions can have parameters as well. Using a param named in the base definition will link the proposition to it.
 73 |    * 
 74 |    * Requirements:
 75 |    * 	Definitions must use correct syntax.
 76 |    * 	Overloaded actions are not allowed; the root names must be unique.
 77 |    * 	An action must have at least one effect.
 78 |    * 	An action param must have both a name and type.
 79 |    * 	Proposition params only have a name.
 80 |    * 	Actions and propositions are allowed to take no parameters, ie MoveRobot().
 81 |    * 	Negate a proposition by putting ~ in front.
 82 |    */
 83 |   
 84 |   ap.addAction("Survey(r:robot, a:location)",	// Base definition
 85 | 	       {"At(r,a)", "~Is_Surveyed(a)"},	// Precondition list
 86 | 	       {"Is_Surveyed(a)"});		// Effect list
 87 |   
 88 |   ap.addAction("MoveRobot(r:robot, destination:location)",
 89 | 	       {"~Docked(r)", "~At(r,destination)"},
 90 | 	       {"At(r,destination)", "~At(r,~destination:location)"}); // negated parameter
 91 |   
 92 |   // Survey location_3 twice
 93 |   ap.addAction("DoubleCheck(r:robot)",
 94 | 	       {"At(r, $location_3:location)", "Is_Surveyed($location_3:location)"},	// literal parameters
 95 | 	       {"Double_Checked_3()"});							// void parameter
 96 |   
 97 |   
 98 |   ap.addAction("Dock(r:robot)",
 99 | 	       {"~Docked(r)"},
100 | 	       {"Docked(r)", "~At(r,ALL:location)"});	// this is an ALL parameter
101 |   
102 |   ap.addAction("Undock(r:robot)",
103 | 	       {"Docked(r)"},
104 | 	       {"~Docked(r)"});
105 |   
106 |   /*
107 |    * Declare the entities in the worldspace
108 |    * 
109 |    * addEntitySet( set_name, list_of_members )
110 |    * The sets are linked to the param types named in the action definitions.
111 |    * EntitySets can be declared before the actions or vice versa; order doesn't matter.
112 |    */
113 |   ap.addEntitySet("robot", {"Pioneer"});
114 |   
115 |   /*
116 |    * You can specify a range of entities.
117 |    * This example will produce location_1, location_2... location_[num_waypoints}
118 |    */
119 |   ap.addEntitySet("location", num_waypoints);
120 |   
121 |   /*
122 |    * buildActionSpace() takes the declared actions and entities and compiles them.
123 |    * 
124 |    * It creates a version of each actions for each valid permutations of their parameters.
125 |    */
126 |   boost::chrono::thread_clock::time_point build_start = clock.now();
127 |   if (!ap.buildActionSpace()) {
128 |     printf("Error: Failed to build action space.");
129 |     return 1;
130 |   }
131 |   boost::chrono::duration<double> build_duration = clock.now() - build_start;
132 | 
133 |   // Print out the created actions and propositions.
134 |   std::cout << ap.description() << std::endl;
135 |   
136 |   /*
137 |    * Define the initial worldstate.
138 |    * Propositions in a worldspace are initialized to false.
139 |    * Here we set the proposition At(Pioneer, location_1) = true
140 |    */
141 |   goap::Worldstate start;
142 |   ap.setProposition(&start, "Docked(Pioneer)", true);
143 | 
144 |   /*
145 |    * Define the goal worldstate.
146 |    * 
147 |    * You can also set a whole group of propositions using ADL.
148 |    * Below we set Is_Surveyed(location) for all values of location. 
149 |    */
150 |   goap::Worldstate goal;
151 |   ap.setProposition(&goal, "Is_Surveyed(ALL:location)", true);
152 |   ap.setProposition(&goal, "Double_Checked_3()", true);
153 |   ap.setProposition(&goal, "Docked(Pioneer)", true);
154 | 
155 |   // Print the starting worldstate
156 |   std::string desc;
157 |   desc = ap.worldstateDescription(&start);
158 |   printf("Start: %s\n", desc.c_str());
159 |   
160 |   // Print the goal worldstate
161 |   desc = ap.worldstateDescription(&goal);
162 |   printf("GOAL: %s", desc.c_str());
163 | 
164 |   std::vector<goap::Worldstate> states;
165 |   goap::action_plan_t plan;
166 | 
167 |   // Create the action plan
168 |   boost::chrono::thread_clock::time_point planning_start = clock.now();
169 |   const int cost = ap.createPlan(start, goal, &plan, &states);
170 |   boost::chrono::duration<double> planning_duration = clock.now() - planning_start;
171 |   
172 |   // Print the action plan
173 |   printf("ACTION PLAN\nPlan cost = %d\n", cost);
174 |   for (int i = 0; i < plan.size(); ++i)
175 |   {
176 |     desc = ap.worldstateDescription(&states.at(i));
177 |     printf("%d: %-20s\n", i, plan.at(i).c_str());
178 |   }
179 |   
180 |   printf("Build time: %f secs\n",  build_duration.count());
181 |   printf("Plan time: %f secs\n",  planning_duration.count());
182 | 
183 |   return 0;
184 | }


--------------------------------------------------------------------------------
/src/action.cpp:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : action.cpp
  3 | //      Project   : task_planning
  4 | //      Created   : 5/25/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | #include <task_planning/action_planner.h>
 32 | #include <boost/foreach.hpp>
 33 | #include <boost/algorithm/string.hpp>
 34 | #include <boost/tokenizer.hpp>
 35 | #include <sstream>
 36 | 
 37 | namespace goap {
 38 | 
 39 | ActionPlanner::Action::Action() {
 40 |   cost = 0;
 41 |   cost_func = NULL;
 42 | }
 43 | 
 44 | ActionPlanner::Action::Action(int num_atoms) :
 45 |   Action()
 46 | {
 47 |   pre.initialize(num_atoms);
 48 |   pst.initialize(num_atoms);
 49 | }
 50 | 
 51 | bool ActionPlanner::Action::parse(std::string definition, string_vec preconditions, string_vec effects)
 52 | {
 53 |   /** Parse the definition **/
 54 |   boost::trim(definition);
 55 |   
 56 |   // Find the start of the parameter list
 57 |   std::size_t param_start = definition.find('(');
 58 |   if (param_start == std::string::npos) {
 59 |     printf("Malformed action %s. '(' expected.\n", definition.c_str());
 60 |     return false;
 61 |   }
 62 |   
 63 |   if (param_start == 0) {
 64 |     printf("Malformed action %s. Missing root.\n", definition.c_str());
 65 |     return false;
 66 |   }
 67 |   
 68 |   std::size_t param_end = definition.find(')', param_start+1);
 69 |   if (param_end == std::string::npos) {
 70 |     printf("Malformed action %s. ')' expected.\n", definition.c_str());
 71 |     return false;
 72 |   }
 73 |   
 74 |   if (param_end < definition.size()-1) {
 75 |     printf("Malformed action %s. Characters found after param list.\n", definition.c_str());
 76 |     return false;
 77 |   }
 78 |   
 79 |   root = definition.substr(0, param_start);
 80 |   name = definition;
 81 |   
 82 |   // Tokenize the parameters
 83 |   std::string param_list = definition.substr(param_start+1);
 84 |   param_list.pop_back();
 85 |   
 86 |   typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
 87 | 
 88 |   boost::char_separator<char> sep(",");
 89 |   tokenizer tokens(param_list, sep);
 90 | 
 91 |    for (tokenizer::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter) {
 92 |     std::string param = *tok_iter;
 93 |     
 94 |     // Separate the label and type
 95 |     std::size_t param_separator = param.find(':');
 96 |     std::string label = param.substr(0, param_separator);
 97 |     std::string type = param.substr(param_separator+1);
 98 |     
 99 |     boost::trim(label);
100 |     boost::trim(type);
101 |     
102 |     // check if this parameter already exists
103 |     parameter* new_param = getParameter(label);
104 |     if (new_param != NULL) {
105 |       printf("Malformed action %s. Multiple params with label %s.\n", definition.c_str(), label.c_str());
106 |       return false;
107 |     }
108 |     
109 |     // build parameter struct and add to list
110 |     new_param = new parameter;
111 |     new_param->label = label;
112 |     new_param->type = type;
113 |     new_param->set_members = NULL;
114 |     params.push_back(*new_param);
115 |     delete new_param;
116 |   }
117 |   
118 |   /** Add preconditions **/  
119 |   BOOST_FOREACH(std::string precondition, preconditions) {
120 |     if (!buildPrecondition(precondition)) {
121 |       return false;
122 |     }
123 |   }
124 |   
125 |   /** Add effects **/
126 |   if (effects.empty()) {
127 |     printf("Malformed action, no effects found.\n");
128 |     return false;
129 |   }
130 |   BOOST_FOREACH(std::string effect, effects) {
131 |     if (!buildEffect(effect)) {
132 |       return false;
133 |     }
134 |   }
135 |   
136 |   return true;
137 | }
138 | 
139 | bool ActionPlanner::Action::buildPrecondition(std::string definition)
140 | {
141 |   boost::trim(definition);
142 |   
143 |   bool req_value;
144 |   
145 |   // Check if first char is '~' indicating NOT
146 |   if (definition.at(0) == '~') {
147 |     definition.erase(0,1); // erase first character
148 |     req_value = false;
149 |   }
150 |   else {
151 |     req_value = true;
152 |   }
153 |   
154 |   // Parse the definition
155 |   Proposition new_prop;
156 |   if (!new_prop.parse(definition)) {
157 |     return false;
158 |   }
159 |   
160 |   preconditions.push_back(condition_t(new_prop, req_value));
161 |   
162 |   return true;
163 | }
164 | 
165 | bool ActionPlanner::Action::buildEffect(std::string definition)
166 | {
167 |   boost::trim(definition);
168 |   
169 |   bool set_value;
170 |   
171 |   // Check if first char is '~' indicating NOT
172 |   if (definition.at(0) == '~') {
173 |     definition.erase(0,1); // erase first character
174 |     set_value = false;
175 |   }
176 |   else {
177 |     set_value = true;
178 |   }
179 |   
180 |   // Parse the definition
181 |   Proposition new_prop;
182 |   if (!new_prop.parse(definition)) {
183 |     return false;
184 |   }
185 |   
186 |   effects.push_back(condition_t(new_prop, set_value));
187 |   
188 |   return true;
189 | }
190 | 
191 | void ActionPlanner::Action::buildName()
192 | {
193 |   std::stringstream output;
194 |   
195 |   output << root << "(";
196 |   
197 |   for (Action::param_vec::iterator param = params.begin(); param != params.end(); param++) {
198 |     if (param != params.begin()) {
199 |       output << ", ";
200 |     }
201 |     
202 |     output << param->label;
203 |   }
204 |   
205 |   output << ")";
206 |   
207 |   name = output.str();
208 | }
209 | 
210 | void ActionPlanner::Action::set_cost(int val)
211 | {
212 |   cost = val;
213 |   cost_func = NULL;
214 | }
215 | 
216 | void ActionPlanner::Action::set_cost(cost_function func)
217 | {
218 |   cost_func = func;
219 | }
220 | 
221 | int ActionPlanner::Action::get_cost(const ActionPlanner* ap, const Worldstate* ws) const
222 | {
223 |   if (cost_func != NULL) {
224 |     return cost_func(ap, ws);
225 |   }
226 |   return cost;
227 | }
228 | 
229 | ActionPlanner::Action::parameter* ActionPlanner::Action::getParameter(param_label label)
230 | {
231 |   parameter* param = NULL;
232 |   param_vec::iterator it = std::find(params.begin(), params.end(), label);
233 |   
234 |   if (it != params.end()) {
235 |     param = &(*it);
236 |   }
237 |   
238 |   return param;
239 | }
240 | 
241 | } // namespace goap


--------------------------------------------------------------------------------
/include/task_planning/task_manager.h:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : task_manager.h
  3 | //      Project   : task_planning
  4 | //      Created   : 5/25/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | #ifndef GOAP_TASK_MANAGER_H
 32 | #define GOAP_TASK_MANAGER_H
 33 | 
 34 | #include "action_planner.h"
 35 | 
 36 | #include <functional>
 37 | #include <unordered_map>
 38 | 
 39 | #include <boost/foreach.hpp>
 40 | 
 41 | namespace goap {
 42 | 
 43 | /*
 44 |  * Class TaskManager - Encapsulate the planning and execution of complex tasks using GOAP.
 45 |  * 
 46 |  * Actions are associated with callback functions (typedef ActionCallback). The action stack
 47 |  * is run in a loop which reacts to the return codes (enum RETURN_TYPE) of the callback functions.
 48 |  */
 49 | class TaskManager
 50 | {
 51 | public:
 52 |   
 53 |   /*
 54 |    * Callback functions must return this type.
 55 |    * CONTINUE        - Action executed successfully. Modify internal worldstate and continue to next action.
 56 |    * CONTINUE_FAILED - Action failed, but continue anyway. Does not modify the internal worldstate. Subsequent actions may not be possible.
 57 |    * REPEAT          - Action did not execute successfully. Try again.
 58 |    * TERMINATE       - Action did not execute successfully. End the action plan.
 59 |    * REPLAN          - Update the plan and continue. Use this if an external change occurs to the worldstate which we must account for.
 60 |    * BAD_CALL        - Use to flag errors unrelated to the actual execution of the action. A useful distinction for debugging.
 61 |    */
 62 |   enum RETURN_TYPE {CONTINUE, CONTINUE_FAILED, REPEAT, TERMINATE, REPLAN, BAD_CALL};
 63 |   
 64 |   // Action callback function with zero parameters
 65 |   typedef std::function<RETURN_TYPE ()> ActionCallback;
 66 |   
 67 |   // Action callback function with one parameter
 68 |   template <typename A>
 69 |   using ActionCallback_1 = std::function<RETURN_TYPE (A)>;
 70 |   
 71 |   // Action callback function with two parameters
 72 |   template <typename A, typename B>
 73 |   using ActionCallback_2 = std::function<RETURN_TYPE (A, B)>;
 74 |   
 75 |   // Action callback function with three parameters
 76 |   template <typename A, typename B, typename C>
 77 |   using ActionCallback_3 = std::function<RETURN_TYPE (A, B, C)>;
 78 |   
 79 |   // Action callback function with four parameters
 80 |   template <typename A, typename B, typename C, typename D>
 81 |   using ActionCallback_4 = std::function<RETURN_TYPE (A, B, C, D)>;
 82 |   
 83 |   // Action callback function with five parameters
 84 |   template <typename A, typename B, typename C, typename D, typename E>
 85 |   using ActionCallback_5 = std::function<RETURN_TYPE (A, B, C, D, E)>;
 86 |   
 87 |   /**
 88 |    * @brief Constructor
 89 |    * @param planner The planner to associate with this TaskManager. Immutable after construction.
 90 |    */
 91 |   TaskManager(ActionPlanner planner);
 92 |   
 93 |   /**
 94 |    * @brief Calls the createPlan method of planner_
 95 |    * @param cost Outputs the cost of the created plan.
 96 |    * @param worldstates Outputs the worldstate sequence of the created plan.
 97 |    * @return True on success. False if no valid plan was found.
 98 |    */
 99 |   bool buildPlan(int* cost, std::vector<Worldstate>* worldstates);
100 |   
101 |   /**
102 |    * @brief Execute the action plan.
103 |    * @return True if the end of the plan was reached. False if an error, BAD_CALL or TERMINATE were encountered.
104 |    */
105 |   bool runPlan();
106 |   
107 |   /**
108 |    * @brief Associate a callback function with the specified action
109 |    * @param action_name The name of the action.
110 |    * @param func The callback function to associate with action. See ActionCallback definition.
111 |    * @return True on success. False if the action was not found.
112 |    */
113 |   bool setActionCallback(std::string action_name, ActionCallback func);
114 |   
115 |   template <typename A>
116 |   bool setActionCallback(std::string action_name, ActionCallback_1<A> func, std::vector<A> first);
117 |   
118 |   template <typename A, typename B>
119 |   bool setActionCallback(std::string action_name, ActionCallback_2<A,B> func, std::vector<A> first, std::vector<B> second);
120 |   
121 |   template <typename A, typename B, typename C>
122 |   bool setActionCallback(std::string action_name, ActionCallback_3<A,B,C> func, std::vector<A> first, std::vector<B> second, std::vector<C> third);
123 |   
124 |   template <typename A, typename B, typename C, typename D>
125 |   bool setActionCallback(std::string action_name, ActionCallback_4 <A,B,C,D> func, std::vector<A> first, std::vector<B> second, std::vector<C> third, std::vector<D> fourth);
126 |   
127 |   template <typename A, typename B, typename C, typename D, typename E>
128 |   bool setActionCallback(std::string action_name, ActionCallback_5<A,B,C,D,E> func, std::vector<A> first, std::vector<B> second, std::vector<C> third, std::vector<D> fourth, std::vector<E> fifth);
129 |   
130 |   /**
131 |    * @brief Set an atom of the start state. The atom must exist in the planner.
132 |    * @param atom Name of the atom to be modified
133 |    * @param value The value to be set
134 |    * @return True on success.
135 |    */
136 |   bool setStartStateProposition(std::string atom, bool value);
137 |   
138 |   /**
139 |    * @brief Set an atom of the current state. The atom must exist in the planner.
140 |    * @param atom Name of the atom to be modified
141 |    * @param value The value to be set
142 |    * @return True on success.
143 |    */
144 |   bool setCurrentStateProposition(std::string atom, bool value);
145 |   
146 |   /**
147 |    * @brief Set an atom of the goal state. The atom must exist in the planner.
148 |    * @param atom Name of the atom to be modified
149 |    * @param value The value to be set
150 |    * @return True on success.
151 |    */
152 |   bool setGoalStateProposition(std::string atom, bool value);
153 |   
154 |   /**
155 |    * @brief Clear the start state.
156 |    */
157 |   void clearStartState();
158 |   
159 |   /**
160 |    * @brief Clear the current state.
161 |    */
162 |   void clearCurrentState();
163 |   
164 |   /**
165 |    * @brief Clear the goal state.
166 |    */
167 |   void clearGoalState();
168 |   
169 |   /**
170 |    * @brief Return the plan held by the manager.
171 |    * @return The action plan.
172 |    */
173 |   action_plan_t getPlan() const;
174 |   
175 |   /**
176 |    * @brief Return const reference to the action planner.
177 |    * @return The action planner
178 |    */
179 |   const goap::ActionPlanner& getPlanner() const;
180 |   
181 |   /**
182 |    * @brief Get the current value of an atom.
183 |    * @param atom Name of the atom to be retrieved
184 |    * @param result Outputs the value of the atom
185 |    * @return True on success.
186 |    */
187 |   bool getCurrentStateProposition(std::string atom, bool* result) const;
188 |   
189 |   /**
190 |    * @breif Sets the start state equal to the current state
191 |    */
192 |   void setStartStatetoCurrent();
193 |   
194 |   /**
195 |    * @brief Get the name of the current action while the plan is running.
196 |    * @return The action name. Returns empty string if the plan is not running.
197 |    */
198 |   std::string getCurrentAction() const;
199 |   
200 | private:
201 |   
202 |   /**
203 |    * @brief Calls the callback function of the given action.
204 |    * @param action The action to execute
205 |    * @return Echos the return type of the callback function. Returns BAD_CALL if the action has no associated callback.
206 |    */
207 |   RETURN_TYPE executeAction(std::string action) const;
208 |   
209 |   /**
210 |    * @brief This function is called prior to each action when running the action stack.
211 |    */
212 |   virtual void preActionFunction() const;
213 |     
214 |   bool actionPermuteHelper(std::string action_name, ActionPlanner::action_vector* permuted_actions);
215 |   
216 |   // GOAP planner used by this manager. It is ummutable after the TaskManager object is constructed.
217 |   const ActionPlanner planner_;
218 |   
219 |   // Stores generated action plans.
220 |   action_plan_t plan_;
221 |   
222 |   // Stores the current location in the action stack.
223 |   std::vector<std::string>::iterator action_counter_;
224 |   
225 |   // Track the necessary world states
226 |   Worldstate start_state_, current_state_, goal_state_;
227 |   
228 |   // Stores the callback functions associated with the actions
229 |   std::unordered_map<std::string, ActionCallback> callback_funcs_;
230 | };
231 | 
232 | /** Template definitions **/
233 | #include "task_manager.tpp"
234 |   
235 | } // namespace goap
236 | 
237 | #endif


--------------------------------------------------------------------------------
/include/task_planning/task_manager.tpp:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : task_manager.tpp
  3 | //      Project   : task_planning
  4 | //      Created   : 6/12/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | // Contains the template definitions for TaskManager
 32 | 
 33 | template <typename A>
 34 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback_1<A> func, std::vector<A> first)
 35 | {
 36 |   const std::size_t arrity = 0;
 37 |   
 38 |   ActionPlanner::action_vector permuted_actions;
 39 |   
 40 |   if (!actionPermuteHelper(action_name, &permuted_actions)) {
 41 |     return false;
 42 |   }
 43 |   
 44 |   BOOST_FOREACH (ActionPlanner::Action action, permuted_actions) {
 45 |     const ActionPlanner::Action::parameter* param;
 46 |     try {
 47 |       param = &action.params.at(arrity);
 48 |     } catch (std::out_of_range) {
 49 |       std::cout << "TaskManager::setActionCallback failed. Expected action with " << arrity+1 << " parameter(s)." << std::endl;
 50 |       return false;
 51 |     }
 52 |     
 53 |     // Get index of parameter in entity list
 54 |     std::vector<std::string>::const_iterator it = std::find(param->set_members->begin(), param->set_members->end(), param->label);
 55 |     std::size_t index = it - param->set_members->begin();
 56 | 
 57 |     A* func_arg;
 58 |     try {
 59 |       func_arg = &first.at(index);
 60 |     } catch (std::out_of_range) {
 61 |       std::cout << "TaskManager::setActionCallback failed. Received insufficient arguments in vector." << std::endl;
 62 |       return false;
 63 |     }
 64 | 
 65 |     ActionCallback new_func = std::bind(func, *func_arg);
 66 |     if (!setActionCallback(action.name, new_func)) {
 67 |       return false;
 68 |     }
 69 |   }
 70 |   return true;
 71 | }
 72 | 
 73 | 
 74 | template <typename A, typename B>
 75 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback_2<A,B> func, std::vector<A> first, std::vector<B> second)
 76 | {
 77 |   const std::size_t arrity = 1;
 78 |   
 79 |   ActionPlanner::action_vector permuted_actions;
 80 |   
 81 |   if (!actionPermuteHelper(action_name, &permuted_actions)) {
 82 |     return false;
 83 |   }
 84 |   
 85 |   BOOST_FOREACH (ActionPlanner::Action action, permuted_actions) {
 86 |     const ActionPlanner::Action::parameter* param;
 87 |     try {
 88 |       param = &action.params.at(arrity);
 89 |     } catch (std::out_of_range) {
 90 |       std::cout << "TaskManager::setActionCallback failed. Expected action with " << arrity+1 << " parameter(s)." << std::endl;
 91 |       return false;
 92 |     }
 93 |     
 94 |     // Get index of parameter in entity list
 95 |     std::vector<std::string>::const_iterator it = std::find(param->set_members->begin(), param->set_members->end(), param->label);
 96 |     std::size_t index = it - param->set_members->begin();
 97 | 
 98 |     B* func_arg;
 99 |     try {
100 |       func_arg = &second.at(index);
101 |     } catch (std::out_of_range) {
102 |       std::cout << "TaskManager::setActionCallback failed. Received insufficient arguments in vector." << std::endl;
103 |       return false;
104 |     }
105 | 
106 |     ActionCallback_1<A> new_func = std::bind(func, std::placeholders::_1, *func_arg);
107 |     if (!setActionCallback(action.name, new_func, first)) {
108 |       return false;
109 |     }
110 |   }
111 |   return true;
112 | }
113 | 
114 | 
115 | template <typename A, typename B, typename C>
116 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback_3<A,B,C> func, std::vector<A> first, std::vector<B> second, std::vector<C> third)
117 | {
118 |   const std::size_t arrity = 2;
119 |   
120 |   ActionPlanner::action_vector permuted_actions;
121 |   
122 |   if (!actionPermuteHelper(action_name, &permuted_actions)) {
123 |     return false;
124 |   }
125 |   
126 |   BOOST_FOREACH (ActionPlanner::Action action, permuted_actions) {
127 |     const ActionPlanner::Action::parameter* param;
128 |     try {
129 |       param = &action.params.at(arrity);
130 |     } catch (std::out_of_range) {
131 |       std::cout << "TaskManager::setActionCallback failed. Expected action with " << arrity+1 << " parameter(s)." << std::endl;
132 |       return false;
133 |     }
134 |     
135 |     // Get index of parameter in entity list
136 |     std::vector<std::string>::const_iterator it = std::find(param->set_members->begin(), param->set_members->end(), param->label);
137 |     std::size_t index = it - param->set_members->begin();
138 | 
139 |     C* func_arg;
140 |     try {
141 |       func_arg = &third.at(index);
142 |     } catch (std::out_of_range) {
143 |       std::cout << "TaskManager::setActionCallback failed. Received insufficient arguments in vector." << std::endl;
144 |       return false;
145 |     }
146 | 
147 |     ActionCallback_2<A,B> new_func = std::bind(func, std::placeholders::_1, std::placeholders::_2, *func_arg);
148 |     if (!setActionCallback(action.name, new_func, first, second)) {
149 |       return false;
150 |     }
151 |   }
152 |   return true;
153 | }
154 | 
155 | 
156 | template <typename A, typename B, typename C, typename D>
157 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback_4 <A,B,C,D> func, std::vector<A> first, std::vector<B> second, std::vector<C> third, std::vector<D> fourth)
158 | {
159 |   const std::size_t arrity = 3;
160 |   
161 |   ActionPlanner::action_vector permuted_actions;
162 |   
163 |   if (!actionPermuteHelper(action_name, &permuted_actions)) {
164 |     return false;
165 |   }
166 |   
167 |   BOOST_FOREACH (ActionPlanner::Action action, permuted_actions) {
168 |     const ActionPlanner::Action::parameter* param;
169 |     try {
170 |       param = &action.params.at(arrity);
171 |     } catch (std::out_of_range) {
172 |       std::cout << "TaskManager::setActionCallback failed. Expected action with " << arrity+1 << " parameter(s)." << std::endl;
173 |       return false;
174 |     }
175 |     
176 |     // Get index of parameter in entity list
177 |     std::vector<std::string>::const_iterator it = std::find(param->set_members->begin(), param->set_members->end(), param->label);
178 |     std::size_t index = it - param->set_members->begin();
179 | 
180 |     D* func_arg;
181 |     try {
182 |       func_arg = &fourth.at(index);
183 |     } catch (std::out_of_range) {
184 |       std::cout << "TaskManager::setActionCallback failed. Received insufficient arguments in vector." << std::endl;
185 |       return false;
186 |     }
187 | 
188 |     ActionCallback_3<A,B,C> new_func = std::bind(func, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, *func_arg);
189 |     if (!setActionCallback(action.name, new_func, first, second, third)) {
190 |       return false;
191 |     }
192 |   }
193 |   return true;
194 | }
195 | 
196 | template <typename A, typename B, typename C, typename D, typename E>
197 | bool TaskManager::setActionCallback(std::string action_name, ActionCallback_5<A,B,C,D,E> func, std::vector<A> first, std::vector<B> second, std::vector<C> third, std::vector<D> fourth, std::vector<E> fifth)
198 | {
199 |   const std::size_t arrity = 4;
200 |   
201 |   ActionPlanner::action_vector permuted_actions;
202 |   
203 |   if (!actionPermuteHelper(action_name, &permuted_actions)) {
204 |     return false;
205 |   }
206 |   
207 |   BOOST_FOREACH (ActionPlanner::Action action, permuted_actions) {
208 |     const ActionPlanner::Action::parameter* param;
209 |     try {
210 |       param = &action.params.at(arrity);
211 |     } catch (std::out_of_range) {
212 |       std::cout << "TaskManager::setActionCallback failed. Expected action with " << arrity+1 << " parameter(s)." << std::endl;
213 |       return false;
214 |     }
215 |     
216 |     // Get index of parameter in entity list
217 |     std::vector<std::string>::const_iterator it = std::find(param->set_members->begin(), param->set_members->end(), param->label);
218 |     std::size_t index = it - param->set_members->begin();
219 | 
220 |     E* func_arg;
221 |     try {
222 |       func_arg = &fifth.at(index);
223 |     } catch (std::out_of_range) {
224 |       std::cout << "TaskManager::setActionCallback failed. Received insufficient arguments in vector." << std::endl;
225 |       return false;
226 |     }
227 | 
228 |     ActionCallback_4<A,B,C,D> new_func = std::bind(func, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, *func_arg);
229 |     if (!setActionCallback(action.name, new_func, first, second, third, fourth)) {
230 |       return false;
231 |     }
232 |   }
233 |   return true;
234 | }
235 | 
236 | bool TaskManager::actionPermuteHelper(std::string action_name, ActionPlanner::action_vector* permuted_actions)
237 | {
238 |   
239 |   permuted_actions->clear();
240 |   
241 |   // Look in built actions
242 |   const ActionPlanner::Action* action_ptr = planner_.findAction(action_name);
243 |   
244 |   // If found
245 |   if (action_ptr != NULL) {
246 |     permuted_actions->push_back(*action_ptr);
247 |   }
248 |   else {
249 |     // Look in unbuilt actions
250 |     action_ptr = planner_.findUnbuiltAction(action_name);
251 |     // If found
252 |     if (action_ptr != NULL) {
253 |       ActionPlanner::Action action = *action_ptr;
254 |       
255 |       if (!planner_.linkEntities(action)) {
256 | 	return false;
257 |       }
258 |       
259 |       // Permute
260 |       if (!planner_.permuteAction(action, permuted_actions)) {
261 | 	return false;
262 |       }
263 |       
264 |     }
265 |     else {
266 |       std::cout << "setActionCallback() - Action " << action_name << " not found." << std::endl;
267 |       return false;
268 |     }
269 |   }
270 |   
271 |   return true;
272 | }


--------------------------------------------------------------------------------
/src/goap.cpp:
--------------------------------------------------------------------------------
  1 | /*-------------------------------------------------------------------------------
  2 |  goap.cpp
  3 |  Version: In Development
  4 |  Commit Date: Never
  5 | 
  6 |  Authors: Blake Anderson
  7 |  The University of Texas at Austin
  8 |  Department of Mechanical Engineering
  9 |  Nuclear and Applied Robotics Group
 10 |  
 11 |  Modified from code by Abraham Stolk.
 12 | 
 13 |  Description: The GOAP class provides an interface for defining a Goal Oriented Action Planner.
 14 | 
 15 |  Command Line Arguments: None
 16 | -------------------------------------------------------------------------------*/
 17 | 
 18 | /*
 19 | Copyright 2012 Abraham T. Stolk
 20 | 
 21 | Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at
 22 | 
 23 | http://www.apache.org/licenses/LICENSE-2.0
 24 | 
 25 | Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
 26 | */
 27 | 
 28 | #include "task_planning/goap.h"
 29 | 
 30 | namespace goap
 31 | {
 32 | 
 33 | GOAP::GOAP() {}
 34 | 
 35 | bool GOAP::load_json(std::string filename, actionplanner_t* ap)
 36 | {
 37 |   Json::Value root;
 38 |   Json::Reader reader;
 39 |   std::ifstream json_stream(filename.c_str(), std::ifstream::in);
 40 |   bool parse_success = reader.parse(json_stream, root);
 41 |   if ( !parse_success )
 42 |   {
 43 |     std::stringstream error_ss;
 44 |     std::cout << "Failed to parse JSON file\n" << reader.getFormattedErrorMessages();
 45 |     
 46 |     return false;
 47 |   }
 48 |   
 49 |   // Clear old planner data
 50 |   goap_actionplanner_clear(ap);
 51 |   
 52 |   // Iterate over the actions
 53 |   for (Json::Value::iterator action = root.begin(); action != root.end(); action++) {
 54 |     std::string action_name = action.memberName();
 55 |     
 56 |     Json::Value pre = (*action)["pre"];
 57 |     Json::Value pst = (*action)["pst"];
 58 |     
 59 |     // Iterate over preconditions
 60 |     for (Json::Value::iterator precondition = pre.begin(); precondition != pre.end(); precondition++) {
 61 |       
 62 |       std::string atom = (*precondition)["atom"].asString();
 63 |       bool value = (*precondition)["value"].asBool();
 64 |       goap_set_pre(ap, action_name, atom, value);
 65 |     }
 66 |     
 67 |     // Iterate over postconditions
 68 |     for (Json::Value::iterator postconditions = pst.begin(); postconditions != pst.end(); postconditions++) {
 69 |       
 70 |       std::string atom = (*postconditions)["atom"].asString();
 71 |       bool value = (*postconditions)["value"].asBool();
 72 |       goap_set_pst(ap, action_name, atom, value);
 73 |     }
 74 |   }
 75 |   
 76 |   return true;
 77 | }
 78 | 
 79 | bool GOAP::find_atom_index(actionplanner_t* ap, std::string atomname, int* index)
 80 | {
 81 |   std::vector<std::string> *vec_ptr = &(ap->atm_names);
 82 |   if (vec_ptr->empty())
 83 |     return false;
 84 | 
 85 |   for (int i = 0; i < vec_ptr->size(); i++) {
 86 |     if (vec_ptr->at(i) == atomname) {
 87 |       *index = i;
 88 |       return true;
 89 |     }
 90 |   }
 91 | 
 92 |   return false;
 93 | }
 94 | 
 95 | bool GOAP::find_action_index(actionplanner_t* ap, std::string actionname, int* index)
 96 | {
 97 |   std::vector<action_t> *vec_ptr = &(ap->actions);
 98 |   if (vec_ptr->empty())
 99 |     return false;
100 |   
101 |   for (int i = 0; i < vec_ptr->size(); i++) {
102 |     if (vec_ptr->at(i).name == actionname) {
103 |       *index = i;
104 |       return true;
105 |     }
106 |   }
107 | 
108 |   return false;
109 | }
110 | 
111 | bool GOAP::add_atom_to_planner(actionplanner_t* ap, std::string atomname, int* index)
112 | {
113 |   if (!find_atom_index(ap, atomname, index)) {
114 |     ap->atm_names.push_back(atomname);
115 |     *index = ap->atm_names.size() - 1;
116 |     
117 |     std::size_t num_atms = ap->atm_names.size();
118 | 
119 |     for (std::vector<action_t>::iterator it = ap->actions.begin(); it != ap->actions.end(); it++) {
120 |       goap_worldstate_clear(&it->pre, num_atms);
121 |       goap_worldstate_clear(&it->pst, num_atms);
122 |     }
123 |     
124 |     return true;
125 |   }
126 | 
127 |   return false;
128 | }
129 | 
130 | bool GOAP::add_action_to_planner(actionplanner_t* ap, std::string actionname, int* index)
131 | {
132 |   if (!find_action_index(ap, actionname, index)) {
133 |     action_t new_act;
134 |     new_act.name = actionname;
135 |  
136 |     std::size_t num_atms = ap->atm_names.size();
137 |     goap_worldstate_clear(&new_act.pre, num_atms);
138 |     goap_worldstate_clear(&new_act.pst, num_atms);
139 |     
140 |     ap->actions.push_back(new_act);
141 |     
142 |     int f = ap->actions.size();
143 |     *index = ap->actions.size() - 1;
144 |     return true;
145 |   }
146 |   
147 |   return false;
148 | }
149 | 
150 | void GOAP::goap_actionplanner_clear(actionplanner_t* ap)
151 | {
152 |   ap->atm_names.clear();
153 |   ap->actions.clear();
154 | }
155 | 
156 | void GOAP::goap_worldstate_clear(worldstate_t* ws, std::size_t new_size)
157 | {
158 |   ws->values.resize(new_size, false);
159 |   ws->care.resize(new_size, false);
160 | }
161 | 
162 | bool GOAP::goap_worldstate_set(actionplanner_t* ap, worldstate_t* ws, std::string atomname, bool value)
163 | {
164 |   int index;
165 |   if(!find_atom_index(ap, atomname, &index)) {
166 |     printf("Error in GOAP::goap_worldstate_set: Atom %s not found in action planner.\n", atomname.c_str());
167 |     return false;
168 |   }
169 |   
170 |   std::size_t num_atms = ap->atm_names.size();	
171 |   goap_worldstate_clear(ws, num_atms);
172 |   
173 |   ws->values[index] = value;
174 |   ws->care[index] = true;
175 | 
176 |   return true;
177 | }
178 | 
179 | bool GOAP::goap_get_atom_value(actionplanner_t* ap, worldstate_t* ws, std::string atomname, bool* result) {
180 |   int index;
181 |   if(!find_atom_index(ap, atomname, &index)) {
182 |     printf("Error in GOAP::goap_get_atom_value: Atom %s not found in action planner.\n", atomname.c_str());
183 |     return false;
184 |   }
185 |   
186 |   *result = ws->values[index];
187 |   return true;
188 | }
189 | 
190 | void GOAP::goap_set_pre(actionplanner_t* ap, std::string actionname, std::string atomname, bool value)
191 | {
192 |   int action_index;
193 |   add_action_to_planner(ap, actionname, &action_index);
194 |   
195 |   int atom_index;
196 |   add_atom_to_planner(ap, atomname, &atom_index);
197 | 
198 |   worldstate_t* preconditions = &ap->actions.at(action_index).pre;
199 |   goap_worldstate_set(ap, preconditions, atomname, value);
200 | }
201 | 
202 | void GOAP::goap_set_pst(actionplanner_t* ap, std::string actionname, std::string atomname, bool value)
203 | {
204 |   int action_index;
205 |   add_action_to_planner(ap, actionname, &action_index);
206 |   
207 |   int atom_index;
208 |   add_atom_to_planner(ap, atomname, &atom_index);
209 | 
210 |   worldstate_t* postconditions = &ap->actions.at(action_index).pst;
211 |   goap_worldstate_set(ap, postconditions, atomname, value);
212 | }
213 | 
214 | bool GOAP::goap_set_cost(actionplanner_t* ap, std::string actionname, float cost)
215 | {
216 |   int index;
217 |   if (!find_action_index(ap, actionname, &index)) {
218 |     printf("Error in GOAP::goap_set_cost: Action %s not found in action planner.\n", actionname.c_str());
219 |     return false;
220 |   }
221 |   
222 |   ap->actions.at(index).set_cost(cost);
223 |   return true;
224 | }
225 | 
226 | bool GOAP::goap_set_cost(actionplanner_t* ap, std::string actionname, float (*func)(actionplanner_t* ap, worldstate_t* ws))
227 | {
228 |   int index;
229 |   if (!find_action_index(ap, actionname, &index)) {
230 |     printf("Error in GOAP::goap_set_cost: Action %s not found in action planner.\n", actionname.c_str());
231 |     return false;
232 |   }
233 |   
234 |   ap->actions.at(index).set_cost(func);
235 |   return true;
236 | }
237 | 
238 | void GOAP::goap_worldstate_description(actionplanner_t* ap, worldstate_t* ws, std::string* output)
239 | {
240 |   goap_worldstate_clear(ws, ap->atm_names.size());
241 |   
242 |   std::stringstream sstream;
243 |   for (uint i = 0; i < ap->atm_names.size(); i++) {
244 |     
245 |     if (ws->care[i]) {
246 |       std::string atom_name = ap->atm_names.at(i);
247 |       
248 |       if (ws->values[i]) {
249 | 	boost::to_upper(atom_name);
250 |       }
251 |       
252 |       sstream << atom_name << ", ";
253 |     }
254 |   }
255 |   *output = sstream.str();
256 | }
257 | 
258 | void GOAP::goap_description(actionplanner_t* ap, std::string* output)
259 | {
260 |   std::stringstream sstream;
261 |   for (std::vector<action_t>::iterator it = ap->actions.begin(); it != ap->actions.end(); it++) {
262 |     sstream << *output << it->name << "\n" << "Preconditions\n";
263 |     
264 |     worldstate_t* pre = &(it->pre);
265 |     worldstate_t* pst = &(it->pst);
266 |     
267 |     for (std::size_t i = 0; i < pre->values.size(); i++) {
268 |       if (pre->care[i])
269 | 	sstream << *output << ap->atm_names.at(i) << ": " << pre->values[i] << "\n";
270 |     }
271 |     sstream << "Postconditions\n";
272 |     
273 |     for (std::size_t i = 0; i < pst->values.size(); i++) {
274 |       if (pst->care[i])
275 | 	sstream << ap->atm_names.at(i) << ": " << pst->values[i] << "\n";
276 |     }
277 |   }
278 |   *output = sstream.str();
279 | }
280 | 
281 | 
282 | bool GOAP::do_action(actionplanner_t* ap, std::string actionname, worldstate_t fr, worldstate_t* to)
283 | {
284 |   int index;
285 |   if (find_action_index(ap, actionname, &index)) {
286 |   
287 |     do_action(ap, index, fr, to);
288 |     return true;
289 |   }
290 |   
291 |   return false;
292 | }
293 | 
294 | int GOAP::calc_h(const worldstate_t* fr, const worldstate_t* to )
295 | {
296 |   const boost::dynamic_bitset<> diff = ( (fr->values ^ to->values) & to->care );
297 |   return (int)diff.count();
298 | }
299 | 
300 | bool GOAP::idx_in_opened(const worldstate_t* ws, int* index)
301 | {
302 |   for (int i = 0; i < opened.size(); i++) {
303 |     if (opened.at(i).ws.values == ws->values) {
304 |       *index = i;
305 |       return true;
306 |     }
307 |   }
308 |   return false;
309 | }
310 | 
311 | bool GOAP::idx_in_closed(const worldstate_t* ws, int* index)
312 | {
313 |   for (int i = 0; i < closed.size(); i++) {
314 |     if (closed.at(i).ws.values == ws->values) {
315 |       *index = i;
316 |       return true;
317 |     }
318 |   }
319 |   return false;
320 | }
321 | 
322 | void GOAP::reconstruct_plan(actionplanner_t* ap, astarnode_t* goalnode, ActionPlan_t* plan, std::vector<worldstate_t>* worldstates)
323 | {
324 |   astarnode_t* curnode = goalnode;
325 |   while (curnode != NULL && curnode->actionname != "start")
326 |   {
327 |     plan->insert(plan->begin(), curnode->actionname);
328 |     worldstates->insert(worldstates->begin(), curnode->ws);
329 |     
330 |     int index;
331 |     if (idx_in_closed(&curnode->parentws, &index))
332 |       curnode = &closed.at(index);
333 |     else
334 |       curnode = NULL;
335 |   }
336 | }
337 | 
338 | std::vector<astarnode_t>::iterator GOAP::get_lowest_rank()
339 | {
340 |   std::vector<astarnode_t>::iterator lowest_rank = opened.begin();
341 |   for (std::vector<astarnode_t>::iterator it = opened.begin(); it != opened.end(); it++) {
342 |     if (lowest_rank->f > it->f)
343 |       lowest_rank = it;
344 |   }
345 |   return lowest_rank;
346 | }
347 | 
348 | int GOAP::astar_plan
349 | ( 
350 |   actionplanner_t* ap,
351 |   worldstate_t start,
352 |   worldstate_t goal,
353 |   ActionPlan_t* plan,
354 |   std::vector<worldstate_t>* worldstates
355 | )
356 | {
357 |   // put start in opened list
358 |   astarnode_t n0;
359 |   n0.ws = start;
360 |   n0.parentws = start;
361 |   n0.g = 0;
362 |   n0.h = calc_h(&start, &goal);
363 |   n0.f = n0.g + n0.h;
364 |   n0.actionname = "start";
365 |   opened.clear();
366 |   closed.clear();
367 |   opened.push_back(n0);
368 | 
369 |   do
370 |   {    
371 |     if (opened.empty()) {
372 |       printf("Error in astar::astar_plan: Path not found.\n");
373 |       return -1;
374 |     }
375 |     std::vector<astarnode_t>::iterator lowest_rank = get_lowest_rank();
376 | 
377 |     // remove the node with the lowest rank
378 |     astarnode_t cur = *lowest_rank;
379 |     opened.erase(lowest_rank);
380 | 
381 |     // if it matches the goal, we are done!
382 |     if ( (cur.ws.values & goal.care) == (goal.values & goal.care) )
383 |     {
384 |       reconstruct_plan(ap, &cur, plan, worldstates);
385 |       return cur.f;
386 |     }
387 |     // add it to closed
388 |     closed.push_back(cur);
389 | 
390 |     // iterate over neighbours
391 |     std::vector<std::string> actionnames;
392 |     std::vector<int> actioncosts;
393 |     std::vector<worldstate_t> to;
394 |     const int numtransitions = goap_get_possible_state_transitions(ap, cur.ws, &to, &actionnames, &actioncosts);
395 | 
396 |     for (int i=0; i<numtransitions; ++i)
397 |     {
398 |       astarnode_t nb;
399 |       const int cost = cur.g + actioncosts[i];
400 |       int idx_o = -1;
401 |       int idx_c = -1;
402 |       idx_in_opened(&to[i], &idx_o);
403 |       idx_in_closed(&to[i], &idx_c);
404 | 
405 |       // if neighbor in OPEN and cost less than g(neighbor):
406 |       if (idx_o != -1 && cost < opened.at(idx_o).g)
407 |       {
408 | 	// remove neighbor from OPEN, because new path is better
409 | 	if (!opened.empty())
410 | 	  opened.erase(opened.begin() + idx_o);
411 | 	idx_o = -1; // BUGFIX: neighbor is no longer in OPEN, signal this so that we can re-add it.
412 |       }
413 |       
414 |       // if neighbor in CLOSED and cost less than g(neighbor):
415 |       if (idx_c != -1 && cost < closed.at(idx_c).g)
416 |       {
417 | 	// remove neighbor from CLOSED
418 | 	if (!closed.empty())
419 | 	  closed.erase(closed.begin() + idx_c);
420 | 	idx_c = -1;
421 |       }
422 |       
423 |       // if neighbor not in OPEN and neighbor not in CLOSED:
424 |       if (idx_c == -1 && idx_o == -1)
425 |       {
426 | 	nb.ws = to[i];
427 | 	nb.g = cost;
428 | 	nb.h = calc_h(&nb.ws, &goal);
429 | 	nb.f = nb.g + nb.h;
430 | 	nb.actionname = actionnames[i];
431 | 	nb.parentws = cur.ws;
432 | 	opened.push_back(nb);
433 |       }
434 |     }
435 |   } while(true);
436 | 
437 |   return -1;
438 | }
439 | 
440 | int GOAP::goap_get_possible_state_transitions(actionplanner_t* ap, worldstate_t fr, std::vector<worldstate_t>* to, std::vector<std::string>* actionnames, std::vector<int>* actioncosts)
441 | {
442 |   for (std::vector<action_t>::iterator it = ap->actions.begin(); it != ap->actions.end(); it++) {
443 |     const worldstate_t* pre = &(it->pre);
444 |     
445 |     const bool met = (pre->values & pre->care) == (fr.values & pre->care);
446 |     
447 |     if (met) {
448 |       actionnames->push_back(it->name);
449 |       actioncosts->push_back(it->get_cost(ap, &fr));
450 |       worldstate_t to_ws;
451 |       do_action(ap, it-ap->actions.begin(), fr, &to_ws);
452 |       to->push_back(to_ws);
453 |     }
454 |   }
455 |   
456 |   return actionnames->size();
457 | }
458 | 
459 | bool GOAP::is_action_possible(actionplanner_t* ap, worldstate_t* ws, std::string action_name, bool* result)
460 | {
461 |   int index;
462 |   if (!find_action_index(ap, action_name, &index)) {
463 |     printf("Error in GOAP::is_action_possible: Action %s not found in action planner.\n", action_name.c_str());
464 |     return false;
465 |   }
466 |   
467 |   worldstate_t* pre = &ap->actions.at(index).pre;
468 |   *result = (pre->values & pre->care) == (ws->values & pre->care);
469 |   return true;
470 | }
471 | 
472 | bool GOAP::do_action(actionplanner_t* ap, int actionnr, worldstate_t fr, worldstate_t* to)
473 | {
474 |   worldstate_t* pst = &ap->actions[actionnr].pst;
475 |   boost::dynamic_bitset<> notcare = pst->care;
476 |   notcare.flip();
477 |   
478 |   to->values = (fr.values & notcare) | (pst->values & pst->care);
479 |   to->care = fr.care | pst->care;
480 |   return true;
481 | }
482 | 
483 | } // namespace goap


--------------------------------------------------------------------------------
/include/task_planning/action_planner.h:
--------------------------------------------------------------------------------
  1 | ///////////////////////////////////////////////////////////////////////////////
  2 | //      Title     : action_planner.h
  3 | //      Project   : task_planning
  4 | //      Created   : 5/25/2016
  5 | //      Author    : Blake Anderson
  6 | //      Platforms : Ubuntu 64-bit
  7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
  8 | //                 
  9 | //          All files within this directory are subject to the following, unless an alternative
 10 | //          license is explicitly included within the text of each file.
 11 | //
 12 | //          This software and documentation constitute an unpublished work
 13 | //          and contain valuable trade secrets and proprietary information
 14 | //          belonging to the University. None of the foregoing material may be
 15 | //          copied or duplicated or disclosed without the express, written
 16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
 17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
 18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
 19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
 20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
 21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
 22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
 23 | //          University be liable for incidental, special, indirect, direct or
 24 | //          consequential damages or loss of profits, interruption of business,
 25 | //          or related expenses which may arise from use of software or documentation,
 26 | //          including but not limited to those resulting from defects in software
 27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
 28 | //
 29 | ///////////////////////////////////////////////////////////////////////////////
 30 | 
 31 | /*
 32 |  * Provides goal oriented action planning interface.
 33 |  */
 34 | 
 35 | #ifndef GOAP_ACTIONPLANNER_H
 36 | #define GOAP_ACTIONPLANNER_H
 37 | 
 38 | #include "worldstate.h"
 39 | 
 40 | #include <vector>
 41 | #include <list>
 42 | #include <forward_list>
 43 | #include <string>
 44 | #include <iostream>
 45 | #include <unordered_map>
 46 | 
 47 | #include <boost/multi_index_container.hpp>
 48 | #include <boost/multi_index/sequenced_index.hpp>
 49 | #include <boost/multi_index/ordered_index.hpp>
 50 | #include <boost/multi_index/hashed_index.hpp>
 51 | #include <boost/multi_index/member.hpp>
 52 | #include <boost/multi_index/mem_fun.hpp>
 53 | #include <boost/multi_index/tag.hpp>
 54 | 
 55 | #include <boost/bimap.hpp>
 56 | 
 57 | namespace goap {
 58 |   
 59 |   class ActionPlanner;
 60 |   
 61 |   typedef std::vector<std::string> action_plan_t;
 62 |   typedef int (*cost_function)(const ActionPlanner* ap, const Worldstate* ws);
 63 | 
 64 | // Action planner that keeps track of world state propositions and its action repertoire.
 65 | class ActionPlanner
 66 | {
 67 |   friend class TaskManager;
 68 |   
 69 | public:
 70 |   
 71 |   /**
 72 |    * @brief Produce an action plan
 73 |    * @param start       - The starting worldstate
 74 |    * @param goal        - The goal worldstate
 75 |    * @param plan        - Outputs the created action plan
 76 |    * @param worldstates - Outputs the worldstates that the plan passes through
 77 |    * @return The plan cost, or -1 if no plan was found.
 78 | 
 79 |       from: http://theory.stanford.edu/~amitp/GameProgramming/ImplementationNotes.html
 80 |       OPEN = priority queue containing START
 81 |       CLOSED = empty set
 82 |       while lowest rank in OPEN is not the GOAL:
 83 | 	current = remove lowest rank item from OPEN
 84 | 	add current to CLOSED
 85 | 	for neighbors of current:
 86 | 	  cost = g(current) + movementcost(current, neighbor)
 87 | 	  if neighbor in OPEN and cost less than g(neighbor):
 88 | 	    remove neighbor from OPEN, because new path is better
 89 | 	  if neighbor in CLOSED and cost less than g(neighbor): **
 90 | 	    remove neighbor from CLOSED
 91 | 	  if neighbor not in OPEN and neighbor not in CLOSED:
 92 | 	    set g(neighbor) to cost
 93 | 	    add neighbor to OPEN
 94 | 	    set priority queue rank to g(neighbor) + h(neighbor)
 95 | 	    set neighbor's parent to current
 96 |    */
 97 |   int createPlan
 98 |   (
 99 |     const Worldstate& start,
100 |     const Worldstate& goal,
101 |     action_plan_t* plan,
102 |     std::vector<Worldstate>* worldstates
103 |   ) const;
104 |   
105 |   /**
106 |    * @brief Clear contents of this planner.
107 |    */
108 |   void clear();
109 |   
110 |   /**
111 |    * @brief Add an action to be planner using ADL syntax.
112 |    * @param definition    - The base action definition
113 |    * @param preconditions - Vector of preconditions
114 |    * @param effects       - Vector of effects
115 |    * @return              - True on success.
116 |    */
117 |   bool addAction(std::string definition, std::vector<std::string> preconditions, std::vector<std::string> effects);
118 |   
119 |   /**
120 |    * @brief Add a set of entities to be planner using ADL syntax. This version takes a vector of names to assign to the members. Member names must be unique.
121 |    * @param name    - The name of the set
122 |    * @param members - Vector of member names
123 |    * @return        - True on success.
124 |    */
125 |   bool addEntitySet(std::string name, std::vector<std::string> members);
126 |   
127 |   /**
128 |    * @brief Add a set of entities to be planner using ADL syntax. This version takes the number of members to create and assigns [set name]_i to the members.
129 |    * @param name - The name of the set
130 |    * @param num  - The number of members to create
131 |    * @return     - True on success.
132 |    */
133 |   bool addEntitySet(std::string name, int num);
134 |   
135 |   /**
136 |    * @brief Build action space from provided ADL actions and entities.
137 |    * @return - True on success.
138 |    */
139 |   bool buildActionSpace();
140 |   
141 |   /**
142 |    * @brief Set a constant cost for named action.
143 |    * @param actionname - The name of the action to be modified
144 |    * @param cost       - The new constant cost
145 |    * @return True on success, false if the action is not in the planner.
146 |    */
147 |   bool setActionCost(std::string actionname, int cost);
148 |   
149 |   /**
150 |    * @brief Set a cost function for named action.
151 |    * @param actionname - The name of the action to be modified
152 |    * @param func       - Pointer to cost function.
153 |    * @return True on success, false if the action is not in the planner.
154 |    */
155 |   bool setActionCost(std::string actionname, cost_function func);
156 |   
157 |   /**
158 |    * @brief Set a proposition of worldstate to specified value.
159 |    * @param ws       - World state to be modified
160 |    * @param name     - The name of the proposition to be modified. This can use literal or generic params.
161 |    * @param value    - The new value of propositionname
162 |    * @return         - True on success, failure if the proposition is not in the planner.
163 |    */
164 |   bool setProposition(Worldstate* ws, std::string name, bool value) const;
165 |   
166 |   /**
167 |    * @brief Get the value of a proposition in a specified worldstate.
168 |    * @param ws       - Worldstate containing the value
169 |    * @param name     - The name of the proposition to be checked
170 |    * @param result   - Outputs the value of the proposition
171 |    * @return         - Teu on success. Failure if the action doesn't exist
172 |    */
173 |   bool getPropositionValue(const Worldstate* ws, std::string name, bool* result) const;
174 |   
175 |   /**
176 |    * @brief Determine if the preconditions of an action are met by a given worldstate
177 |    * @param ws          - The worldstate to be tested
178 |    * @param action_name - The action name
179 |    * @param result      - Outputs whether the action is possible.
180 |    * @return            - True on success. False if the action doesn't exist.
181 |    */
182 |   bool isActionPossible(const Worldstate* ws, std::string action_name, bool* result) const;
183 |   
184 |   /**
185 |    * @brief Performs an action on a worldstate, producing the resultant worldstate
186 |    * @param actionnname - The name of the action
187 |    * @param fr          - The starting worldstate
188 |    * @param to          - Outputs the resultant worldspace
189 |    * return             - True on success, false on failure
190 |    */  
191 |   bool doAction(std::string actionname, const Worldstate* fr, Worldstate* to) const;
192 |   
193 |   /**
194 |    * @brief Get string which describes the action planner by listing all actions with preconditions and effects.
195 |    * @param output - Description string
196 |    */
197 |   std::string description() const;
198 |   
199 |   /**
200 |    * @brief Describe the worldstate by listing propositions that matter, in lowercase for false-valued, and uppercase for true-valued propositions.
201 |    * @param ws - The worldstate to be described
202 |    * @return   - String containing the output
203 |    */
204 |   std::string worldstateDescription(const Worldstate* ws) const;
205 |   
206 |   /**
207 |    * @brief Update the size of a worldstate to match the number of propositions in this planner.
208 |    * @param ws The worldstate to modify.
209 |    */
210 |   void updateWorldstateSize(Worldstate* ws) const;
211 |   
212 | private:
213 | 
214 |   typedef std::string param_label, param_type;
215 |   typedef std::vector<std::string> string_vec;
216 |   
217 |   // Used internally by the createPlan function. Represents a node in the graph search.
218 |   struct astarnode_t
219 |   {
220 |     Worldstate ws;		// The state of the world at this node.
221 |     int g;			// The cost so far.
222 |     int f;			// g+heuristic.
223 |     std::string actionname;	// How did we get to this node?
224 |     Worldstate* parentws;	// Where did we come from?
225 |   };
226 |   
227 |   class NodeMap
228 |   {
229 |   public:
230 |     
231 |     struct sequenced{};
232 |     struct hash_key {};
233 |     
234 |     typedef boost::multi_index::tag<sequenced> sequenced_tag;
235 |     typedef boost::multi_index::tag<hash_key> hash_tag;
236 |     
237 |     typedef boost::multi_index_container<
238 |       astarnode_t*,
239 |       boost::multi_index::indexed_by<
240 | 	boost::multi_index::hashed_unique<
241 | 	  hash_tag,
242 | 	  boost::multi_index::member<astarnode_t, Worldstate, &astarnode_t::ws>,
243 | 	  Worldstate::hash
244 | 	>,
245 | 	boost::multi_index::sequenced<sequenced_tag>
246 |       >
247 |     > node_map_t;
248 |     
249 |     typedef node_map_t::index<hash_key>::type hash_index;
250 |     typedef node_map_t::index<sequenced>::type sequence_index;
251 |     typedef node_map_t::index_iterator<hash_key>::type hash_iter;
252 |     typedef node_map_t::index_iterator<sequenced>::type sequence_iter;
253 |     
254 |     node_map_t& operator() () { return map_; }
255 |     
256 |     hash_index& getHashed() { return map_.get<hash_key>(); };
257 |     
258 |     sequence_index& getSequence() { return map_.get<sequenced>(); };
259 |     
260 |     ~NodeMap()
261 |     {
262 |       for (sequence_iter it = getSequence().begin(); it != getSequence().end(); ++it)
263 | 	delete *it;
264 |     }
265 |     
266 |   private:
267 |     node_map_t map_;
268 |   };
269 |   
270 |   struct EntitySet
271 |   {
272 |     param_type name;
273 |     
274 |     typedef std::vector<param_label> member_list;
275 |     member_list members;
276 |     
277 |     bool operator==(const param_type &rhs) const { return name == rhs; }
278 |   };
279 | 
280 |   class Proposition
281 |   {
282 |   public:
283 |     /*
284 |      * A "variable" parameter references a parameter in the base action.
285 |      * This causes the proposition to receive the value of the action parameter.
286 |      * 		For example:	RobotAt(a) sets RobotAt true for the value of a.
287 |      * 				~RobotAt(a) sets RobotAt false for the value of a.
288 |      * 
289 |      * A "literal" parameter references a specific entity. This makes the proposition the same for all versions of the action.
290 |      * A litetal parameter has a '$' in front of the parameter label.
291 |      * 		For example:	RobotAt($location_1) sets RobotAt true for location_1.
292 |      * 				RobotAt($location_1) sets RobotAt false for location_1.
293 |      * 
294 |      * A "negated" parameter references all entities in a set other than the specified one. You must also provide the set name.
295 |      * A negated parameter has a '~' in front of the parameter label.
296 |      * 		For example: 	RobotAt(~a:location) sets RobotAt true for all values other than a in set location.
297 |      *			 	~RobotAt(~a:location) sets RobotAt false for all values other than a in set location.
298 |      * 
299 |      * You can declare a parameter that is both negated and literal.
300 |      * 		For example: 	RobotAt(~$location_1:location) sets RobotAt true for all values other than location_1 in set location.
301 |      *			 	~RobotAt(~$location_1:location) sets RobotAt false for all values other than location_1 in set location.
302 |      * 
303 |      * An "ALL" parameter references all entities of a set.
304 |      * 		For example:	RobotAt(ALL:location) sets RobotAt true for all locations.
305 |      * 				~RobotAt(ALL:location) sets RobotAt false for all locations.
306 |      * 
307 |      * You cannot negate an all parameter.
308 |      */
309 |     typedef struct {
310 |       std::string raw_text;		// The raw text provided by client
311 |       param_label label;		// Holds name of parsed parameter label
312 |       param_type entity_set_name;	// Name of set referenced by parameter
313 |       const EntitySet::member_list* set_members; // Points to entities referenced by parameter
314 |       
315 |       bool negated;			// Is this a "not" param?
316 |       bool all;				// Is this an "all" param?
317 |       bool literal;			// Is the text a literal?
318 |       
319 |       bool operator==(const std::string &rhs) const { return label == rhs; }
320 |     } parameter;
321 |     std::vector<parameter> params;
322 |     
323 |     /**
324 |      * @breif Build this proposition by parsing the user text
325 |      * @param definition The raw text from user
326 |      * @return True on successful parse
327 |      */
328 |     bool parse(std::string definition);
329 |     
330 |     /**
331 |      * @breif Build the proposition name using the parameter values
332 |      */
333 |     void buildName();
334 |     
335 |     /**
336 |      * @breif Helper function for parse(). Parses a parameter.
337 |      * @param param The raw text to parse
338 |      * @param parameter Output reference to the created parameter object
339 |      * @return True on successful parse.
340 |      */
341 |     bool parseParam(std::string param, parameter* parameter);
342 |     
343 |     /**
344 |      * @breif Search function for the parameter list
345 |      * @param label The derived_value to search for.
346 |      * @return Pointer to parameter, or NULL if not found.
347 |      */
348 |     parameter* getParameter(std::string value);
349 |     
350 |     std::string name, root;
351 |     
352 |     
353 |     bool operator==(const Proposition &rhs) const { return name == rhs.name; }
354 |     
355 |     bool operator==(const std::string &rhs) const { return name == rhs; }
356 |   };
357 | 
358 |   class Action
359 |   {
360 |   public:
361 |     Action();
362 |     Action(int);
363 |     
364 |     // action params are modeled like this
365 |     typedef struct {
366 |       param_label label;
367 |       param_type type;
368 |       const std::vector<param_label>* set_members;
369 |       
370 |       bool operator==(const param_label &rhs) const { return label == rhs; }
371 |     } parameter;
372 |     
373 |     /**
374 |      * @breif Build this action by parsing the user text
375 |      * @param definition The raw text from user
376 |      * @return True on successful parse
377 |      */
378 |     bool parse(std::string definition, string_vec preconditions, string_vec effects);
379 |     
380 |     /**
381 |      * @breif Helper for parse. Parses a proposition and adds to precondition list
382 |      * @param definition The raw text from user
383 |      * @return True on successful parse
384 |      */
385 |     bool buildPrecondition(std::string definition);
386 |     
387 |     /**
388 |      * @breif Helper for parse. Parses a proposition and adds to effect list
389 |      * @param definition The raw text from user
390 |      * @return True on successful parse
391 |      */
392 |     bool buildEffect(std::string definition);
393 |     
394 |     /**
395 |     * @brief Set the cost to a constant value.
396 |     * @param val - The new constant value
397 |     */
398 |     void set_cost(int val);
399 |     
400 |     /**
401 |     * @brief Associate a cost function with the action. This allows the cost to depend on the worldstate.
402 |     * @param func - Function pointer referencing the cost function
403 |     */
404 |     void set_cost(cost_function func);
405 |     
406 |     /**
407 |     * @brief Get the cost of the action.
408 |     * @param ap - The action planner. Not needed if we are not using a cost function for thsi action.
409 |     * @param ws - The worldstate. Not needed if we are not using a cost function for thsi action.
410 |     * @return The action cost
411 |     */
412 |     int get_cost(const ActionPlanner* ap = NULL, const Worldstate* ws = NULL) const;
413 |     
414 |     /**
415 |      * @breif Updates the name member using current parameter values
416 |      */
417 |     void buildName();
418 |     
419 |     /**
420 |      * @breif Search function for the parameter list
421 |      * @param label The label to search for.
422 |      * @return Pointer to parameter, or NULL if not found.
423 |      */
424 |     parameter* getParameter(param_label label);
425 |     
426 |     std::string name; // full name
427 |     std::string root; // name without the parameter list
428 |     
429 |     typedef std::vector<parameter> param_vec;
430 |     param_vec params; // holds the params
431 |     
432 |     typedef std::pair<Proposition, bool> condition_t;
433 |     std::vector<condition_t> preconditions, effects;
434 |     
435 |     // Use this criterion for sorting Actions alphabetically in a vector
436 |     bool operator<(const Action &rhs) const { return name < rhs.name; }
437 |     
438 |     bool operator==(const Action &rhs) const { return name == rhs.name; }
439 |     
440 |     bool operator==(const std::string &rhs) const { return name == rhs; }
441 |     
442 |     // These store the compiled preconditions and effects. The planner algorithm uses these.
443 |     Worldstate pre, pst;
444 |     
445 |   private:
446 |     int cost;
447 |     int (*cost_func)(const ActionPlanner*, const Worldstate*);
448 |   };
449 |   
450 |   typedef std::list<Action> action_vector;
451 |   
452 |   /**
453 |    * @brief Add a new action to the action planner.
454 |    * @param actionname - The desired action name
455 |    * @return           - Reference to the action
456 |    */
457 |   Action* addAction(std::string actionname);
458 |     
459 |   /**
460 |    * @brief Add a precondition for an action. If this proposition is not already present in the planner, it will be added.
461 |    * @param actionname - The action to be modified.
462 |    * @param name       - The proposition to be modified.
463 |    * @param value      - The new value of the precondition.
464 |    * @return           - True on success
465 |    */
466 |   bool setPrecondition(std::string actionname, std::string name, bool value);
467 |   
468 |   /**
469 |    * @brief Add an effect for an action. If this proposition is not already present in the planner, it will be added.
470 |    * @param actionname - The action to be modified.
471 |    * @param name       - The proposition to be modified.
472 |    * @param value      - The new value of precondition.
473 |    * @return           - True on success
474 |    */
475 |   bool setEffect(std::string actionname, std::string name, bool value);
476 |   
477 |   /**
478 |    * @brief Add a new proposition to the action planner.
479 |    * @param propositionname - The desired proposition name
480 |    * @param index    - Index of the proposition in proposition_names_
481 |    * @return         - True if the success, false if proposition with that name already existed.
482 |    */
483 |   bool addProposition(std::string propositionname);
484 |   
485 |   /**
486 |    * @brief Attempt to match an ADL proposition to existing atoms.
487 |    * @param propositionname - The desired proposition name
488 |    * @param index    - Index of the proposition in proposition_names_
489 |    * @return         - True if the success, false if proposition with that name already existed.
490 |    */
491 |   bool setPropositionADL(Worldstate* ws, std::string name, bool value) const;
492 |   
493 |   /**
494 |    * @brief Get the lowest rank node in the opened nodes.
495 |    * @param nodes The node map to search
496 |    * @return Iterator to the lowest ranked node
497 |    */
498 |   NodeMap::sequence_iter getLowestRank(NodeMap* nodes) const;
499 |   
500 |   /**
501 |    * @brief Internal function to reconstruct the plan by tracing from last node to initial node.
502 |    * @param goalnode    - The goal
503 |    * @param plan        - Vector of actions to be taken
504 |    * @param worldstates - The worldstates we will pass through
505 |    */
506 |   void reconstructPlan(astarnode_t* goalnode, NodeMap* nodes, action_plan_t* plan, std::vector<Worldstate>* worldstates) const;
507 |   
508 |   /**
509 |   * @brief Given the specified 'from' state, list all possible 'to' states along with the action required, and the action cost. For internal use.
510 |   * @param fr - The 'from' state
511 |   * @param to - the 'to' state
512 |   * @return The number of possible transitions
513 |   */
514 |   int getPossibleStateTransitions(const Worldstate* fr, std::vector<Worldstate>* to, std::vector<const std::string*>* actionnames, std::vector<int>* actioncosts) const;
515 |   
516 |   /**
517 |    * @brief Perform action specified by number on a worldstate
518 |    * @param actionnr - The index of the action to perform
519 |    * @param fr - The worldstate to be acted on
520 |    * @param to - The new worldstate
521 |    * @return   - True on success. False on failure.
522 |    */
523 |   bool doActionInternal(const Action* action, const Worldstate* fr, Worldstate* to) const;
524 |   
525 |   /**
526 |    * @brief Return the index of the indicated proposition.
527 |    * @param name     - The desired proposition name
528 |    * @param index    - Outputs the index of the proposition in proposition_names_
529 |    * @return         - True if the proposition was found, else false
530 |    */
531 |   bool findPropositionIndex(std::string name, int* index = NULL) const;
532 |   
533 |   /**
534 |    * @brief Return a pointer to the indicated action.
535 |    * @param actionname - The desired action name
536 |    * @return           - Reference to the action
537 |    */
538 |    Action* findAction(std::string actionname);
539 |    
540 |    /**
541 |    * @brief Return a pointer to the indicated action. Const-overloaded version.
542 |    * @param actionname - The desired action name
543 |    * @return           - Reference to the action
544 |    */
545 |    const Action* findAction(std::string actionname) const;
546 |    
547 |    /**
548 |    * @brief Return a pointer to the indicated action.
549 |    * @param actionname - The desired action name
550 |    * @return           - Reference to the action
551 |    */
552 |    Action* findUnbuiltAction(std::string actionname);
553 |    
554 |    /**
555 |    * @brief Return a pointer to the indicated action. Const-overloaded version.
556 |    * @param actionname - The desired action name
557 |    * @return           - Reference to the action
558 |    */
559 |    const Action* findUnbuiltAction(std::string actionname) const;
560 |    
561 |    /**
562 |    * @brief Return a pointer to the indicated EntitySet.
563 |    * @param actionname - The desired action name
564 |    * @return           - Reference to the action
565 |    */
566 |    EntitySet* findEntitySet(std::string name);
567 |    
568 |    /**
569 |    * @brief Return a pointer to the indicated EntitySet. Const-overloaded version.
570 |    * @param actionname - The desired action name
571 |    * @return           - Reference to the action
572 |    */
573 |    const EntitySet* findEntitySet(std::string name) const;
574 |   
575 |   /**
576 |    * @brief This is our heuristic: estimate for remaining distance is the number of mismatched propositions that matter.
577 |    * @param fr - The starting worldspace
578 |    * @param to - The end worldspace
579 |    * @return The distance between the two worldspaces
580 |    */
581 |   int calcH(const Worldstate* fr, const Worldstate* to) const;
582 |   
583 |   /**
584 |    * @breif Build permutations of an action.
585 |    * @param action - The action to be permuted
586 |    * @return The permutations
587 |    */
588 |   bool permuteAction(const Action &action, action_vector* result) const;
589 |   
590 |   /**
591 |    * @breif Helper function for permuteAction to insert permuted parameters into an action.
592 |    * @param action      - The action to be modified
593 |    * @param original    - The param name to be replaced
594 |    * @param replacement - The param name to be inserted
595 |    * @return true on success
596 |    */
597 |   bool substituteParamLabel(Action &action, param_label original, param_label replacement) const;
598 |   
599 |   /**
600 |    * @brief Helper function for permuteAction to verify the mutual exclusion rule for action parameters.
601 |    * @param labels - The parameter labels
602 |    * @param v      - The parameter values
603 |    * @return True if the mutual exclusion rule is met
604 |    */
605 |   template <typename T>
606 |   bool elementUniqueness(const string_vec& labels, const std::vector<T>& v) const;
607 |   
608 |   /**
609 |    * @breif Links the action definitions to the entity sets.
610 |    * @return true on success
611 |    */
612 |   bool linkEntities(Action& action) const;
613 |   
614 |   /**
615 |    * @breif Expands parameters set ALL.
616 |    * @return true on success
617 |    */
618 |   bool expandAllParams(Action &action) const;
619 |   
620 |   /**
621 |    * @breif Expands parameters set ALL.
622 |    * @return The Propositions produced by the expansion
623 |    */
624 |   std::vector<ActionPlanner::Action::condition_t> expandAllCondition(Action::condition_t condition) const;
625 |   
626 |   /**
627 |    * @breif Expands negated parameters.
628 |    * @return true on success
629 |    */
630 |   bool expandNegatedParams(Action &action) const;
631 |   
632 |   /**
633 |    * @breif Expands negated parameters.
634 |    * @return The Propositions produced by the expansion
635 |    */
636 |   std::vector<ActionPlanner::Action::condition_t> expandNegatedCondition(Action::condition_t condition) const;
637 |   
638 |   action_vector actions_;
639 |   action_vector unbuilt_actions_;
640 |   
641 |   typedef std::forward_list<EntitySet> entity_list;
642 |   entity_list entities_;
643 |   
644 |   typedef boost::bimaps::bimap<std::size_t, std::string> prop_index_map_t;
645 |   typedef prop_index_map_t::value_type prop_index_t;
646 |   prop_index_map_t proposition_index_;
647 | };
648 | 
649 | } // namespace goap
650 | 
651 | #endif


--------------------------------------------------------------------------------
/src/action_planner.cpp:
--------------------------------------------------------------------------------
   1 | ///////////////////////////////////////////////////////////////////////////////
   2 | //      Title     : action_planner.cpp
   3 | //      Project   : task_planning
   4 | //      Created   : 5/25/2016
   5 | //      Author    : Blake Anderson
   6 | //      Platforms : Ubuntu 64-bit
   7 | //      Copyright : Copyright© The University of Texas at Austin, 2014-2017. All rights reserved.
   8 | //                 
   9 | //          All files within this directory are subject to the following, unless an alternative
  10 | //          license is explicitly included within the text of each file.
  11 | //
  12 | //          This software and documentation constitute an unpublished work
  13 | //          and contain valuable trade secrets and proprietary information
  14 | //          belonging to the University. None of the foregoing material may be
  15 | //          copied or duplicated or disclosed without the express, written
  16 | //          permission of the University. THE UNIVERSITY EXPRESSLY DISCLAIMS ANY
  17 | //          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
  18 | //          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
  19 | //          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
  20 | //          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
  21 | //          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
  22 | //          THE SOFTWARE OR DOCUMENTATION. Under no circumstances shall the
  23 | //          University be liable for incidental, special, indirect, direct or
  24 | //          consequential damages or loss of profits, interruption of business,
  25 | //          or related expenses which may arise from use of software or documentation,
  26 | //          including but not limited to those resulting from defects in software
  27 | //          and/or documentation, or loss or inaccuracy of data of any kind.
  28 | //
  29 | ///////////////////////////////////////////////////////////////////////////////
  30 | 
  31 | #include <task_planning/action_planner.h>
  32 | #include <jsoncpp/json/json.h>
  33 | 
  34 | #include <boost/algorithm/string.hpp>
  35 | #include <boost/foreach.hpp>
  36 | 
  37 | #include <algorithm>
  38 | #include <sstream>
  39 | #include <fstream>
  40 | #include <iostream>
  41 | #include <unordered_set>
  42 | 
  43 | namespace goap {
  44 |   
  45 | /**
  46 |  * @breif Produces permutation and then increments the counter. Run this in a do...while loop to run the counter forward.
  47 |  * @param v Vector of vector references containing the elements to be permuted.
  48 |  * @param it Counter vector. It is incremented by one.
  49 |  * @return True if we have not reached the end of the permutations.
  50 |  */
  51 | template <typename T>
  52 | bool next_permutation(const std::vector<const std::vector<T>*>& v, std::vector<std::size_t>& it)
  53 | {  
  54 |   // Increment the counter
  55 |   for (std::size_t i = 0, size = it.size(); i != size; ++i) {
  56 |     const std::size_t index = size - 1 - i;
  57 |     ++it[index];
  58 |     if (it[index] == v[index]->size()) {
  59 |       it[index] = 0;
  60 |     } else {
  61 |       return true;
  62 |     }
  63 |   }
  64 |   return false;
  65 | }
  66 | 
  67 | 
  68 | 
  69 | /**
  70 |  * @breif Determine if all elements in a container are unique
  71 |  * @param first Front of range
  72 |  * @param last End of range
  73 |  * @return True if all elements in the range are unique
  74 |  */
  75 | template <class FwdIterator>
  76 | bool check_unique(FwdIterator first, FwdIterator last)
  77 | {
  78 |     FwdIterator result = first;
  79 |     std::unordered_set<typename FwdIterator::value_type> seen;
  80 | 
  81 |     for (; first != last; ++first) {
  82 |         if (!seen.insert(*first).second) {
  83 |             return false;
  84 | 	}
  85 |     }
  86 |     return true;
  87 | }
  88 | 
  89 | 
  90 | 
  91 | 
  92 | int ActionPlanner::createPlan
  93 |   (
  94 |     const Worldstate& start,
  95 |     const Worldstate& goal,
  96 |     action_plan_t* plan,
  97 |     std::vector<Worldstate>* worldstates
  98 |   ) const
  99 | {
 100 |   NodeMap opened;	// The set of nodes we should consider.
 101 |   NodeMap closed;	// The set of nodes we already visited.
 102 | 
 103 |   // create start node
 104 |   astarnode_t* n0 = new astarnode_t;
 105 |   n0->ws = start;
 106 |   n0->parentws = new Worldstate;
 107 |   *n0->parentws = start;
 108 |   n0->g = 0;
 109 |   n0->f = n0->g + calcH(&start, &goal);
 110 |   n0->actionname = "start";
 111 |   opened().insert(n0);
 112 |   
 113 |   while (!opened().empty()) {
 114 | 
 115 |     const NodeMap::sequence_iter lowest_rank = getLowestRank(&opened);
 116 |     
 117 |     // remove the node with the lowest rank
 118 |     astarnode_t* cur;
 119 |     cur = *lowest_rank;
 120 |     
 121 |     opened.getSequence().erase(lowest_rank);
 122 |     
 123 |     // if it matches the goal, we are done!
 124 |     if ( (cur->ws.values & goal.care) == (goal.values & goal.care) ) {
 125 |       reconstructPlan(cur, &closed, plan, worldstates);
 126 |       return cur->f;
 127 |     }
 128 |     // add it to closed
 129 |     closed().insert(cur);
 130 |     
 131 |     // iterate over neighbours
 132 |     std::vector<const std::string*> actionnames;
 133 |     std::vector<int> actioncosts;
 134 |     std::vector<Worldstate> to;
 135 | 
 136 |     const int numtransitions = getPossibleStateTransitions(&cur->ws, &to, &actionnames, &actioncosts);
 137 | 
 138 |     for (int i=0; i<numtransitions; ++i) {
 139 |       const float cost = cur->g + actioncosts[i];
 140 |       
 141 |       // if neighbor in CLOSED and cost less than g(neighbor):
 142 |       NodeMap::hash_iter closed_it = closed.getHashed().find(to[i]);
 143 |       if (closed_it != closed().end()) {
 144 | 	if (cost < (**closed_it).g) {
 145 | 	  // remove neighbor from CLOSED
 146 | 	  if (!closed().empty()) {
 147 | 	    (**closed_it).g = cost;
 148 | 	    (**closed_it).f = cost + calcH(&to[i], &goal);
 149 | 	    (**closed_it).parentws = &cur->ws;
 150 | 	    opened().insert(*closed_it);
 151 | 	    closed().erase(closed_it);
 152 | 	  }
 153 | 	}
 154 | 	else continue;
 155 |       }
 156 |       else {
 157 | 	// if neighbor in OPEN and cost less than g(neighbor):
 158 | 	NodeMap::hash_iter opened_it = opened.getHashed().find(to[i]);
 159 | 	if (opened_it != opened().end()) {
 160 | 	  if (cost < (**opened_it).g) {
 161 | 	    // remove neighbor from OPEN, because new path is better
 162 | 	    if (!opened().empty()) {
 163 | 	      (**opened_it).g = cost;
 164 | 	      (**opened_it).f = cost + calcH(&to[i], &goal);
 165 | 	      (**opened_it).parentws = &cur->ws;
 166 | 	    }
 167 | 	  }
 168 | 	  else continue;
 169 | 	}
 170 | 	else {
 171 | 	  // if neighbor not in OPEN and neighbor not in CLOSED:
 172 | 	  astarnode_t* new_node = new astarnode_t;
 173 | 	  new_node->ws = to[i];
 174 | 	  new_node->g = cost;
 175 | 	  new_node->f = cost + calcH(&to[i], &goal);
 176 | 	  new_node->actionname = *(actionnames[i]);
 177 | 	  new_node->parentws = &cur->ws;
 178 | 	  opened().insert(new_node);
 179 | 	}
 180 |       }
 181 |       
 182 |       // if neighbor not in OPEN and neighbor not in CLOSED:
 183 | //       astarnode_t new_node;
 184 | //       new_node.ws = to[i];
 185 | //       new_node.g = cost;
 186 | //       new_node.f = cost + calcH(&to[i], &goal);
 187 | //       new_node.actionname = *(actionnames[i]);
 188 | //       new_node.parentws = cur.ws;
 189 | //       opened.insert(new_node);
 190 |     }
 191 |   } // end while
 192 |   
 193 |   printf("Error in ActionPlanner::createPlan - Path not found.\n");
 194 |   return -1;
 195 | }
 196 | 
 197 | 
 198 | 
 199 | bool ActionPlanner::buildActionSpace()
 200 | {
 201 |   // Clear built action list
 202 |   actions_.clear();
 203 |   
 204 |   // For each Action
 205 |   BOOST_FOREACH (Action& action, unbuilt_actions_) {
 206 |     
 207 |     if (!linkEntities(action)) {
 208 |       return false;
 209 |     }
 210 |     
 211 |     // handle the "all" propositions
 212 |     if (!expandAllParams(action)) {
 213 |       return false;
 214 |     }
 215 |     
 216 |     // Build permutations of Action
 217 |     action_vector permuted_actions;
 218 |     if (!permuteAction(action, &permuted_actions)) {
 219 |       printf("ActionPlanner::buildActionSpace - Permutation failed for action %s.", action.name.c_str());
 220 |       return false;
 221 |     }
 222 |     
 223 |     // handle the negated propositions
 224 |     BOOST_FOREACH (Action& action, permuted_actions) {
 225 |       if (!expandNegatedParams(action)) {
 226 | 	return false;
 227 |       }
 228 |     }
 229 | 
 230 |     // Add permuted Actions to list (addAction guarantees that actions will be unique)
 231 |     actions_.splice(actions_.end(), permuted_actions);
 232 |   }
 233 |   
 234 |   // Set pre and pst of built actions
 235 |   BOOST_FOREACH (Action& action, actions_) {
 236 |     
 237 |     // Set pre
 238 |     BOOST_FOREACH (Action::condition_t precondition, action.preconditions) {
 239 |       
 240 |       if (!setPrecondition(action.name, precondition.first.name, precondition.second)) {
 241 | 	printf("ActionPlanner::buildActionSpace - Setting pre failed for action %s.", action.name.c_str());
 242 | 	return false;
 243 |       }
 244 |     }
 245 |     
 246 |     // Set effects
 247 |     BOOST_FOREACH (Action::condition_t effect, action.effects) {
 248 |       if (!setEffect(action.name, effect.first.name, effect.second)) {
 249 | 	printf("ActionPlanner::buildActionSpace - Setting pst failed for action %s.", action.name.c_str());
 250 | 	return false;
 251 |       }
 252 |     }
 253 |   }
 254 |   
 255 |   return true;
 256 | }
 257 | 
 258 | 
 259 | 
 260 | void ActionPlanner::clear()
 261 | {
 262 |   proposition_index_.clear();
 263 |   entities_.clear();
 264 |   actions_.clear();
 265 |   unbuilt_actions_.clear();
 266 | }
 267 | 
 268 | 
 269 | 
 270 | bool ActionPlanner::addAction(std::string definition, std::vector<std::string> preconditions, std::vector<std::string> effects)
 271 | {
 272 |   Action new_action;
 273 |   if (!new_action.parse(definition, preconditions, effects)) {
 274 |     printf("ActionPlanner::addAction - Failed to add action %s.", definition.c_str());
 275 |     return false;
 276 |   }
 277 |   
 278 |   unbuilt_actions_.push_back(new_action);
 279 |   
 280 |   return true;
 281 | }
 282 | 
 283 | 
 284 | 
 285 | bool ActionPlanner::addEntitySet(std::string name, std::vector<std::string> members)
 286 | {
 287 |   // Validate
 288 |   if (members.empty()) {
 289 |     printf("Error in ActionPlanner::addEntitySet: No members received.");
 290 |     return false;
 291 |   }
 292 |   
 293 |   if (name.empty()) {
 294 |     printf("Error in ActionPlanner::addEntitySet: Empty name received.");
 295 |     return false;
 296 |   }
 297 |   
 298 |   // Verify that all member names are unique
 299 |   if (!check_unique(members.begin(), members.end())) {
 300 |     printf("Error in ActionPlanner::addEntitySet: Member list contains duplicate elements.");
 301 |     return false;
 302 |   }
 303 |   
 304 |   // Check if a set with this name exists
 305 |   EntitySet* new_set = findEntitySet(name);
 306 |   if (new_set == NULL) {
 307 |     // Set does not already exist. Create new one
 308 |     new_set = new EntitySet;
 309 |     new_set->name = name;
 310 |     new_set->members = members;
 311 |     entities_.push_front(*new_set);
 312 |     delete new_set;
 313 |   }
 314 |   else {
 315 |     // Set already exists. Update members.
 316 |     new_set->members = members;
 317 |   }
 318 |   
 319 |   return true;
 320 | }
 321 | 
 322 | 
 323 | 
 324 | bool ActionPlanner::addEntitySet(std::string name, int num)
 325 | {
 326 |   // Validate
 327 |   if (num < 1) {
 328 |     printf("Error in ActionPlanner::addEntitySet: num must be positive. Received %d.\n", num);
 329 |     return false;
 330 |   }
 331 |   
 332 |   // Build member list with generic names
 333 |   string_vec members;
 334 |   members.reserve(num);
 335 |   for (int i = 1; i <= num; ++i) {
 336 |     members.push_back(name + "_" + std::to_string(i));
 337 |   }
 338 |   
 339 |   return addEntitySet(name, members);
 340 | }
 341 | 
 342 | 
 343 | 
 344 | bool ActionPlanner::setPrecondition(std::string actionname, std::string name, bool value)
 345 | {
 346 |   Action* action = findAction(actionname);
 347 |   if (!action) {
 348 |     return false;
 349 |   }
 350 |   
 351 |   int proposition_index;
 352 |   addProposition(name);
 353 | 
 354 |   return setProposition(&action->pre, name, value);
 355 | }
 356 | 
 357 | 
 358 | 
 359 | bool ActionPlanner::setEffect(std::string actionname, std::string name, bool value)
 360 | {
 361 |   Action* action = findAction(actionname);
 362 |   if (!action) {
 363 |     return false;
 364 |   }
 365 |   
 366 |   int proposition_index;
 367 |   addProposition(name);
 368 |   
 369 |   return setProposition(&action->pst, name, value);
 370 | }
 371 | 
 372 | 
 373 | 
 374 | bool ActionPlanner::setActionCost(std::string actionname, int cost)
 375 | {
 376 |   Action* action = findAction(actionname);
 377 |   if (!action) {
 378 |     printf("Error in ActionPlanner::setActionCost: Action %s not found in action planner.\n", actionname.c_str());
 379 |     return false;
 380 |   }
 381 |   
 382 |   action->set_cost(cost);
 383 |   return true;
 384 | }
 385 | 
 386 | 
 387 | 
 388 | bool ActionPlanner::setActionCost(std::string actionname, int (*func)(const ActionPlanner* ap, const Worldstate* ws))
 389 | {
 390 |   Action* action = findAction(actionname);
 391 |   if (!action) {
 392 |     printf("Error in ActionPlanner::setActionCost: Action %s not found in action planner.\n", actionname.c_str());
 393 |     return false;
 394 |   }
 395 |   
 396 |   action->set_cost(func);
 397 |   return true;
 398 | }
 399 | 
 400 | 
 401 | 
 402 | bool ActionPlanner::setProposition(Worldstate* ws, std::string name, bool value) const
 403 | {
 404 |   // See if name matches any existing atoms
 405 |   int index;
 406 |   if(!findPropositionIndex(name, &index)) {
 407 |     
 408 |     // See if name can be interpreted via ADL
 409 |     if (setPropositionADL(ws, name, value)) {
 410 |       return true;
 411 |     }
 412 |     
 413 |     printf("Error in ActionPlanner::setProposition: Proposition %s not found in action planner.\n", name.c_str());
 414 |     return false;
 415 |   }
 416 |   
 417 |   updateWorldstateSize(ws);
 418 |   
 419 |   ws->values[index] = value;
 420 |   ws->care[index] = true;
 421 | 
 422 |   return true;
 423 | }
 424 | 
 425 | 
 426 | 
 427 | bool ActionPlanner::setPropositionADL(Worldstate* ws, std::string name, bool value) const
 428 | {
 429 |   Worldstate new_ws = *ws;
 430 |   
 431 |   Proposition prop;
 432 |   if (!prop.parse(name)) {
 433 |     return false;
 434 |   }
 435 |   
 436 |   Action temp_action;
 437 |   temp_action.preconditions.push_back(Action::condition_t(prop, value));
 438 |   
 439 |   if (!linkEntities(temp_action)) {
 440 |     return false;
 441 |   }
 442 |   
 443 |   // handle the "all" propositions
 444 |   if (!expandAllParams(temp_action)) {
 445 |     return false;
 446 |   }
 447 |   
 448 |   // handle the negated propositions
 449 |   if (!expandNegatedParams(temp_action)) {
 450 |     return false;
 451 |   }
 452 |   
 453 |   std::vector<Action::condition_t> created_conditions = temp_action.preconditions;
 454 |   
 455 |   BOOST_FOREACH (Action::condition_t& condition, created_conditions) {
 456 |     condition.first.buildName();
 457 |     
 458 |     int index;
 459 |     if(!findPropositionIndex(condition.first.name, &index)) {
 460 |       return false;
 461 |     }
 462 |     
 463 |     updateWorldstateSize(&new_ws);
 464 |     
 465 |     new_ws.values[index] = condition.second;
 466 |     new_ws.care[index] = true;
 467 |   }
 468 | 
 469 |   *ws = new_ws;
 470 |   return true;
 471 | }
 472 | 
 473 | 
 474 | 
 475 | bool ActionPlanner::getPropositionValue(const Worldstate* ws, std::string name, bool* result) const
 476 | {
 477 |   int index;
 478 |   if(!findPropositionIndex(name, &index)) {
 479 |     printf("Error in GOAP::getPropositionValue: Proposition %s not found in action planner.\n", name.c_str());
 480 |     return false;
 481 |   }
 482 |   
 483 |   *result = ws->values[index];
 484 |   return true;
 485 | }
 486 | 
 487 | 
 488 | 
 489 | bool ActionPlanner::isActionPossible(const Worldstate* ws, std::string action_name, bool* result) const
 490 | {
 491 |   const Action* action = findAction(action_name);
 492 |   if (!action) {
 493 |     printf("ActionPlanner::isActionPossible: Action %s not found in action planner.\n", action_name.c_str());
 494 |     return false;
 495 |   }
 496 |   
 497 |   const Worldstate* pre = &action->pre;
 498 |   *result = (pre->values & pre->care) == (ws->values & pre->care);
 499 |   return true;
 500 | }
 501 | 
 502 | 
 503 | 
 504 | bool ActionPlanner::doAction(std::string actionname, const Worldstate* fr, Worldstate* to) const
 505 | {
 506 |   const Action* action = findAction(actionname);
 507 |   if (action) {
 508 |     doActionInternal(action, fr, to);
 509 |     return true;
 510 |   }
 511 |   
 512 |   return false;
 513 | }
 514 | 
 515 | 
 516 | 
 517 | std::string ActionPlanner::description() const
 518 | {
 519 |   std::stringstream sstream;
 520 |   
 521 |   // Print the proposition list
 522 |   sstream << "Propositions:";
 523 |   //for (prop_index_map_t::right_const_iterator it = proposition_index_.right.begin(); it != proposition_index_.right.end(); it++) {
 524 |   for (std::size_t i = 0, size = proposition_index_.size(); i < size; ++i) {
 525 |     sstream << " " << proposition_index_.left.at(i);
 526 |   }
 527 |   sstream << "\n\n";
 528 |   
 529 |   // Print the actions
 530 |   BOOST_FOREACH (const Action& action, actions_) {
 531 |     
 532 |     const Worldstate* pre = &(action.pre);
 533 |     const Worldstate* pst = &(action.pst);
 534 |     
 535 |     // Print preconditions
 536 |     sstream << "Action: " << action.name << "\nPreconditions:\n";
 537 |     BOOST_FOREACH (Action::condition_t precondition, action.preconditions) {
 538 |       sstream << "\t" << precondition.first.name << ": " << precondition.second << "\n";
 539 |     }
 540 |     
 541 |     // Print effects
 542 |     sstream << "Effects:\n";
 543 |     BOOST_FOREACH (Action::condition_t effect, action.effects) {
 544 |       sstream << "\t" << effect.first.name << ": " << effect.second << "\n";
 545 |     }
 546 |     
 547 |     sstream << "\n";
 548 |   }
 549 |   return sstream.str();
 550 | }
 551 | 
 552 | 
 553 | 
 554 | std::string ActionPlanner::worldstateDescription(const Worldstate* ws) const
 555 | {
 556 |   std::stringstream sstream;
 557 |   // Iterate through all propositions in planner
 558 |   for (uint i = 0; i < proposition_index_.size(); i++) {
 559 |     
 560 |     // Ignore if care == false
 561 |     if (ws->care[i]) {
 562 |       
 563 |       std::string proposition_name = proposition_index_.left.at(i);
 564 |       
 565 |       // Add ~ prefix for false
 566 |       if (!ws->values[i]) {
 567 | 	proposition_name = "~" + proposition_name;
 568 |       }
 569 |       
 570 |       sstream << proposition_name << ", ";
 571 |     }
 572 |   }
 573 |   sstream << std::endl;
 574 |   
 575 |   return sstream.str();
 576 | }
 577 | 
 578 | 
 579 | 
 580 | void ActionPlanner::updateWorldstateSize(Worldstate* ws) const
 581 | {
 582 |   ws->initialize(proposition_index_.size());
 583 | }
 584 | 
 585 | 
 586 | 
 587 | ActionPlanner::Action* ActionPlanner::addAction(std::string actionname)
 588 | {
 589 |   Action* new_action = findAction(actionname);
 590 |   if (!new_action) {
 591 |     new_action = new Action(proposition_index_.size());
 592 |     new_action->name = actionname;
 593 |     actions_.push_back(*new_action);
 594 |     
 595 |     delete new_action;
 596 |     new_action = &actions_.back();
 597 |   }
 598 |   
 599 |   return new_action;
 600 | }
 601 | 
 602 | 
 603 |   
 604 | bool ActionPlanner::addProposition(std::string name)
 605 | {  
 606 |   // See if proposition already exists
 607 |   try {
 608 |     proposition_index_.right.at(name);
 609 |   } catch (std::out_of_range) { // Does not exist
 610 |     proposition_index_.insert( prop_index_t(proposition_index_.size(), name) );
 611 |     
 612 |     // Update size of all action preconditions and postconditions
 613 |     for (action_vector::iterator it = actions_.begin(); it != actions_.end(); ++it) {
 614 |       updateWorldstateSize(&it->pre);
 615 |       updateWorldstateSize(&it->pst);
 616 |     }
 617 |     
 618 |     return true;
 619 |   }
 620 |     
 621 |   return false;
 622 | }
 623 | 
 624 | 
 625 | 
 626 | ActionPlanner::NodeMap::sequence_iter ActionPlanner::getLowestRank(NodeMap* nodes) const
 627 | {
 628 |   NodeMap::sequence_iter lowest_rank = nodes->getSequence().begin();
 629 |   for (NodeMap::sequence_iter it = nodes->getSequence().begin(), end = nodes->getSequence().end(); it != end; ++it) {
 630 |     if ((**lowest_rank).f > (**it).f)
 631 |       lowest_rank = it;
 632 |   }
 633 |   return lowest_rank;
 634 | }
 635 | 
 636 | 
 637 | 
 638 | void ActionPlanner::reconstructPlan(astarnode_t* goalnode, NodeMap* nodes, action_plan_t* plan, std::vector<Worldstate>* worldstates) const
 639 | {
 640 |   plan->clear();
 641 |   worldstates->clear();
 642 |   
 643 |   const astarnode_t* curnode = goalnode;
 644 |   while (curnode != NULL && curnode->actionname != "start") {
 645 |     plan->insert(plan->begin(), curnode->actionname);
 646 |     worldstates->insert(worldstates->begin(), curnode->ws);
 647 |     
 648 |     const NodeMap::hash_iter it = nodes->getHashed().find(*curnode->parentws);
 649 |     if (it != nodes->getHashed().end())
 650 |       curnode = *it;
 651 |     else
 652 |       curnode = NULL;
 653 |   }
 654 | }
 655 | 
 656 | 
 657 | 
 658 | int ActionPlanner::getPossibleStateTransitions(const Worldstate* fr, std::vector<Worldstate>* to, std::vector<const std::string*>* actionnames, std::vector<int>* actioncosts) const
 659 | {
 660 |   Worldstate to_ws;
 661 |   updateWorldstateSize(&to_ws);
 662 |   
 663 |   for (action_vector::const_iterator it = actions_.begin(); it != actions_.end(); ++it) {
 664 |     const Worldstate* pre = &(it->pre);
 665 |     
 666 |     if ((pre->values & pre->care) == (fr->values & pre->care)) {
 667 |       actionnames->push_back(&(it->name));
 668 |       actioncosts->push_back(it->get_cost(this, fr));
 669 |       
 670 |       doActionInternal(&(*it), fr, &to_ws);
 671 |       to->push_back(to_ws);
 672 |     }
 673 |   }
 674 |   
 675 |   return actionnames->size();
 676 | }
 677 | 
 678 | 
 679 | 
 680 | bool ActionPlanner::doActionInternal(const Action* action, const Worldstate* fr, Worldstate* to) const
 681 | {
 682 |   const Worldstate* pst = &action->pst;
 683 |   boost::dynamic_bitset<> notcare = pst->care;
 684 |   notcare.flip();
 685 |   
 686 |   to->values = (fr->values & notcare) | (pst->values & pst->care);
 687 |   to->care = fr->care | pst->care;
 688 |   return true;
 689 | }
 690 | 
 691 | 
 692 | 
 693 | bool ActionPlanner::findPropositionIndex(std::string name, int* index) const
 694 | {  
 695 |   try {
 696 |     *index = proposition_index_.right.at(name);
 697 |     return true;
 698 |   } catch (std::out_of_range) { // Does not exist
 699 |     return false;
 700 |   }
 701 | }
 702 | 
 703 | 
 704 | 
 705 | ActionPlanner::Action* ActionPlanner::findAction(std::string actionname)
 706 | {
 707 |   Action* action = NULL;
 708 |   action_vector::iterator it = std::find(actions_.begin(), actions_.end(), actionname);
 709 |   
 710 |   if (it != actions_.end()) {
 711 |     action = &(*it);
 712 |   }
 713 |   
 714 |   return action;
 715 | }
 716 | 
 717 | 
 718 | 
 719 | const ActionPlanner::Action* ActionPlanner::findAction(std::string actionname) const
 720 | {
 721 |   const Action* action = NULL;
 722 |   action_vector::const_iterator it = std::find(actions_.begin(), actions_.end(), actionname);
 723 |   
 724 |   if (it != actions_.end()) {
 725 |     action = &(*it);
 726 |   }
 727 |   
 728 |   return action;
 729 | }
 730 | 
 731 | 
 732 | ActionPlanner::Action* ActionPlanner::findUnbuiltAction(std::string actionname)
 733 | {
 734 |   Action* action = NULL;
 735 | 
 736 |   action_vector::iterator it = std::find(unbuilt_actions_.begin(), unbuilt_actions_.end(), actionname);
 737 |   
 738 |   if (it != unbuilt_actions_.end()) {
 739 |     action = &(*it);
 740 |   }
 741 |   
 742 |   return action;
 743 | }
 744 | 
 745 | 
 746 | 
 747 | const ActionPlanner::Action* ActionPlanner::findUnbuiltAction(std::string actionname) const
 748 | {
 749 |   const Action* action = NULL;
 750 | 
 751 |   action_vector::const_iterator it = std::find(unbuilt_actions_.begin(), unbuilt_actions_.end(), actionname);
 752 |   
 753 |   if (it != unbuilt_actions_.end()) {
 754 |     action = &(*it);
 755 |   }
 756 |   
 757 |   return action;
 758 | }
 759 | 
 760 | 
 761 | 
 762 | ActionPlanner::EntitySet* ActionPlanner::findEntitySet(std::string name)
 763 | {
 764 |   EntitySet* entity_set = NULL;
 765 |   entity_list::iterator it = std::find(entities_.begin(), entities_.end(), name);
 766 |   
 767 |   if (it != entities_.end()) {
 768 |     entity_set = &(*it);
 769 |   }
 770 |   
 771 |   return entity_set;
 772 | }
 773 | 
 774 | 
 775 | 
 776 | const ActionPlanner::EntitySet* ActionPlanner::findEntitySet(std::string name) const
 777 | {
 778 |   const EntitySet* entity_set = NULL;
 779 |   entity_list::const_iterator it = std::find(entities_.begin(), entities_.end(), name);
 780 |   
 781 |   if (it != entities_.end()) {
 782 |     entity_set = &(*it);
 783 |   }
 784 |   
 785 |   return entity_set;
 786 | }
 787 | 
 788 | 
 789 | 
 790 | int ActionPlanner::calcH(const Worldstate* fr, const Worldstate* to ) const
 791 | {
 792 |   const boost::dynamic_bitset<> diff = ( (fr->values ^ to->values) & to->care );
 793 |   return (int)diff.count();
 794 | }
 795 | 
 796 | 
 797 | 
 798 | bool ActionPlanner::permuteAction(const Action &action, action_vector* result) const
 799 | {
 800 |   action_vector permutations;
 801 |   
 802 |   // Collect this action's entities
 803 |   std::vector<param_label> original_labels;
 804 |   std::vector<const EntitySet::member_list*> sets;
 805 |   sets.reserve(action.params.size());
 806 |   BOOST_FOREACH (Action::parameter param, action.params) {
 807 |     sets.push_back(param.set_members);
 808 |     original_labels.push_back(param.label);
 809 |   }
 810 |   
 811 |   Action new_action = action;
 812 |   std::vector<std::size_t> counters(sets.size());
 813 |   std::vector<param_label> permutation(sets.size());
 814 | 
 815 |   // Produce the action permutations
 816 |   do {
 817 |     // Build this permutation
 818 |     for (std::size_t i = 0, size = counters.size(); i != size; ++i) {
 819 |       permutation[i] = sets[i]->at(counters[i]);
 820 |     }
 821 |     
 822 |     // Verify that we obey the mutual excusion rule for the param labels
 823 |     if (elementUniqueness(original_labels, permutation)) {
 824 | 
 825 |       new_action.params = action.params;
 826 |       new_action.preconditions = action.preconditions;
 827 |       new_action.effects = action.effects;
 828 |       
 829 |       // Insert permuted params into the action
 830 |       int i = 0;
 831 |       BOOST_FOREACH (param_label original, original_labels) {
 832 | 	if (!substituteParamLabel(new_action, original, permutation[i])) {
 833 | 	  printf("ActionPlanner::permuteAction - Param substitution failed for action %s.", action.name.c_str());
 834 | 	  return false;
 835 | 	}
 836 | 	++i;
 837 |       }
 838 |       
 839 |       new_action.buildName();
 840 |       
 841 |       // Add to output
 842 |       permutations.push_back(new_action);
 843 |     }
 844 |   } while (next_permutation(sets, counters)); // Increments the permutation counter
 845 |   
 846 |   *result = permutations;
 847 |   return true;
 848 | }
 849 | 
 850 | 
 851 | 
 852 | bool ActionPlanner::substituteParamLabel(Action &action, param_label original, param_label replacement) const
 853 | {
 854 |   // Verify that the action contains the original parameter
 855 |   Action::parameter* action_param = action.getParameter(original);
 856 |   if (action_param == NULL) {
 857 |     printf("ActionPlanner::substituteParamLabel() - Action %s does not contain param %s.\n", action.name.c_str(), original.c_str());
 858 |     return false;
 859 |   }
 860 |   // Insert replacement label
 861 |   action_param->label = replacement;
 862 |   
 863 |   // Iterate over action preconditions
 864 |   BOOST_FOREACH(Action::condition_t &condition, action.preconditions) {
 865 | 
 866 |     // Iterate over the parameters of this condition
 867 |     BOOST_FOREACH(Proposition::parameter &param, condition.first.params) {
 868 | 	
 869 |       // if this parameter references an action parameter (is neither a literal nor all)
 870 |       if (!param.literal && !param.all) {
 871 | 	  
 872 | 	// check for match
 873 | 	if (param.label == original) {
 874 | 	  // Insert replacement label into parameter
 875 | 	  param.label = replacement;
 876 | 	}
 877 |       }
 878 |     }
 879 |     
 880 |     // Update name of this Proposition to use substituted params
 881 |     condition.first.buildName();
 882 |   }
 883 |   
 884 |   // Do the same thing for the effects
 885 |   BOOST_FOREACH(Action::condition_t &condition, action.effects) {
 886 | 
 887 |     // Iterate over the parameters of this condition
 888 |     BOOST_FOREACH(Proposition::parameter &param, condition.first.params) {
 889 | 	
 890 |       // if this parameter references an action parameter (is neither a literal nor all)
 891 |       if (!param.literal && !param.all) {
 892 | 	  
 893 | 	// check for match
 894 | 	if (param.label == original) {
 895 | 	  // Insert replacement label into parameter
 896 | 	  param.label = replacement;
 897 | 	}
 898 |       }
 899 |     }
 900 |     
 901 |     // Update name of this Proposition to use substituted params
 902 |     condition.first.buildName();
 903 |   }
 904 |   
 905 |   // Update the action name to use substituted params
 906 |   action.buildName();
 907 |   
 908 |   return true;
 909 | }
 910 | 
 911 | 
 912 | 
 913 | template <typename T>
 914 | bool ActionPlanner::elementUniqueness(const string_vec& labels, const std::vector<T>& v) const
 915 | {
 916 |   for (size_t i = 0, size = labels.size(); i < size; ++i) {
 917 |     for (size_t j = i+1, size = labels.size(); j < size; ++j) {
 918 |       if (labels[i] != labels[j]) { // if the labels are different...
 919 | 	if (v[i] == v[j]) {         // then the values must be different
 920 | 	  return false;
 921 | 	}
 922 |       }
 923 |     }
 924 |   }
 925 |   
 926 |   return true;
 927 | }
 928 | 
 929 | 
 930 | 
 931 | bool ActionPlanner::linkEntities(Action& action) const
 932 | {  
 933 |   // For each action parameter
 934 |   BOOST_FOREACH(Action::parameter& param, action.params) {
 935 |     
 936 |     // Confirm that the needed EntitySet exists
 937 |     const EntitySet* set = findEntitySet(param.type);
 938 |     if (set == NULL) {
 939 |       printf("ActionPlanner::associateEntities() - EntitySet %s not found for action %s.\n", param.type.c_str(), action.name.c_str());
 940 |       return false;
 941 |     }
 942 |     
 943 |     // Associate the set with the parameter
 944 |       param.set_members = &(set->members);
 945 |   }
 946 |   
 947 |   // For each precondition
 948 |   BOOST_FOREACH(Action::condition_t& precondition, action.preconditions) {
 949 |     
 950 |     // For each parameter of the proposition
 951 |     BOOST_FOREACH (Proposition::parameter& param, precondition.first.params) {
 952 |       
 953 |       // If this parameter references an entity set
 954 |       if (!param.entity_set_name.empty()) {
 955 | 	
 956 | 	// Confirm that the needed EntitySet exists
 957 | 	const EntitySet* set = findEntitySet(param.entity_set_name);
 958 | 	if (set == NULL) {
 959 | 	  printf("ActionPlanner::associateEntities() - Failed to link EntitySet %s in action %s.\n", param.entity_set_name.c_str(), action.name.c_str());
 960 | 	  return false;
 961 | 	}
 962 | 	
 963 | 	// Associate the set with parameter
 964 | 	param.set_members = &(set->members);
 965 | 	
 966 | 	// For a literal parameter, verify that the set contains the literal
 967 | 	if (param.literal) {
 968 | 	  // search the set members
 969 | 	  EntitySet::member_list::const_iterator it = std::find(param.set_members->begin(), param.set_members->end(), param.label);
 970 | 	  
 971 | 	  // not found
 972 | 	  if (it == param.set_members->end()) {
 973 | 	    printf("ActionPlanner::associateEntities() - Failed to link literal parameter %s in action %s to EntitySet %s.\n", param.label.c_str(), action.name.c_str(), param.entity_set_name.c_str());
 974 | 	    return false;
 975 | 	  }
 976 | 	  
 977 | 	  precondition.first.buildName();
 978 | 	}
 979 |       }
 980 |     }
 981 |   }
 982 |     
 983 |   // For each effect
 984 |   BOOST_FOREACH(Action::condition_t& effect, action.effects) {
 985 |     
 986 |     // For each parameter of the proposition
 987 |     BOOST_FOREACH (Proposition::parameter& param, effect.first.params) {
 988 |       
 989 |       // If this parameter references an entity set
 990 |       if (!param.entity_set_name.empty()) {
 991 | 	
 992 | 	// Confirm that the needed EntitySet exists
 993 | 	const EntitySet* set = findEntitySet(param.entity_set_name);
 994 | 	if (set == NULL) {
 995 | 	  printf("ActionPlanner::associateEntities() - Failed to link EntitySet %s in action %s.\n", param.entity_set_name.c_str(), action.name.c_str());
 996 | 	  return false;
 997 | 	}
 998 | 	
 999 | 	// Associate the set with parameter
1000 | 	param.set_members = &(set->members);
1001 | 	
1002 | 	// For a literal parameter, verify that the set contains the literal
1003 | 	if (param.literal) {
1004 | 	  // search the set members
1005 | 	  EntitySet::member_list::const_iterator it = std::find(param.set_members->begin(), param.set_members->end(), param.label);
1006 | 	  
1007 | 	  // not found
1008 | 	  if (it == param.set_members->end()) {
1009 | 	    printf("ActionPlanner::associateEntities() - Failed to link literal parameter %s in action %s to EntitySet %s.\n", param.label.c_str(), action.name.c_str(), param.entity_set_name.c_str());
1010 | 	    return false;
1011 | 	  }
1012 | 	  
1013 | 	  effect.first.buildName();
1014 | 	}
1015 |       }
1016 |     }
1017 |   }
1018 |   
1019 |   return true;
1020 | }
1021 | 
1022 | 
1023 | 
1024 | bool ActionPlanner::expandAllParams(Action &action) const
1025 | {
1026 |   std::vector<ActionPlanner::Action::condition_t> expanded_conditions;
1027 |   
1028 |   // Iterate over action preconditions
1029 |   for(std::vector<Action::condition_t>::iterator condition = action.preconditions.begin(); condition != action.preconditions.end(); ) {
1030 | 
1031 |     // Build expanded conditions
1032 |     expanded_conditions = expandAllCondition(*condition);
1033 |     
1034 |     // We found a condition to expand
1035 |     if (!expanded_conditions.empty()) {
1036 |       // Delete the condition that we expanded
1037 |       action.preconditions.erase(condition);
1038 |       
1039 |       // Append expanded conditions to action
1040 |       action.preconditions.insert(action.preconditions.end(), expanded_conditions.begin(), expanded_conditions.end());
1041 |       
1042 |       // Reset the iteration to the beginning
1043 |       condition = action.preconditions.begin();
1044 |       
1045 |       expanded_conditions.clear();
1046 |     }
1047 |     else {
1048 |       ++condition;
1049 |     }
1050 |   }
1051 |   
1052 |   // Iterate over action preconditions
1053 |   for(std::vector<Action::condition_t>::iterator condition = action.effects.begin(); condition != action.effects.end(); ) {
1054 | 
1055 |     // Build expanded conditions
1056 |     expanded_conditions = expandAllCondition(*condition);
1057 |     
1058 |     // We found a condition to expand
1059 |     if (!expanded_conditions.empty()) {
1060 |       // Delete the condition that we expanded
1061 |       action.effects.erase(condition);
1062 |       
1063 |       // Append expanded conditions to action
1064 |       action.effects.insert(action.effects.end(), expanded_conditions.begin(), expanded_conditions.end());
1065 |       
1066 |       // Reset the iteration to the beginning
1067 |       condition = action.effects.begin();
1068 |       
1069 |       expanded_conditions.clear();
1070 |     }
1071 |     else {
1072 |       ++condition;
1073 |     }
1074 |   }
1075 |   
1076 |   return true;
1077 | }
1078 | 
1079 | 
1080 | 
1081 | std::vector<ActionPlanner::Action::condition_t> ActionPlanner::expandAllCondition(Action::condition_t condition) const
1082 | {
1083 |   std::vector<Action::condition_t> output;
1084 | 
1085 |   // Search for an ALL parameter
1086 |   BOOST_FOREACH(Proposition::parameter& param, condition.first.params) {
1087 |     if (param.all) {
1088 |       param.all = false;
1089 |       
1090 |       // Expand this parameter
1091 |       BOOST_FOREACH (param_label member, *param.set_members) {
1092 | 	param.label = member;
1093 | 	condition.first.buildName();
1094 | 	output.push_back(condition);
1095 |       }
1096 |       break;
1097 |     }
1098 |   }
1099 |   
1100 |   return output;
1101 | }
1102 | 
1103 | 
1104 | 
1105 | 
1106 | bool ActionPlanner::expandNegatedParams(Action &action) const
1107 | {
1108 |   std::vector<ActionPlanner::Action::condition_t> expanded_conditions;
1109 |   
1110 |   // Iterate over action preconditions
1111 |   for(std::vector<Action::condition_t>::iterator condition = action.preconditions.begin(); condition != action.preconditions.end(); ) {
1112 | 
1113 |     // Build expanded conditions
1114 |     expanded_conditions = expandNegatedCondition(*condition);
1115 |     
1116 |     // We found a condition to expand
1117 |     if (!expanded_conditions.empty()) {
1118 |       // Delete the condition that we expanded
1119 |       action.preconditions.erase(condition);
1120 |       
1121 |       // Append expanded conditions to action
1122 |       action.preconditions.insert(action.preconditions.end(), expanded_conditions.begin(), expanded_conditions.end());
1123 |       
1124 |       // Reset the iteration to the beginning
1125 |       condition = action.preconditions.begin();
1126 |       
1127 |       expanded_conditions.clear();
1128 |     }
1129 |     else {
1130 |       ++condition;
1131 |     }
1132 |   }
1133 |   
1134 |   // Iterate over action preconditions
1135 |   for(std::vector<Action::condition_t>::iterator condition = action.effects.begin(); condition != action.effects.end(); ) {
1136 | 
1137 |     // Build expanded conditions
1138 |     expanded_conditions = expandNegatedCondition(*condition);
1139 | 
1140 |     // We found a condition to expand
1141 |     if (!expanded_conditions.empty()) {
1142 |       // Delete the condition that we expanded
1143 |       action.effects.erase(condition);
1144 |       
1145 |       // Append expanded conditions to action
1146 |       action.effects.insert(action.effects.end(), expanded_conditions.begin(), expanded_conditions.end());
1147 |       
1148 |       // Reset the iteration to the beginning
1149 |       condition = action.effects.begin();
1150 |       
1151 |       expanded_conditions.clear();
1152 |     }
1153 |     else {
1154 |       ++condition;
1155 |     }
1156 |   }
1157 |   
1158 |   return true;
1159 | }
1160 | 
1161 | 
1162 | 
1163 | std::vector<ActionPlanner::Action::condition_t> ActionPlanner::expandNegatedCondition(Action::condition_t condition) const
1164 | {
1165 |   std::vector<Action::condition_t> output;
1166 | 
1167 |   // Search for an ALL parameter
1168 |   BOOST_FOREACH(Proposition::parameter& param, condition.first.params) {
1169 |     if (param.negated) {
1170 |       std::string original = param.label;
1171 |       param.negated = false;
1172 |       
1173 |       // Expand this parameter
1174 |       BOOST_FOREACH (param_label member, *param.set_members) {
1175 | 	if (member != original) {
1176 | 	  param.label = member;
1177 | 	  condition.first.buildName();
1178 | 	  output.push_back(condition);
1179 | 	}
1180 |       }
1181 |       break;
1182 |     }
1183 |   }
1184 |   
1185 |   return output;
1186 | }
1187 | 
1188 | 
1189 | } // namespace goap


--------------------------------------------------------------------------------