Sample PDDL domain for Task and Motion Planning with the Toyota HSR

pddlstream_sample_domain.pddl

(define (domain hsr_tamp)
  (:requirements :strips :equality)
  (:predicates 
               ; Static Predicates
               (Robot ?r)           ; Represents the robot
               (Obj ?o)             ; Object representation
               (Location ?l)        ; Location representation
               (Room ?m)            ; Room representation
               (Type ?t)            ; Type of location or object
               (GraspAng ?q)        ; Grasp angle
               (Is ?o ?t)           ; Type correspondence of location or object
               (Pose ?o ?p)         ; Pose type

               ; Fluent Predicates
               (HandEmpty ?r)       ; Whether the robot's gripper is empty
               (At ?o ?l)           ; Robot/Object's location, or location's Room
               (AtPose ?o ?p)       ; Pose of an object
               (AtGrasp ?o ?q)      ; Check how an object is grasped (e.g. top vs side)
               (Holding ?r ?o)      ; Check if the robot is holding an object or object type
               (Has ?r ?t)          ; Check existence of object types in locations
               (HasNone ?r ?t)      ; Check nonexistence of object types in locations
               (HasAll ?r ?t)       ; Check exclusivity of object types in locations
               (IsOpen ?l)          ; Is location open
               (IsClosed ?l)        ; Is location closed (opposite of IsOpen)
               (UnsafeTraj ?l ?r ?pr ?o ?p ?q)  ; Unsafe straight-line trajectory for pick/place

               ; Stream Verified Predicates
               (NavPose ?l ?p)          ; Navigation pose of a location
               (CanGrasp ?l ?q)         ; Can grasp at an angle
               (Inside ?l ?o ?p)        ; Sampled position is valid
               (TrajFree ?r ?pr ?q ?o1 ?p1 ?o2 ?p2) ; Collision-free trajectory
 )

  ; FUNCTIONS : See their descriptions in the stream PDDL file
  (:functions 
    (Dist ?l1 ?l2)
    (PickPlaceCost ?l ?o)
    (OpenCloseCost ?l)
  )

  ; ACTIONS 
  ; MOVE: Moves the robot from one location to the other
  (:action move
    :parameters (?r ?l1 ?l2 ?p1 ?p2)
    :precondition (and (Robot ?r) 
                       (At ?r ?l1)
                       (AtPose ?r ?p1)
                       (NavPose ?l2 ?p2)
                  )
    :effect (and (At ?r ?l2)
                 (not (At ?r ?l1))
                 (AtPose ?r ?p2)
                 (not (AtPose ?r ?p1))
                 (increase (total-cost) (Dist ?l1 ?l2))
            )
  )

  ; PICK: Picks up an object from a specified location
  (:action pick
    :parameters (?r ?o ?l ?p ?q ?pr)
    :precondition (and (Robot ?r)
                       (Obj ?o)
                       (IsOpen ?l)
                       (CanGrasp ?l ?q)
                       (HandEmpty ?r) 
                       (At ?r ?l)
                       (At ?o ?l)
                       (AtPose ?o ?p)
                       (AtPose ?r ?pr)
                       (not (UnsafeTraj ?l ?r ?pr ?o ?p ?q))
                  )
    :effect (and (AtGrasp ?o ?q)
                 (not (HandEmpty ?r)) 
                 (not (At ?o ?l))
                 (not (AtPose ?o ?p))
                 (increase (total-cost) (PickPlaceCost ?l ?o)))
  )

  ; PLACE: Places an object in a specified location
  (:action place
    :parameters (?r ?o ?l ?p ?q ?pr)
    :precondition (and (Robot ?r)
                       (IsOpen ?l)
                       (At ?r ?l)
                       (AtPose ?r ?pr)
                       (CanGrasp ?l ?q)
                       (AtGrasp ?o ?q)
                       (not (HandEmpty ?r)) 
                       (Inside ?l ?o ?p)
                       (not (UnsafeTraj ?l ?r ?pr ?o ?p ?q))
                    )
    :effect (and (HandEmpty ?r)
                 (At ?o ?l)
                 (AtPose ?o ?p)
                 (not (AtGrasp ?o ?q))
                 (increase (total-cost) (PickPlaceCost ?l ?o)))
  )

  ; OPEN: Opens a location
  (:action open
    :parameters (?r ?l)
    :precondition (and (Robot ?r)
                       (At ?r ?l)
                       (not (IsOpen ?l))
                       (HandEmpty ?r)
                  )
    :effect (and (IsOpen ?l)
                 (increase (total-cost) (OpenCloseCost ?l))
                 )
  )

  ; CLOSE: Closes a location
  (:action close
    :parameters (?r ?l)
    :precondition (and (Robot ?r)
                       (At ?r ?l)
                       (IsOpen ?l)
                       (HandEmpty ?r)
                  )
    :effect (and (not (IsOpen ?l))
                 (increase (total-cost) (OpenCloseCost ?l))
                 )
  )
 
  ; DERIVED PREDICATES
  ; HOLDING: Checks if a robot is holding an object or object type
  (:derived (Holding ?r ?t)
    ; CASE 1: Object is instance
    (and (Robot ?r)
    (or
      (and (Obj ?t)
           (exists (?q) (and (GraspAng ?q) (AtGrasp ?t ?q)))
      )
    ; CASE 2: Object is type
      (and (Type ?t)
           (exists (?q ?o) (and (Obj ?o)
                                (GraspAng ?q)
                                (Is ?o ?t)
                                (AtGrasp ?o ?q))
           )
      )
    )
    )
  )

  ; ISCLOSED: Checks is a location is closed
  (:derived (IsClosed ?l)
    (not (IsOpen ?l))
  )

  ; UNSAFETRAJ: Checks if a trajectory to a pose has collisions
  (:derived (UnsafeTraj ?l ?r ?pr ?o ?p ?q)
     (exists (?o2 ?p2)
        (and (Obj ?o2) 
             (At ?o2 ?l)
             (AtPose ?o2 ?p2)
             (not (= ?o ?o2))
             (not (TrajFree ?r ?pr ?q ?o ?p ?o2 ?p2))
        )
     )
  )

  ; HAS: Checks object locations using object and location types or instances, 
  ;      or even room names as locations
  (:derived (Has ?t ?r)
    ; CASE 1: Object and location specified as instances
    (or 
        (and (or (Obj ?t) (Robot ?t))
             (Location ?r)
             (At ?t ?r))
    ; CASE 2: Object is instance, location is types
        (exists (?l) (and (or (Obj ?t) (Robot ?t))
                          (Location ?l) 
                          (or (and (Type ?r) (Is ?l ?r))
                              (and (Room ?r) (At ?l ?r)))
                          (At ?t ?l)))
    ; CASE 3: Object is type, location is instance
        (exists (?o) (and (Type ?t) (Obj ?o) 
                          (Location ?r)
                          (Is ?o ?t)
                          (At ?o ?r)))
    ; CASE 4: Object and location specified as types
        (exists (?o ?l) (and (Type ?t) (Obj ?o) (Location ?l) 
                             (or (and (Type ?r) (Is ?l ?r))
                                 (and (Room ?r) (At ?l ?r)))
                             (Is ?o ?t)
                             (At ?o ?l)))
    )
  )

  ; HASNONE: The opposite of "HAS". 
  ; Checks that a location or location type has no object type 
  ; or instances of an object type
  (:derived (HasNone ?t ?r)
    (not (Has ?t ?r))
  )

  ; HASALL: A variant of "HAS" for all rather than any objects.
  ; Checks that an object or all instances of an object type are in a 
  ; specific location or location type
  (:derived (HasAll ?t ?r)
    ; CASE 1: Object and location specified as instances
    (or 
        (and (Obj ?t)
             (Location ?r)
             (At ?t ?r))
    ; CASE 2: Object is instance, location is types
        (exists (?l) (and (Obj ?t) (Location ?l) 
                          (or (and (Type ?r) (Is ?l ?r))
                              (and (Room ?r) (At ?l ?r)))
                          (At ?t ?l)))
    ; CASE 3: Object is type, location is instance
        (forall (?o) 
          (imply
            (and (Obj ?o) (Type ?t) (Is ?o ?t)) 
            (and (Location ?r) (At ?o ?r))
          )
        )
    ; CASE 4: Object and location specified as types
        (forall (?o)
          (imply 
            (and (Obj ?o) (Type ?t) (Is ?o ?t))
            (exists (?l) (and 
                            (Location ?l)
                            (or (and (Type ?r) (Is ?l ?r))
                                (and (Room ?r) (At ?l ?r)))
                            (At ?o ?l))
                         )
          )
        )
    )
  )


)