II) Operators#

The skills and corresponding constraints with the world are included in the domain via PDDL operators.

Operators for General-Purpose Skills#

The parameters for these operators include continuous variables produced by generators, which can be a uniform sampler, learned sampler, a sampling-based algorithm or an optimization algorithm. Please refer to the PDDLStream paper [1] for the format of those generators (:stream.

Pick#

(:action pick
  :parameters (?a ?o ?p ?g ?q ?t ?w)
  :precondition (and (KinWConf ?a ?o ?p ?g ?q ?t ?w)
                     (AtPose ?o ?p) (HandEmpty ?a)
                     (AtBConf ?q) (InWConf ?w)
                     (not (UnsafeApproach ?o ?p ?g))
                     (not (UnsafeATraj ?t))
                )
  :effect (and (AtGrasp ?a ?o ?g) (CanMove)
               (not (AtPose ?o ?p)) (not (HandEmpty ?a))
               (increase (total-cost) (PickCost))
          )
)

Pick with a single gripper grasp requires

  • the joint sampling of grasp ?g and arm trajectory ?t with (KinWConf ?a ?o ?p ?g ?q ?t ?w), which is a constraint that says "pick with arm ?a the object ?o from its original pose ?p by going to base configuration ?q with arm trajectory ?t, in world configuration ?w"
(:stream sample-grasp
  :inputs (?o)
  :domain (Graspable ?o)
  :outputs (?g)
  :certified (Grasp ?o ?g)
)
(:stream inverse-kinematics-wconf
  :inputs (?a ?o ?p ?g ?w)
  :domain (and (Controllable ?a) (Pose ?o ?p) (Grasp ?o ?g) (WConf ?w))
  :outputs (?q ?t)
  :certified (and (BConf ?q) (ATraj ?t) (KinWConf ?a ?o ?p ?g ?q ?t ?w))
)
  • checking that the grasp approach path (straight line) is free of collisions from movable objects with (not (UnsafeApproach ?o ?p ?g))
  • checking that the arm path from default arm pose to grasp approach pose is free of collisions from movable objects with (not (UnsafeATraj ?t))

Place#

(:action place
  :parameters (?a ?o ?p ?g ?q ?t ?w)
  :precondition (and (KinWConf ?a ?o ?p ?g ?q ?t ?w)
                     (AtGrasp ?a ?o ?g) (AtBConf ?q) (InWConf ?w)
                     (not (UnsafePose ?o ?p))
                     (not (UnsafeApproach ?o ?p ?g))
                     (not (UnsafeATraj ?t))
                )
  :effect (and (AtPose ?o ?p) (HandEmpty ?a) (CanMove)
               (not (AtGrasp ?a ?o ?g))
               (increase (total-cost) (PlaceCost))
          )
)

Place, compared to pick, requires additionally

  • sampling of placement pose ?p on possible surfaces that supports the object
(:stream sample-pose
  :inputs (?o ?r)
  :domain (Stackable ?o ?r)
  :outputs (?p)
  :certified (and (Pose ?o ?p) (Supported ?o ?p ?r))
)

Grasp Handle#

Handles are body links to grasp for changing joint positions or body poses. The Grasp Handle operator is similar to a pick. But the motion after a handle grasp is constraint by the category of object

  • for door links (one revolute joint), the end-effector motion needs to be along a circular trajectory
  • for drawer links (one prismatic joint), the end-effector motion needs to be along a straight line
  • for knob links (one revolute joint where the articulated link is the handle itself), only the arm wrist link can move
  • for cart objects (an array of prismatic joints), the end-effector motion needs to be along a straight line
  • for chair objects (an array of prismatic and revolute joints), the end-effector motion needs to move on a plane parallel to the floor
(:action grasp_handle
  :parameters (?a ?o ?p ?g ?q ?aq1 ?aq2 ?t)
  :precondition (and (Joint ?o)
                     (KinGraspHandle ?a ?o ?p ?g ?q ?aq2 ?t)
                     (AtPosition ?o ?p) (HandEmpty ?a)
                     (AtBConf ?q) (AtAConf ?a ?aq1)
                )
  :effect (and (AtHandleGrasp ?a ?o ?g) (not (HandEmpty ?a))
               (not (CanMove)) (CanPull) (not (CanUngrasp))
               (not (AtAConf ?a ?aq1)) (AtAConf ?a ?aq2)
               (increase (total-cost) (PickCost))
          )
)

Handle grasps and grasping arm trajectories are sampled by the streams:

(:stream sample-handle-grasp
  :inputs (?o)
  :domain (Joint ?o)
  :outputs (?g)
  :certified (HandleGrasp ?o ?g)
)
(:stream inverse-kinematics-grasp-handle
  :inputs (?a ?o ?p ?g)
  :domain (and (Controllable ?a) (Position ?o ?p) (HandleGrasp ?o ?g) (IsClosedPosition ?o ?p))
  :outputs (?q ?aq ?t)
  :certified (and (BConf ?q) (AConf ?a ?aq) (ATraj ?t) (KinGraspHandle ?a ?o ?p ?g ?q ?aq ?t))
)

For each of the first four types of articulated objects ?o, we have a corresponding pull handle operator described in the next subsection.

Note that those four operators take the joint as input object instead of handle likes. That's because the handle link may just be one of many large links whose poses are changed with the joint position change, e.g. handle of fridge is a small part of the door links that's easy to grasp stably.

Pull Handle#

Pull Drawer Handle

(:action pull_drawer_handle
  :parameters (?a ?o ?p1 ?p2 ?g ?q1 ?q2 ?bt)
  :precondition (and (Drawer ?o) (not (= ?p1 ?p2)) (CanPull)
                     (AtPosition ?o ?p1) (Position ?o ?p2)
                     (KinPullDrawerHandle ?a ?o ?p1 ?p2 ?g ?q1 ?q2 ?bt)
                     (AtHandleGrasp ?a ?o ?g) (AtBConf ?q1)
                     ;(not (UnsafeBTraj ?bt))
                )
  :effect (and (not (CanPull)) (CanUngrasp)
              (AtPosition ?o ?p2) (not (AtPosition ?o ?p1))
              (AtBConf ?q2) (not (AtBConf ?q1))
          )
)

Pull Door Handle

(:action pull_door_handle
  :parameters (?a ?o ?p1 ?p2 ?g ?q1 ?q2 ?bt ?aq1 ?aq2 ?at ?w1 ?w2)
  :precondition (and (Door ?o) (not (= ?p1 ?p2)) (CanPull)
                     (AtPosition ?o ?p1) (Position ?o ?p2) (AtHandleGrasp ?a ?o ?g)
                     (KinPullDoorHandle ?a ?o ?p1 ?p2 ?g ?q1 ?q2 ?bt ?aq1 ?aq2 ?at)
                     (AtBConf ?q1) (AtAConf ?a ?aq1)
                     (InWConf ?w1) (WConf ?w2)
                     (NewWConfPst ?w1 ?o ?p2 ?w2)
                     ;(not (UnsafeApproach ?o ?p2 ?g))
                     ;(not (UnsafeATraj ?at))
                     ;(not (UnsafeBTraj ?bt))
                )
  :effect (and (not (CanPull)) (CanUngrasp)
              (AtPosition ?o ?p2) (not (AtPosition ?o ?p1))
              (AtBConf ?q2) (not (AtBConf ?q1))
              (AtAConf ?a ?aq2) (not (AtAConf ?a ?aq1))
              (InWConf ?w2) (not (InWConf ?w1))
          )
)

Turn Knob

(:action turn_knob
  :parameters (?a ?o ?p1 ?p2 ?g ?q ?aq1 ?aq2 ?at)
  :precondition (and (Knob ?o) (not (= ?p1 ?p2)) (CanPull)
                     (AtPosition ?o ?p1) (Position ?o ?p2) (AtHandleGrasp ?a ?o ?g)
                     (KinTurnKnob ?a ?o ?p1 ?p2 ?g ?q ?aq1 ?aq2 ?at)
                     (AtBConf ?q) (AtAConf ?a ?aq1)
                     ;(not (UnsafeApproach ?o ?p2 ?g))
                     ;(not (UnsafeATraj ?at))
                     ;(not (UnsafeBTraj ?bt))
                )
  :effect (and (not (CanPull)) (CanUngrasp) (KnobTurned ?a ?o)
              (AtPosition ?o ?p2) (not (AtPosition ?o ?p1))
              (UngraspBConf ?q)
              (AtAConf ?a ?aq2) (not (AtAConf ?a ?aq1))
          )
)

The corresponding Ungrasp action is needed to break from this mode of motion planning:

(:action ungrasp_handle
  :parameters (?a ?o ?p ?g ?q ?aq1 ?aq2 ?t)
  :precondition (and (Joint ?o) (AConf ?aq1) (AConf ?aq2) (AtPosition ?o ?p)
                     (KinUngraspHandle ?a ?o ?p ?g ?q ?aq1 ?aq2 ?t)
                     (AtHandleGrasp ?a ?o ?g) (CanUngrasp)
                     (AtBConf ?q) (UngraspBConf ?q) (AtAConf ?a ?aq1)
                )
  :effect (and (GraspedHandle ?o) (HandEmpty ?a) (CanMove)
               (not (AtHandleGrasp ?a ?o ?g))
               (not (AtAConf ?a ?aq1)) (AtAConf ?a ?aq2)
               (increase (total-cost) (PlaceCost))
          )
)

Operators for Symbolic Goals#

References#

  1. Garrett, Caelan Reed, Tomás Lozano-Pérez, and Leslie Pack Kaelbling. "PDDLStream: Integrating symbolic planners and blackbox samplers via optimistic adaptive planning." Proceedings of the International Conference on Automated Planning and Scheduling. Vol. 30. 2020.