Merge pull request #320 from Rezenders/exists

Support for existential preconditions

plansys2_msgs/msg/Node.msg

@@ -11,24 +11,25 @@ uint8 FUNCTION = 6
 uint8 EXPRESSION = 7
 uint8 FUNCTION_MODIFIER = 8
 uint8 NUMBER = 9
+uint8 EXISTS = 10
 
 # Expression types
-uint8 COMP_EQ = 10
-uint8 COMP_GE = 11
-uint8 COMP_GT = 12
-uint8 COMP_LE = 13
-uint8 COMP_LT = 14
-uint8 ARITH_MULT = 15
-uint8 ARITH_DIV = 16
-uint8 ARITH_ADD = 17
-uint8 ARITH_SUB = 18
+uint8 COMP_EQ = 11
+uint8 COMP_GE = 12
+uint8 COMP_GT = 13
+uint8 COMP_LE = 14
+uint8 COMP_LT = 15
+uint8 ARITH_MULT = 16
+uint8 ARITH_DIV = 17
+uint8 ARITH_ADD = 18
+uint8 ARITH_SUB = 19
 
 # Function modifier types
-uint8 ASSIGN = 19
-uint8 INCREASE = 20
-uint8 DECREASE = 21
-uint8 SCALE_UP = 22
-uint8 SCALE_DOWN = 23
+uint8 ASSIGN = 20
+uint8 INCREASE = 21
+uint8 DECREASE = 22
+uint8 SCALE_UP = 23
+uint8 SCALE_DOWN = 24
 
 uint8 node_type
 uint8 expression_type

plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h

