Task and

Motion Planning

MIT 6.4210/2:

Robotic Manipulation

Fall 2022, Lecture 21

Follow live at https://slides.com/d/4rigVS4/live

(or later at https://slides.com/russtedrake/fall22-lec21)

/// Base type for all action primitives
class ActionPrimitiveInterface
{
  virtual bool IsCandidate(const State& state) const = 0;
  virtual Container GetOutcomes(const State& state) const = 0;
  virtual double EstimateActionCost(
      const State& current, const State& target) const = 0;
  virtual Container Execute(const State& state) = 0;
  virtual double Ranking() const = 0;
};

OpenDishwasherDoor
CloseDishwasherDoor
StartDishwasher
LoadSoapPacket
PullOutLowerRack
PullOutUpperRack
PullOutSilverwareRack
PushInLowerRack
PushInUpperRack
PushInSilverwareRack
PutPlateInDishwasher
ReorientMug
PutMugInDishwasher
OptimisticPutMugInDishwasher
PutSilverwareInDishwasher
PushUnmanipulableDish
SenseDishCounts
SenseDishToManipulate

work by Calder Phillips-Grafflin et al. at TRI

DishTaskState(const DishState& active_dish_state,
              int32_t clean_items_put_away, int32_t clean_items_in_dishwasher,
              int32_t dirty_items_in_dishwasher,
              int32_t dirty_items_available,
              const DishwasherState& current_dishwasher_state);

DishState(const RigidTransformd& dish_pose, DishType dish_type,
          DishProgress dish_progress, RackRequirement rack_required);

DishwasherState(bool known, bool door_open, bool lower_rack_out,
                bool upper_rack_out, bool silverware_rack_out,
                bool lower_rack_full, bool upper_rack_full,
                bool silverware_rack_full, bool started, bool soap_loaded);
enum class DishType : uint8_t {
  Unknown = 0x00,
  Mug = 0x01,
  Plate = 0x02,
  Silverware = 0x03
};

enum class RackRequirement : uint8_t {
  Unknown = 0x00,
  LowerRack = 0x01,
  UpperRack = 0x02,
  SilverwareRack = 0x03
};
enum class DishProgress : uint8_t {
  Unknown = 0x00,
  Unmanipulable = 0x01,
  LikelyManipulable = 0x02,
  Sensed = 0x03,
  OptimisticOriented = 0x04,
  Oriented = 0x05,
  Placed = 0x06,
  Released = 0x07
};

work by Calder Phillips-Grafflin et al. at TRI

STRIPS problem definitions

https://en.wikipedia.org/wiki/Stanford_Research_Institute_Problem_Solver

  • Initial state
  • Goal state(s)
  • Set of actions. For each:
    • preconditions
    • effects
/// Base type for all action primitives
class ActionPrimitiveInterface
{
  virtual bool IsCandidate(const State& state) const = 0;
  virtual Container GetOutcomes(const State& state) const = 0;
  ...
};

PDDL = Planning Domain Definition Language

from https://www.cs.toronto.edu/~sheila/2542/s14/A1/introtopddl2.pdf

Example: Gripper task with four balls

There is a robot that can move between two rooms and pick up or drop balls with either of his two arms. Initially, all balls and the robot are in the first room. We want the balls to be in the second room.

  • Objects: The two rooms, four balls and two robot arms.
  • Predicates: Is x a room? Is x a ball? Is ball x inside room y? Is robot arm x empty? [...]
  • Initial state: All balls and the robot are in the first room. All robot arms are empty. [...]
  • Goal specification: All balls must be in the second room.
  • Actions/Operators: The robot can move between rooms, pick up a ball or drop a ball.

PDDL = Planning Domain Definition Language

(define (domain gripper-strips)
   (:predicates (room ?r)
		(ball ?b)
		(gripper ?g)
		(at-robby ?r)
		(at ?b ?r)
		(free ?g)
		(carry ?o ?g))

   (:action move
       :parameters  (?from ?to)
       :precondition (and  (room ?from) (room ?to) (at-robby ?from))
       :effect (and  (at-robby ?to)
		     (not (at-robby ?from))))



   (:action pick
       :parameters (?obj ?room ?gripper)
       :precondition  (and  (ball ?obj) (room ?room) (gripper ?gripper)
			    (at ?obj ?room) (at-robby ?room) (free ?gripper))
       :effect (and (carry ?obj ?gripper)
		    (not (at ?obj ?room)) 
		    (not (free ?gripper))))


   (:action drop
       :parameters  (?obj  ?room ?gripper)
       :precondition  (and  (ball ?obj) (room ?room) (gripper ?gripper)
			    (carry ?obj ?gripper) (at-robby ?room))
       :effect (and (at ?obj ?room)
		    (free ?gripper)
		    (not (carry ?obj ?gripper)))))

https://github.com/SoarGroup/Domains-Planning-Domain-Definition-Language/blob/master/pddl/gripper.pddl

PDDL = Planning Domain Definition Language

(define (problem strips-gripper4)
   (:domain gripper-strips)
   (:objects rooma roomb ball1 ball2 ball3 ball4 left right)
   (:init (room rooma)
          (room roomb)
          (ball ball1)
          (ball ball2)
          (ball ball3)
          (ball ball4)
          (gripper left)
          (gripper right)
          (at-robby rooma)
          (free left)
          (free right)
          (at ball1 rooma)
          (at ball2 rooma)
          (at ball3 rooma)
          (at ball4 rooma))
   (:goal (and (at ball1 roomb)
               (at ball2 roomb)
               (at ball3 roomb))))

https://github.com/SoarGroup/Domains-Planning-Domain-Definition-Language/blob/master/pddl/gripper-4.pddl

Feedback via online replanning

In the dish-loading example, we replan before each action to handle unexpected outcomes...

2D Pick and Place example from Caelen Garrett

A nice survey (from Oct, 2020):

Taxonomy of TAMP approaches (Garrett, 2020)

Logic-Geometric Programming

PDDLStream

Taxonomy of TAMP approaches (Garrett, 2020)

Logic-Geometric Programming

PDDLStream

Danny Driess and Jung-Su Ha and Marc Toussaint, Deep Visual Reasoning: Learning to Predict Action Sequences for Task and Motion Planning from an Initial
Scene Image, Robotics: Science and Systems (R:SS) 2020.

Kinematic trajectory optimization

\begin{aligned} \min_{x_0, ..., x_N} \quad & \sum_{n=0}^{N-1} | x_{n+1} - x_n|_2^2 & \\ \text{subject to} \quad & x_0 = x_{start} \\ & x_N = x_{goal} \\ & |x_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

collision-avoidance

(outside the \(L^1\) ball)

nonconvex

Hybrid kinematic trajectory optimization

Branch and bound

b_0 = 0.9, b_1 = 0.4
b_0 = 1
b_0 = 0
b_1 = 1
b_1 = 0

Convex relaxations provide lower bounds

Feasible solutions provide upper bounds

convex

convex

convex

convex

convex

\begin{aligned} \underset{x, b}{\text{minimize}} \quad & f(x, b) \\ \text{subject to} \quad & g(x, b) \le 0 \\ & b_i \in \{0, 1\}, \forall i \end{aligned}

Logic-Geometric Programming

  • Branch and bound search over action sequences
  • Hybrid kinematic trajectory optimization
    • Note: this is non-convex
    • => heuristic branch and bound
  • Many additional clever heuristics for fast pruning (e.g. solve an IK problem before solving full traj opt.)

Marc Toussaint and Jung-Su Ha and Danny Driess, Describing Physics For Physical Reasoning: Force-based Sequential Manipulation Planning, IEEE Robotics and Automation Letters 2020.

Taxonomy of TAMP approaches (Garrett, 2020)

Logic-Geometric Programming

PDDLStream

PDDLStream

Interleave sampling (from IK, or collision-free motion planner) with fast symbolic search.

Graphs of Convex Sets

 

  • For each \(i \in V:\)
    • Compact convex set \(X_i \subset \R^d\)
    • A point \(x_i \in X_i \) 
  • Edge length given by a convex function \[ \ell(x_i, x_j) \]

Coming Soon: TAMP via GCS

Work by Savva Morozov

PDDLStream (by Caelan)

Coming Soon: TAMP via GCS

Work by Savva Morozov

And now Boyuan's presentation on

Large Language Models

Follow live at https://slides.com/d/EyfmqBY/live

(or later at slides.com/buoyancy99/deck)