; Specification in PDDL1 of the rockin TBM2 drill test domain
; author : Oscar Lima, [email protected]

(define (domain idmind_agent)

(:requirements :strips :typing :fluents :negative-preconditions :disjunctive-preconditions :durative-actions )
 
 (:types 
    location      		; locations in the arena, points of interest
  	robot         		; your amazing yet powerful robot
  	object				; objects to be manipulated by the robot
  	gripper				; robot gripper
  	robot_platform		; platform slots for the robot to store objects
 )

 (:predicates

	; robot ?r is at location ?l
 	(at ?r - robot ?l - location) 		
    ; object ?o is on location ?l
 	(on ?o - object ?l - location)
 	; object ?o is stored on robot platform ?rp
 	(stored ?o - object ?rp - robot_platform) 	
 	; robot platform ?rp is occupied, yb has 3 free places to store objects
	(occupied ?rp - robot_platform)
	; gripper ?g is holding object ?o
	(holding ?g - gripper ?o - object)
    ; gripper ?g is free (does not contain object)
	(gripper_is_free ?g - gripper)

 )


; moves a robot ?r from ?source - location to a ?destination - location
; NOTE : the situation in which the robot arm is in any position before moving 
; is not handled at the planning level, hence we advise to always move the arm 
; to a folded position, then navigate
 (:durative-action move_base
     :parameters (?source ?destination - location ?r - robot ?g - gripper)
     :duration (= ?duration 1)
     :condition (and (at start (at ?r ?source))
     				 (over all (gripper_is_free ?g))
     			   )
     :effect (and 
                    (at start (not (at ?r ?source)))
     			    (at end (at ?r ?destination))
     		 )
 )

 ; pick an object ?o which is inside a location ?l with a free gripper ?g 
 ; with robot ?r that is at location ?l
 (:durative-action pick                                
     :parameters (?o - object ?l - location ?r - robot ?g - gripper)
     :duration (= ?duration 1)    
     :condition 	(and 	(at start (on ?o ?l))
                      		(over all (at ?r ?l))
                      		(at start (gripper_is_free ?g))
                   	)
     :effect (and  	(at end (holding ?g ?o)) 
                   	(at start (not (on ?o ?l)))
                   	(at start (not (gripper_is_free ?g)))
             )
 )

 ; stage an object ?o in a robot platform ?rp which is not occupied with a gripper ?g 
 ; which is holding the object ?o
 (:durative-action stage
     :parameters (?o - object ?rp - robot_platform ?g - gripper)
     :duration (= ?duration 1)
     :condition 	(and 	(at start (holding ?g ?o))
                      		(at start (not (occupied ?rp)))
                      		(at start (not (gripper_is_free ?g)))
                   	)
     :effect (and  	(at start (not (holding ?g ?o)))
     				(at start (gripper_is_free ?g))
     			   	(at start (stored ?o ?rp))
                   	(at start (occupied ?rp))
             )
 )

 ; unstage an object ?o stored on a robot platform ?rp with a free gripper ?g
 (:durative-action unstage
     :parameters (?o - object ?rp - robot_platform ?g - gripper)
     :duration (= ?duration 1)
     :condition 	(and 	(at start (gripper_is_free ?g))
                      		(at start (stored ?o ?rp))
                      		(at start (not (holding ?g ?o)))
                   	)
     :effect (and  	(at start (not (gripper_is_free ?g)))
     			   	(at start (not (stored ?o ?rp)))
                   	(at start (not (occupied ?rp)))
                   	(at start(holding ?g ?o))
             )
 )

; places and object ?o with a gripper ?g which is holding the object ?o
; with a robot ?r at a location ?l on robot_platform ?rp
 (:durative-action drop_on_platform
     :parameters (?o - object ?l - location ?g - gripper ?r - robot ?rp - robot_platform)
     :duration (= ?duration 1)
     :condition 	(and 	(over all (at ?r ?l))
     						(at start (holding ?g ?o))
     						(at start (not (stored ?o ?rp)))
                   	)
     :effect (and 	(at end (gripper_is_free ?g))	; the gripper is free
     				(at end (on ?o ?l))				; object is now on location l
     				(at start (not (holding ?g ?o)))	; the gripper is no longer holding the object
             )
 )

)

(define (problem problem_1)
	
  (:domain idmind_agent)

  (:objects 
  	dock S1 S2 S3 S4 S5 - location
  	platform_middle platform_left platform_right - robot_platform
  	idmind - robot
  	o1 o2 - object
  	robotiq - gripper
  )

  (:init 
  	(at idmind dock)
  	(on o1 S2)
  	(on o2 S2)
	(not (occupied platform_middle))
	(not (occupied platform_left))
	(not (occupied platform_right))
	(gripper_is_free robotiq)
  ) 

  (:goal
  		(and	
                (on o1 S1)
  				(on o2 S4)
  		)
  )

 )

rosrun rosplan_planning_system popf new.domain.pddl new_problem.pddl
Critical Errors Encountered in Domain/Problem File
--------------------------------------------------

Due to critical errors in the supplied domain/problem file, the planner
has to terminate.  The errors encountered are as follows:

Errors: 1, warnings: 9
new_problem.pddl: line: 11: Warning: Undeclared requirement :typing 
new_problem.pddl: line: 11: Warning: Undeclared requirement :typing 
new_problem.pddl: line: 11: Warning: Undeclared requirement :typing 
new_problem.pddl: line: 11: Warning: Undeclared requirement :typing 
new_problem.pddl: line: 11: Warning: Undeclared requirement :typing 
new_problem.pddl: line: 14: Warning: Undeclared symbol: at
new_problem.pddl: line: 15: Warning: Undeclared symbol: on
new_problem.pddl: line: 17: Warning: Undeclared symbol: occupied
new_problem.pddl: line: 20: Warning: Undeclared symbol: gripper_is_free
new_problem.pddl: line: 30: Error: Syntax error in problem file - types used, but no :types section in domain file.