Sample PDDLStream stream definition for Task and Motion Planning with the Toyota HSR

pddlstream_sample_stream.pddl

(define (stream hsr_stream)
  
  ; DIST: Distance between two locations
  (:function (Dist ?l1 ?l2)
             (and (Location ?l1) (Location ?l2))
  )

  ; PICKPLACECOST: Cost to perform pick and place at a location
  (:function (PickPlaceCost ?l ?o)
             (and (Location ?l) (Obj ?o))
  )

  ; OPENCLOSECLOST: Cost to open or close a location
  (:function (OpenCloseCost ?l)
             (Location ?l)
  )

  ; NAVPOSE: Navigation pose
  (:stream s-navpose
    :inputs (?l ?r)
    :domain (and (Robot ?r) (Location ?l))
    :outputs (?p)
    :certified (and (Pose ?r ?p) (NavPose ?l ?p))
  )

  ; S-REGION: Sample a pose in a location
  (:stream s-location
    :inputs (?l ?o)
    :domain (and (Location ?l) (Obj ?o))
    :outputs (?p)
    :certified (and (Pose ?o ?p) (Inside ?l ?o ?p))
  )

  ; S-GRASP-ANG: Sample a grasp angle
  (:stream s-grasp-ang
    :outputs (?q)
    :certified (GraspAng ?q)
  )

  ; TRAJFREE: Check if a trajectory a pose is collision free
  (:stream t-trajfree
    :inputs (?r ?pr ?q ?o1 ?p1 ?o2 ?p2)
    :domain (and (GraspAng ?q)
                 (Pose ?r ?pr)
                 (Pose ?o1 ?p1) 
                 (Pose ?o2 ?p2)
            )
    :certified (TrajFree ?r ?pr ?q ?o1 ?p1 ?o2 ?p2)
  )

)