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

(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

\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.