Automating Robot Programming Through Constraint Solving and Motion Planning
The project aims to develop a high-level programming framework, called Robosynth, for personal robots. Here, rather than writing low-level code that defines how a robot must perform a task, the user of the robot writes a specification that defines what is to be accomplished. Given this specification and a model of the robot's environment, Robosynth automatically synthesizes a program that can be executed on the robot. So long as the environment behaves according to the assumed model, all executions of this program are guaranteed to satisfy the user-defined requirements.This approach and its derivatives can make robot programming accessible to a vast untapped body of inexperienced programmers. The technical highlights of the project are the specification language using which users interact with Robosynth, and the algorithms that Robosynth uses for automatic code synthesis. These algorithms simultaneously reason about a logical task level that is concerned with the high-level goals of the robot, as well as a continuous motion level concerned with navigating and manipulating a physical space. At the task level, Robosynth leverages recent methods for analyzing complex systems of logical constraints, for example SMT-solving and symbolic solution of graph games. Motion-level reasoning is performed using sampling-based motion planning techniques.
This work has been supported by grant NSF SHF 1514372.
Related Publications
- Y. Wang, S. Chaudhuri, and L. E. Kavraki, “Point-Based Policy Synthesis for POMDPs with Boolean and Quantitative Objectives,” IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. 1860–1867, Apr. 2019.
BibTeX
@article{wang2019point-based-policy, abstract = {Effectively planning robust executions under uncertainty is critical for building autonomous robots. Partially Observable Markov Decision Processes (POMDPs) provide a standard framework for modeling many robot applications under uncertainty. We study POMDPs with two kinds of objectives: (1) boolean objectives for a correctness guarantee of accomplishing tasks and (2) quantitative objectives for optimal behaviors. For robotic domains that require both correctness and optimality, POMDPs with boolean and quantitative objectives are natural formulations. We present a practical policy synthesis approach for POMDPs with boolean and quantitative objectives by combining policy iteration and policy synthesis for POMDPs with only boolean objectives. To improve efficiency, our approach produces approximate policies by performing the point-based backup on a small set of representative beliefs. Despite being approximate, our approach maintains validity (satisfying boolean objectives) and guarantees improved policies at each iteration before termination. Moreover, the error due to approximation is bounded. We evaluate our approach in several robotic domains. The results show that our approach produces good approximate policies that guarantee task completion.}, author = {Wang, Yue and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {Point-Based Policy Synthesis for {POMDP}s with Boolean and Quantitative Objectives}, journal = {IEEE Robotics and Automation Letters}, year = {2019}, volume = {4}, number = {2}, pages = {1860--1867}, month = apr, keywords = {planning from high-level specifications}, doi = {10.1109/LRA.2019.2898045} }DetailsAbstract
Effectively planning robust executions under uncertainty is critical for building autonomous robots. Partially Observable Markov Decision Processes (POMDPs) provide a standard framework for modeling many robot applications under uncertainty. We study POMDPs with two kinds of objectives: (1) boolean objectives for a correctness guarantee of accomplishing tasks and (2) quantitative objectives for optimal behaviors. For robotic domains that require both correctness and optimality, POMDPs with boolean and quantitative objectives are natural formulations. We present a practical policy synthesis approach for POMDPs with boolean and quantitative objectives by combining policy iteration and policy synthesis for POMDPs with only boolean objectives. To improve efficiency, our approach produces approximate policies by performing the point-based backup on a small set of representative beliefs. Despite being approximate, our approach maintains validity (satisfying boolean objectives) and guarantees improved policies at each iteration before termination. Moreover, the error due to approximation is bounded. We evaluate our approach in several robotic domains. The results show that our approach produces good approximate policies that guarantee task completion. - F. Lagriffoul, N. Dantam, C. Garrett, A. Akbari, S. Srivastava, and L. E. Kavraki, “Platform-Independent Benchmarks for Task and Motion Planning,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 3765–3772, Oct. 2018.
BibTeX
@article{lagriffoul2018tmp-benchmarks, abstract = {We present the first platform-independent evaluation method for task and motion planning (TAMP). Previously point, various problems have been used to test individual planners for specific aspects of TAMP. However, no common set of metrics, formats, and problems have been accepted by the community. We propose a set of benchmark problems covering the challenging aspects of TAMP and a planner-independent specification format for these problems. Our objective is to better evaluate and compare TAMP planners, foster communication, and progress within the field, and lay a foundation to better understand this class of planning problems.}, author = {Lagriffoul, Fabien and Dantam, Neil and Garrett, Caelan and Akbari, Aliakbar and Srivastava, Siddharth and Kavraki, Lydia E.}, title = {Platform-Independent Benchmarks for Task and Motion Planning}, journal = {IEEE Robotics and Automation Letters}, year = {2018}, month = oct, volume = {3}, issue = {4}, pages = {3765--3772}, doi = {10.1109/LRA.2018.2856701}, keywords = {planning from high-level specifications; uncertainty} }DetailsAbstract
We present the first platform-independent evaluation method for task and motion planning (TAMP). Previously point, various problems have been used to test individual planners for specific aspects of TAMP. However, no common set of metrics, formats, and problems have been accepted by the community. We propose a set of benchmark problems covering the challenging aspects of TAMP and a planner-independent specification format for these problems. Our objective is to better evaluate and compare TAMP planners, foster communication, and progress within the field, and lay a foundation to better understand this class of planning problems. - N. T. Dantam, S. Chaudhuri, and L. E. Kavraki, “The Task Motion Kit,” Robotics and Automation Magazine, vol. 25, no. 3, pp. 61–70, Sep. 2018.
BibTeX
@article{dantam2018task-motion-kit, abstract = {Robots require novel reasoning systems to achieve complex objectives in new environments. Daily activities in the physical world combine two types of reasoning: discrete and continuous. For example, to set the table, the robot must make discrete decisions about which and in what order to pick objects, and it must execute these decisions by computing continuous motions to reach objects or desired locations. Robotics has traditionally treated these issues in isolation. Reasoning about discrete events is referred to as task planning, while reasoning about and computing continuous motions is in the realm of motion planning. However, several recent works have shown that separating task planning from motion planning is problematic. This article provides an introduction to task-motion planning (TMP), this concept tightly couples task planning and motion planning, producing a sequence of steps that can actually be executed by a real robot. The implementation and use of an open-source TMP framework tht is adaptable to new robots is also discussed.}, author = {Dantam, Neil T. and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {The Task Motion Kit}, journal = {Robotics and Automation Magazine}, publisher = {IEEE}, year = {2018}, volume = {25}, number = {3}, month = sep, pages = {61--70}, doi = {10.1109/MRA.2018.2815081}, keywords = {planning from high-level specifications} }DetailsAbstract
Robots require novel reasoning systems to achieve complex objectives in new environments. Daily activities in the physical world combine two types of reasoning: discrete and continuous. For example, to set the table, the robot must make discrete decisions about which and in what order to pick objects, and it must execute these decisions by computing continuous motions to reach objects or desired locations. Robotics has traditionally treated these issues in isolation. Reasoning about discrete events is referred to as task planning, while reasoning about and computing continuous motions is in the realm of motion planning. However, several recent works have shown that separating task planning from motion planning is problematic. This article provides an introduction to task-motion planning (TMP), this concept tightly couples task planning and motion planning, producing a sequence of steps that can actually be executed by a real robot. The implementation and use of an open-source TMP framework tht is adaptable to new robots is also discussed. - Y. Wang, S. Chaudhuri, and L. E. Kavraki, “Online Partial Conditional Plan Synthesis for POMDPs with Safe-Reachability
Objectives,” in Workshop on the Algorithmic Foundations of Robotics, 2018.
BibTeX
@inproceedings{wang2018partial, title = {Online Partial Conditional Plan Synthesis for POMDPs with Safe-Reachability Objectives}, booktitle = {Workshop on the Algorithmic Foundations of Robotics}, year = {2018}, keywords = {planning from high-level specifications; uncertainty}, author = {Wang, Yue and Chaudhuri, Swarat and Kavraki, Lydia E.}, abstract = {The framework of Partially Observable Markov Decision Processes (POMDPs) offers a standard approach to model uncertainty in many robot tasks. Traditionally, POMDPs are formulated with optimality objectives. However, for robotic domains that require a correctness guarantee of accomplishing tasks, boolean objectives are natural formulations. We study POMDPs with a common boolean objective: safe-reachability, which requires that, with a probability above a threshold, the robot eventually reaches a goal state while keeping the probability of visiting unsafe states below a different threshold. The solutions to POMDPs are policies or conditional plans that specify the action to take contingent on every possible event. A full policy or conditional plan that covers all possible events is generally expensive to compute. To improve efficiency, we introduce the notion of partial conditional plans that only cover a sampled subset of all possible events. Our approach constructs a partial conditional plan parameterized by a replanning probability. We prove that the probability of the constructed partial conditional plan failing is bounded by the replanning probability. Our approach allows users to specify an appropriate bound on the replanning probability to balance efficiency and correctness. We validate our approach in several robotic domains. The results show that our approach outperforms a previous approach for POMDPs with safe-reachability objectives in these domains.} }DetailsAbstract
The framework of Partially Observable Markov Decision Processes (POMDPs) offers a standard approach to model uncertainty in many robot tasks. Traditionally, POMDPs are formulated with optimality objectives. However, for robotic domains that require a correctness guarantee of accomplishing tasks, boolean objectives are natural formulations. We study POMDPs with a common boolean objective: safe-reachability, which requires that, with a probability above a threshold, the robot eventually reaches a goal state while keeping the probability of visiting unsafe states below a different threshold. The solutions to POMDPs are policies or conditional plans that specify the action to take contingent on every possible event. A full policy or conditional plan that covers all possible events is generally expensive to compute. To improve efficiency, we introduce the notion of partial conditional plans that only cover a sampled subset of all possible events. Our approach constructs a partial conditional plan parameterized by a replanning probability. We prove that the probability of the constructed partial conditional plan failing is bounded by the replanning probability. Our approach allows users to specify an appropriate bound on the replanning probability to balance efficiency and correctness. We validate our approach in several robotic domains. The results show that our approach outperforms a previous approach for POMDPs with safe-reachability objectives in these domains. - Y. Wang, S. Chaudhuri, and L. E. Kavraki, “Bounded Policy Synthesis for POMDPs with Safe-Reachability Objectives,” in Proceedings of the 17th International Conference on Autonomous Agents and
Multiagent Systems, Stockholm, Sweden, 2018, pp. 238–246.
BibTeX
@inproceedings{wang2018bounded-policy-synthesis, abstract = {Planning robust executions under uncertainty is a fundamental challenge for building autonomous robots. Partially Observable Markov Decision Processes (POMDPs) provide a standard framework for modeling uncertainty in many applications. In this work, we study POMDPs with safe-reachability objectives, which require that with a probability above some threshold, a goal state is eventually reached while keeping the probability of visiting unsafe states below some threshold. This POMDP formulation is different from the traditional POMDP models with optimality objectives and we show that in some cases, POMDPs with safe-reachability objectives can provide a better guarantee of both safety and reachability than the existing POMDP models through an example. A key algorithmic problem for POMDPs is policy synthesis, which requires reasoning over a vast space of beliefs (probability distributions). To address this challenge, we introduce the notion of a goal-constrained belief space, which only contains beliefs reachable from the initial belief under desired executions that can achieve the given safe-reachability objective. Our method compactly represents this space over a bounded horizon using symbolic constraints, and employs an incremental Satisfiability Modulo Theories (SMT) solver to efficiently search for a valid policy over it. We evaluate our method using a case study involving a partially observable robotic domain with uncertain obstacles. The results show that our method can synthesize policies over large belief spaces with a small number of SMT solver calls by focusing on the goal-constrained belief space.}, address = {Stockholm, Sweden}, author = {Wang, Yue and Chaudhuri, Swarat and Kavraki, Lydia E.}, keywords = {planning from high-level specifications; uncertainty}, booktitle = {Proceedings of the 17th International Conference on Autonomous Agents and Multiagent Systems}, series = {AAMAS 2018}, title = {Bounded Policy Synthesis for {POMDP}s with Safe-Reachability Objectives}, year = {2018}, url = {http://dl.acm.org/citation.cfm?id=3237383.3237424}, acmid = {3237424}, pages = {238--246} }DetailsAbstract
Planning robust executions under uncertainty is a fundamental challenge for building autonomous robots. Partially Observable Markov Decision Processes (POMDPs) provide a standard framework for modeling uncertainty in many applications. In this work, we study POMDPs with safe-reachability objectives, which require that with a probability above some threshold, a goal state is eventually reached while keeping the probability of visiting unsafe states below some threshold. This POMDP formulation is different from the traditional POMDP models with optimality objectives and we show that in some cases, POMDPs with safe-reachability objectives can provide a better guarantee of both safety and reachability than the existing POMDP models through an example. A key algorithmic problem for POMDPs is policy synthesis, which requires reasoning over a vast space of beliefs (probability distributions). To address this challenge, we introduce the notion of a goal-constrained belief space, which only contains beliefs reachable from the initial belief under desired executions that can achieve the given safe-reachability objective. Our method compactly represents this space over a bounded horizon using symbolic constraints, and employs an incremental Satisfiability Modulo Theories (SMT) solver to efficiently search for a valid policy over it. We evaluate our method using a case study involving a partially observable robotic domain with uncertain obstacles. The results show that our method can synthesize policies over large belief spaces with a small number of SMT solver calls by focusing on the goal-constrained belief space. - N. T. Dantam, Z. K. Kingston, S. Chaudhuri, and L. E. Kavraki, “An Incremental Constraint-Based Framework for Task and Motion Planning,” International Journal of Robotics Research, vol. 37, no. 10, pp. 1134-1151. (Invited
Article), 2018.
BibTeX
@article{dantam2018incremental-tmp, abstract = {We present a new constraint-based framework for task and motion planning (TMP). Our approach is extensible, probabilistically-complete, and offers improved performance and generality compared to a similar, state-of-the-art planner. The key idea is to leverage incremental constraint solving to efficiently incorporate geometric information at the task level. Using motion feasibility information to guide task planning improves scalability of the overall planner. Our key abstractions address the requirements of manipulation and object rearrangement. We validate our approach on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and improved scalability from additional geometric guidance. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future.}, title = {An Incremental Constraint-Based Framework for Task and Motion Planning}, journal = {International Journal of Robotics Research, vol. 37, no. 10, pp. 1134-1151. (Invited Article)}, year = {2018}, author = {Dantam, Neil T. and Kingston, Zachary K. and Chaudhuri, Swarat and Kavraki, Lydia E.}, doi = {10.1177/0278364918761570}, keywords = {planning from high-level specifications} }DetailsAbstract
We present a new constraint-based framework for task and motion planning (TMP). Our approach is extensible, probabilistically-complete, and offers improved performance and generality compared to a similar, state-of-the-art planner. The key idea is to leverage incremental constraint solving to efficiently incorporate geometric information at the task level. Using motion feasibility information to guide task planning improves scalability of the overall planner. Our key abstractions address the requirements of manipulation and object rearrangement. We validate our approach on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and improved scalability from additional geometric guidance. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future. - S. Butler, M. Moll, and L. E. Kavraki, “A General Algorithm for Time-Optimal Trajectory Generation Subject to Minimum and
Maximum Constraints,” in Workshop on the Algorithmic Foundations of Robotics, 2016.
BibTeX
@inproceedings{butler2016a-general-algorithm-for-time-optimal-trajectory, abstract = {This paper presents a new algorithm which generates time-optimal trajectories given a path as input. The algorithm improves on previous approaches by generically handling a broader class of constraints on the dynamics. It eliminates the need for heuristics to select trajectory segments that are part of the optimal trajectory through an exhaustive, but efficient search. We also present an algorithm for computing all achievable velocities at the end of a path given an initial range of velocities. This algorithm effectively computes bundles of feasible trajectories for a given path and is a first step toward a new generation of more efficient kinodynamic motion planning algorithms. We present results for both algorithms using a simulated WAM arm with a Barrett hand subject to dynamics constraints on joint torque, joint velocity, momentum, and end effector velocity. The new algorithms are compared with a state-of-the-art alternative approach.}, author = {Butler, Stephen and Moll, Mark and Kavraki, Lydia E.}, booktitle = {Workshop on the Algorithmic Foundations of Robotics}, month = dec, title = {A General Algorithm for Time-Optimal Trajectory Generation Subject to Minimum and Maximum Constraints}, year = {2016}, keywords = {kinodynamic systems} }DetailsAbstract
This paper presents a new algorithm which generates time-optimal trajectories given a path as input. The algorithm improves on previous approaches by generically handling a broader class of constraints on the dynamics. It eliminates the need for heuristics to select trajectory segments that are part of the optimal trajectory through an exhaustive, but efficient search. We also present an algorithm for computing all achievable velocities at the end of a path given an initial range of velocities. This algorithm effectively computes bundles of feasible trajectories for a given path and is a first step toward a new generation of more efficient kinodynamic motion planning algorithms. We present results for both algorithms using a simulated WAM arm with a Barrett hand subject to dynamics constraints on joint torque, joint velocity, momentum, and end effector velocity. The new algorithms are compared with a state-of-the-art alternative approach. - N. T. Dantam, K. Bøndergaard, M. A. Johansson, T. Furuholm, and L. E. Kavraki, “Unix Philosophy and the Real World: Control Software for Humanoid Robots,” Frontiers in Robotics and Artificial Intelligence, vol. 3, Mar. 2016.
BibTeX
@article{dantam2016unix, author = {Dantam, Neil T. and B{\o}ndergaard, Kim and Johansson, Mattias A. and Furuholm, Tobias and Kavraki, Lydia E.}, title = {Unix Philosophy and the Real World: Control Software for Humanoid Robots}, journal = {Frontiers in Robotics and Artificial Intelligence}, keywords = {other robotics}, month = mar, year = {2016}, volume = {3}, doi = {10.3389/frobt.2016.00006}, abstract = {Robot software combines the challenges of general purpose and real-time software, requiring complex logic and bounded resource use. Physical safety, particularly for dynamic systems such as humanoid robots, depends on correct software. General purpose computation has converged on unix-like operating systems -- standardized as POSIX, the Portable Operating System Interface -- for devices from cellular phones to supercomputers. The modular, multi-process design typical of POSIX applications is effective for building complex and reliable software. Absent from POSIX, however, is an interproccess communication mechanism that prioritizes newer data as typically desired for control of physical systems. We address this need in the Ach communication library which provides suitable semantics and performance for real-time robot control. Although initially designed for humanoid robots, Ach has broader applicability to complex mechatronic devices -- humanoid and otherwise -- that require real-time coupling of sensors, control, planning, and actuation. The initial user space implementation of Ach was limited in the ability to receive data from multiple sources. We remove this limitation by implementing Ach as a Linux kernel module, enabling Ach's high-performance and latest-message-favored semantics within conventional POSIX communication pipelines. We discuss how these POSIX interfaces and design principles apply to robot software, and we present a case study using the Ach kernel module for communication on the Baxter robot.} }DetailsAbstract
Robot software combines the challenges of general purpose and real-time software, requiring complex logic and bounded resource use. Physical safety, particularly for dynamic systems such as humanoid robots, depends on correct software. General purpose computation has converged on unix-like operating systems – standardized as POSIX, the Portable Operating System Interface – for devices from cellular phones to supercomputers. The modular, multi-process design typical of POSIX applications is effective for building complex and reliable software. Absent from POSIX, however, is an interproccess communication mechanism that prioritizes newer data as typically desired for control of physical systems. We address this need in the Ach communication library which provides suitable semantics and performance for real-time robot control. Although initially designed for humanoid robots, Ach has broader applicability to complex mechatronic devices – humanoid and otherwise – that require real-time coupling of sensors, control, planning, and actuation. The initial user space implementation of Ach was limited in the ability to receive data from multiple sources. We remove this limitation by implementing Ach as a Linux kernel module, enabling Ach’s high-performance and latest-message-favored semantics within conventional POSIX communication pipelines. We discuss how these POSIX interfaces and design principles apply to robot software, and we present a case study using the Ach kernel module for communication on the Baxter robot. - N. T. Dantam, Z. K. Kingston, S. Chaudhuri, and L. E. Kavraki, “Incremental Task and Motion Planning: A Constraint-Based Approach,” in Robotics: Science and Systems, 2016.
BibTeX
@inproceedings{dantam2016tmp, author = {Dantam, Neil T. and Kingston, Zachary K. and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {Incremental Task and Motion Planning: A Constraint-Based Approach}, booktitle = {Robotics: Science and Systems}, year = {2016}, abstract = {We present a new algorithm for task and motion planning (TMP) and discuss the requirements and abstractions necessary to obtain robust solutions for TMP in general. Our Iteratively Deepened Task and Motion Planning (IDTMP) method is probabilistically-complete and offers improved performance and generality compared to a similar, state-of-the-art, probabilistically-complete planner. The key idea of IDTMP is to leverage incremental constraint solving to efficiently add and remove constraints on motion feasibility at the task level. We validate IDTMP on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and a four-times self-comparison speedup from our extensions. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future.}, keywords = {planning from high-level specifications}, doi = {10.15607/RSS.2016.XII.002} }DetailsAbstract
We present a new algorithm for task and motion planning (TMP) and discuss the requirements and abstractions necessary to obtain robust solutions for TMP in general. Our Iteratively Deepened Task and Motion Planning (IDTMP) method is probabilistically-complete and offers improved performance and generality compared to a similar, state-of-the-art, probabilistically-complete planner. The key idea of IDTMP is to leverage incremental constraint solving to efficiently add and remove constraints on motion feasibility at the task level. We validate IDTMP on a physical manipulator and evaluate scalability on scenarios with many objects and long plans, showing order-of-magnitude gains compared to the benchmark planner and a four-times self-comparison speedup from our extensions. Finally, in addition to describing a new method for TMP and its implementation on a physical robot, we also put forward requirements and abstractions for the development of similar planners in the future. - Y. Wang, N. T. Dantam, S. Chaudhuri, and L. E. Kavraki, “Task and Motion Policy Synthesis as Liveness Games,” in International Conference on Automated Planning and Scheduling, 2016, pp. 536–540.
BibTeX
@inproceedings{wang2016task, author = {Wang, Yue and Dantam, Neil T. and Chaudhuri, Swarat and Kavraki, Lydia E.}, title = {Task and Motion Policy Synthesis as Liveness Games}, booktitle = {International Conference on Automated Planning and Scheduling}, publisher = {AAAI}, year = {2016}, abstract = {We present a novel and scalable policy synthesis approach for robots. Rather than producing single-path plans for a static environment, we consider changing environments with uncontrollable agents, where the robot needs a policy to respond correctly over the infinite-horizon interaction with the environment. Our approach operates on task and motion domains, and combines actions over discrete states with continuous, collision-free paths. We synthesize a task and motion policy by iteratively generating a candidate policy and verifying its correctness. For efficient policy generation, we use grammars for potential policies to limit the search space and apply domain-specific heuristics to generalize verification failures, providing stricter constraints on policy candidates. For efficient policy verification, we construct compact, symbolic constraints for valid policies and employ a Satisfiability Modulo Theories (SMT) solver to check the validity of these constraints. Furthermore, the SMT solver enables quantitative specifications such as energy limits. The results show that our approach offers better scalability compared to a state-of-the-art policy synthesis tool in the tested benchmarks and demonstrate an order-of-magnitude speedup from our heuristics for the tested mobile manipulation domain.}, keywords = {planning from high-level specifications}, url = {http://www.aaai.org/ocs/index.php/ICAPS/ICAPS16/paper/view/13146}, pages = {536-540} }DetailsAbstract
We present a novel and scalable policy synthesis approach for robots. Rather than producing single-path plans for a static environment, we consider changing environments with uncontrollable agents, where the robot needs a policy to respond correctly over the infinite-horizon interaction with the environment. Our approach operates on task and motion domains, and combines actions over discrete states with continuous, collision-free paths. We synthesize a task and motion policy by iteratively generating a candidate policy and verifying its correctness. For efficient policy generation, we use grammars for potential policies to limit the search space and apply domain-specific heuristics to generalize verification failures, providing stricter constraints on policy candidates. For efficient policy verification, we construct compact, symbolic constraints for valid policies and employ a Satisfiability Modulo Theories (SMT) solver to check the validity of these constraints. Furthermore, the SMT solver enables quantitative specifications such as energy limits. The results show that our approach offers better scalability compared to a state-of-the-art policy synthesis tool in the tested benchmarks and demonstrate an order-of-magnitude speedup from our heuristics for the tested mobile manipulation domain.