@@ -34,6 +34,7 @@ class Domain {
 	bool universal;                     // whether domain has universal precons
 	bool fluents;                       // whether domains contains fluents
 	bool derivedpred;                   // whether domain contains derived predicates
+	bool existentialcond;               // whether domain contains existential predicates
 
 	TokenStruct< Type * > types;        // types
 	TokenStruct< Lifted * > preds;      // predicates
@@ -46,7 +47,7 @@ class Domain {
 		: equality( false ), strips( false ), adl( false ), condeffects( false )
 		, typed( false ), cons( false ), costs( false ), temp( false )
 		, nondet( false ), neg( false ), disj( false ), universal( false )
-		, fluents( false ), derivedpred( false )
+		, fluents( false ), derivedpred( false ), existentialcond( false )
 	{
 		types.insert( new Type( "object" ) ); // Type 0 is always "object", whether the domain is typed or not
 	}
@@ -137,6 +138,7 @@ class Domain {
 		else if ( s == "fluents" ) fluents = true;
 		else if ( s == "disjunctive-preconditions" ) disj = true;
 		else if ( s == "derived-predicates" ) derivedpred = true;
+		else if ( s == "existential-preconditions" ) existentialcond = true;
 		else return false; // Unknown requirement
 
 		return true;
@@ -594,6 +596,7 @@ class Domain {
 		if ( fluents ) os << " :fluents";
 		if ( disj ) os << " :disjunctive-preconditions";
 		if ( derivedpred ) os << " :derived-predicates";
+		if ( existentialcond ) os << " :existential-preconditions";
 		os << " )\n";
 		return os;
 	}

plansys2_pddl_parser/include/plansys2_pddl_parser/Utils.h

@@ -100,6 +100,8 @@ std::string toStringExpression(const plansys2_msgs::msg::Tree & tree, uint32_t n
 
 std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate);
 
+std::string toStringExists(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate);
+
 /// This function creates a complete tree.
 /**
  * This function recursivelly extracts the logic expressions and predicates from the expression.
@@ -115,6 +117,8 @@ plansys2_msgs::msg::Tree fromString(const std::string & expr, bool negate = fals
 
 plansys2_msgs::msg::Node fromStringPredicate(const std::string & predicate);
 
+plansys2_msgs::msg::Node fromStringExists(const std::string & exists);
+
 plansys2_msgs::msg::Node fromStringFunction(const std::string & function);
 
 plansys2_msgs::msg::Param fromStringParam(const std::string & name, const std::string & type = {});

plansys2_pddl_parser/src/plansys2_pddl_parser/Exists.cpp

@@ -5,12 +5,13 @@ namespace parser { namespace pddl {
 
 void Exists::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< std::string > & ts, const Domain & d ) const {
 	tabindent( s, indent );
-	s << "( exists\n";
+	s << "( exists ";
 
 	TokenStruct< std::string > fstruct( ts );
 
-	tabindent( s, indent + 1 );
-	printParams( 0, s, fstruct, d );
+	for ( unsigned i = 0; i < params.size(); ++i ) {
+    s << " ?" + std::to_string(params[i]);
+  }
 
 	if ( cond ) cond->PDDLPrint( s, indent + 1, fstruct, d );
 	else {
@@ -23,18 +24,38 @@ void Exists::PDDLPrint( std::ostream & s, unsigned indent, const TokenStruct< st
 }
 
 plansys2_msgs::msg::Node::SharedPtr Exists::getTree( plansys2_msgs::msg::Tree & tree, const Domain & d, const std::vector<std::string> & replace ) const {
-    throw UnsupportedConstruct("Exists");
+  plansys2_msgs::msg::Node::SharedPtr node = std::make_shared<plansys2_msgs::msg::Node>();
+  node->node_type = plansys2_msgs::msg::Node::EXISTS;
+  node->node_id = tree.nodes.size();
+  for ( unsigned i = 0; i < params.size(); ++i ) {
+      plansys2_msgs::msg::Param param;
+      if (i < replace.size() && params[i]<replace.size()) {
+        if (params[i] >= 0) {
+          // param has a variable value; replace by action-args
+          param.name = replace[params[i]];
+        }
+      } else {
+        param.name = "?" + std::to_string(params[i]);
+      }
+      node->parameters.push_back(param);
+  }
+
+  tree.nodes.push_back(*node);
+  plansys2_msgs::msg::Node::SharedPtr child = cond->getTree(tree, d, replace);
+  tree.nodes[node->node_id].children.push_back(child->node_id);
+
+  return node;
 }
 
 void Exists::parse( Stringreader & f, TokenStruct< std::string > & ts, Domain & d ) {
 	f.next();
 	f.assert_token( "(" );
 
 	TokenStruct< std::string > es = f.parseTypedList( true, d.types );
-	params = d.convertTypes( es.types );
+  TokenStruct< std::string > estruct( ts );
+  params = incvec(estruct.size(), estruct.size() + es.size());
 
-	TokenStruct< std::string > estruct( ts );
-	estruct.append( es );
+  estruct.append( es );
 
 	f.next();
 	f.assert_token( "(" );

plansys2_pddl_parser/src/plansys2_pddl_parser/Ground.cpp

@@ -38,7 +38,7 @@ plansys2_msgs::msg::Node::SharedPtr Ground::getTree( plansys2_msgs::msg::Tree &
     node->name = name;
     for ( unsigned i = 0; i < params.size(); ++i ) {
         plansys2_msgs::msg::Param param;
-        if (i < replace.size()) {
+        if (i < replace.size() && params[i]<replace.size()) {
           if (params[i] >= 0) {
             // param has a variable value; replace by action-args
             param.name = replace[params[i]];

plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp

@@ -49,6 +49,9 @@ static void printN(std::ostream & s, uint8_t node_type) {
     case plansys2_msgs::msg::Node::NUMBER:
       s << "NODE TYPE NUM" << std::endl;
       break;
+    case plansys2_msgs::msg::Node::EXISTS:
+      s << "NODE TYPE EXISTS" << std::endl;
+      break;
     case plansys2_msgs::msg::Node::COMP_GE:
       s << "NODE TYPE COMP GE" << std::endl;
       break;
@@ -139,6 +142,13 @@ uint8_t getNodeType(const std::string & expr, uint8_t default_node_type)
     }
   }
 
+  if (std::regex_search(expr, match, std::regex("\\(\\s*exists[ (]"))) {
+    if (static_cast<int>(match.position()) < first) {
+      first = static_cast<int>(match.position());
+      node_type = plansys2_msgs::msg::Node::EXISTS;
+    }
+  }
+
   std::tuple<uint8_t, int> modifier_search_result = getFunMod(expr);
   if (std::get<0>(modifier_search_result) != plansys2_msgs::msg::Node::UNKNOWN) {
     if (std::get<1>(modifier_search_result) < first) {
@@ -438,6 +448,9 @@ std::string toString(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bo
     case plansys2_msgs::msg::Node::FUNCTION_MODIFIER:
       ret = toStringFunctionModifier(tree, node_id, negate);
       break;
+    case plansys2_msgs::msg::Node::EXISTS:
+      ret = toStringExists(tree, node_id, negate);
+      break;
     default:
       std::cerr << "Unsupported node to string conversion" << std::endl;
       break;
@@ -680,6 +693,33 @@ std::string toStringFunctionModifier(const plansys2_msgs::msg::Tree & tree, uint
   return ret;
 }
 
+std::string toStringExists(const plansys2_msgs::msg::Tree & tree, uint32_t node_id, bool negate)
+{
+  if (node_id >= tree.nodes.size()) {
+    return {};
+  }
+
+  if (tree.nodes[node_id].children.empty()) {
+    return {};
+  }
+
+  std::string ret = "(exists (";
+
+  bool first_param = true;
+  for (const auto & param : tree.nodes[node_id].parameters) {
+    ret += first_param ? param.name : " " + param.name;
+    first_param = false;
+  }
+  ret+= ") ";
+
+  for (auto child_id : tree.nodes[node_id].children) {
+    ret += toString(tree, child_id, negate);
+  }
+  ret += ")";
+
+  return ret;
+}
+
 plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, const std::string & expr, bool negate, uint8_t parent)
 {
   std::string wexpr = getReducedString(expr);
@@ -816,6 +856,22 @@ plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree,
 
         return node;
     }
+
+    case plansys2_msgs::msg::Node::EXISTS: {
+      auto node = std::make_shared<plansys2_msgs::msg::Node>(fromStringExists(wexpr));
+      node->node_id = tree.nodes.size();
+      node->negate = negate;
+      tree.nodes.push_back(*node);
+
+      size_t begin_exists = wexpr.find("exists", 0) + 6;
+      size_t end_exists = wexpr.find(")", begin_exists);
+      wexpr = wexpr.substr(end_exists + 1, std::string::npos);
+
+      auto child = fromString(tree, wexpr, negate, node_type);
+      tree.nodes[node->node_id].children.push_back(child->node_id);
+
+      return node;
+    }
     // LCOV_EXCL_START
     default:
       std::cerr << "fromString: Error parsing expresion [" << wexpr << "]" << std::endl;
@@ -865,6 +921,39 @@ plansys2_msgs::msg::Node fromStringPredicate(const std::string & predicate)
   return ret;
 }
 
+plansys2_msgs::msg::Node fromStringExists(const std::string & exists)
+{
+  plansys2_msgs::msg::Node ret;
+  ret.node_type = plansys2_msgs::msg::Node::EXISTS;
+
+  std::vector<std::string> tokens;
+  size_t start = 0;
+  size_t end_exists = exists.find(")", start);
+  size_t end = 0;
+
+  while (end < end_exists) {
+    end = exists.find(" ", start);
+    tokens.push_back(
+      exists.substr(
+        start,
+        (end == std::string::npos) ? std::string::npos : end - start));
+    start = ((end > (std::string::npos - 1)) ? std::string::npos : end + 1);
+  }
+
+  tokens.back().pop_back();
+  tokens[1].erase(0, 1);
+
+  for (size_t i = 1; i < tokens.size(); i++) {
+    plansys2_msgs::msg::Param param;
+    param.name = tokens[i];
+    ret.parameters.push_back(param);
+  }
+
+  ret.value = 0;
+
+  return ret;
+}
+
 plansys2_msgs::msg::Node fromStringFunction(const std::string & function)
 {
   plansys2_msgs::msg::Node ret;

plansys2_pddl_parser/test/pddl/dom1.pddl

@@ -1,5 +1,5 @@
 (define (domain simple)
-(:requirements :strips :typing :adl :fluents :durative-actions)
+(:requirements :strips :typing :adl :fluents :durative-actions :existential-preconditions)
 (:types
     robot
     room
@@ -79,4 +79,32 @@
     )
 )
 
-)
+(:action action_test4
+    :parameters (?r - robot)
+    :precondition (and
+      (exists (?ro)
+        (and
+          (robot_at ?r ?ro)
+          (charging_point_at ?ro)
+        )
+      )
+      (and (> (battery_level ?r) 1) (< (battery_level ?r) 200))
+    )
+    :effect (and
+      (battery_full ?r)
+    )
+)
+
+(:action action_test5
+    :parameters (?r - robot)
+    :precondition
+      (exists (?ro1 ?ro2)
+        (and
+          (robot_at ?r ?ro1)
+          (connected ?ro1 ?ro2)
+        )
+      )
+    :effect (and )
+)
+
+)

plansys2_pddl_parser/test/pddl_parser_test.cpp

@@ -67,9 +67,45 @@ TEST(PDDLParserTestCase, pddl_parser)
       std::cout << instance << std::endl;
       okprint = true;
     } catch (std::runtime_error e) {
+      std::cerr << e.what() << std::endl;
     }
   } catch (std::runtime_error e) {
+    std::cerr << e.what() << std::endl;
   }
   ASSERT_TRUE(okparse);
   ASSERT_TRUE(okprint);
 }
+
+TEST(PDDLParserTestCase, exists_get_tree)
+{
+  std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_pddl_parser");
+  std::string domain_file = pkgpath + "/pddl/dom1.pddl";
+
+  std::ifstream domain_ifs(domain_file);
+  std::string domain_str((
+      std::istreambuf_iterator<char>(domain_ifs)),
+                         std::istreambuf_iterator<char>());
+  parser::pddl::Domain domain( domain_str );
+
+  auto action = domain.actions.get("action_test4");
+  plansys2_msgs::msg::Tree tree;
+  action->pre->getTree(tree, domain);
+  std::string str = parser::pddl::toString(tree);
+
+  ASSERT_EQ(str,
+    "(and (exists (?1) (and (robot_at ?0 ?1)(charging_point_at ?1)))(and (>  (battery_level ?0) 1.000000)(<  (battery_level ?0) 200.000000)))");
+
+  plansys2_msgs::msg::Tree tree2;
+  std::vector<std::string> replace = {"rob1"};
+  action->pre->getTree(tree2, domain, replace);
+  std::string str2 = parser::pddl::toString(tree2);
+  ASSERT_EQ(str2,
+    "(and (exists (?1) (and (robot_at rob1 ?1)(charging_point_at ?1)))(and (>  (battery_level rob1) 1.000000)(<  (battery_level rob1) 200.000000)))");
+
+  auto action2 = domain.actions.get("action_test5");
+  plansys2_msgs::msg::Tree tree3;
+  action2->pre->getTree(tree3, domain);
+  std::string str3 = parser::pddl::toString(tree3);
+  ASSERT_EQ(str3,
+    "(exists (?1 ?2) (and (robot_at ?0 ?1)(connected ?1 ?2)))");
+}

plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp

@@ -113,7 +113,7 @@ POPFPlanSolver::getPlan(
   problem_out.close();
 
   RCLCPP_INFO(
-    lc_node_->get_logger(), "[%s-popf] called with timeout %d seconds",
+    lc_node_->get_logger(), "[%s-popf] called with timeout %f seconds",
     lc_node_->get_name(), solver_timeout.seconds());
 
   const auto plan_file_path = output_dir / std::filesystem::path("plan");

plansys2_problem_expert/include/plansys2_problem_expert/Utils.hpp

@@ -177,4 +177,5 @@ void  cart_product(
 
 }  // namespace plansys2
 
+
 #endif  // PLANSYS2_PROBLEM_EXPERT__UTILS_HPP_